From: Jochen Sprickerhof <git@jochen.sprickerhof.de>
Date: Sat, 25 Sep 2021 16:36:20 +0000 (+0200)
Subject: New upstream version 1.12.0+dfsg
X-Git-Tag: archive/raspbian/1.14.0+dfsg-2+rpi1^2~10^2~3
X-Git-Url: https://dgit.raspbian.org/?a=commitdiff_plain;h=b73b4a1ab4e48791a53805e7658f47c0b2e3dfc3;p=pcl.git

New upstream version 1.12.0+dfsg
---

diff --git a/.ci/azure-pipelines/azure-pipelines.yaml b/.ci/azure-pipelines/azure-pipelines.yaml
index a0bf28f0..28154042 100644
--- a/.ci/azure-pipelines/azure-pipelines.yaml
+++ b/.ci/azure-pipelines/azure-pipelines.yaml
@@ -1,15 +1,33 @@
+trigger:
+  paths:
+    exclude:
+    - doc
+    - README.md
+    - CHANGES.md
+    - CONTRIBUTING.md
+
+pr:
+  paths:
+    exclude:
+    - doc
+    - README.md
+    - CHANGES.md
+    - CONTRIBUTING.md
+
 resources:
   containers:
+    - container: winx86
+      image: pointcloudlibrary/env:winx86
+    - container: winx64
+      image: pointcloudlibrary/env:winx64
     - container: fmt
       image: pointcloudlibrary/fmt
-    - container: env1604
-      image: pointcloudlibrary/env:16.04
     - container: env1804
       image: pointcloudlibrary/env:18.04
     - container: env2004
       image: pointcloudlibrary/env:20.04
-    - container: doc
-      image: pointcloudlibrary/doc
+    - container: env2010
+      image: pointcloudlibrary/env:20.10
 
 stages:
   - stage: formatting
@@ -24,26 +42,26 @@ stages:
       - job: ubuntu
         displayName: Ubuntu
         pool:
-          vmImage: 'Ubuntu 16.04'
+          vmImage: 'Ubuntu 20.04'
         strategy:
           matrix:
-            16.04 GCC:
-              CONTAINER: env1604
+            18.04 GCC:  # oldest LTS
+              CONTAINER: env1804
               CC: gcc
               CXX: g++
-              BUILD_GPU: OFF
+              BUILD_GPU: ON
               CMAKE_ARGS: '-DPCL_WARNINGS_ARE_ERRORS=ON'
-            20.04 GCC:
-              CONTAINER: env2004
+            20.10 GCC:  # latest Ubuntu
+              CONTAINER: env2010
               CC: gcc
               CXX: g++
               BUILD_GPU: OFF
-              CMAKE_ARGS: ''
         container: $[ variables['CONTAINER'] ]
         timeoutInMinutes: 0
         variables:
           BUILD_DIR: '$(Agent.BuildDirectory)/build'
-          CMAKE_CXX_FLAGS: '-Wall -Wextra'
+          CMAKE_CXX_FLAGS: '-Wall -Wextra -Wnoexcept-type'
+          DISPLAY: :99.0 # Checked for in CMake
         steps:
           - template: build/ubuntu.yaml
 
@@ -75,14 +93,15 @@ stages:
         # Placement of Ubuntu Clang job after macOS ensures an extra parallel job doesn't need to be created.
         # Total time per run remains same since macOS is quicker so it finishes earlier, and remaining time is used by this job
         # Therefore, number of parallel jobs and total run time of entire pipeline remains unchanged even after addition of this job
+        # The version of Ubuntu is chosen to cover more versions than covered by GCC based CI
         dependsOn: osx
         condition: succeededOrFailed()
         pool:
-          vmImage: 'Ubuntu 16.04'
+          vmImage: 'Ubuntu 20.04'
         strategy:
           matrix:
-            18.04 Clang:
-              CONTAINER: env1804
+            20.04 Clang:
+              CONTAINER: env2004
               CC: clang
               CXX: clang++
               BUILD_GPU: ON
@@ -92,6 +111,7 @@ stages:
         variables:
           BUILD_DIR: '$(Agent.BuildDirectory)/build'
           CMAKE_CXX_FLAGS: '-Wall -Wextra'
+          DISPLAY: :99.0 # Checked for in CMake
         steps:
           - template: build/ubuntu.yaml
       - job: ubuntu_indices
@@ -100,11 +120,11 @@ stages:
         dependsOn: osx
         condition: succeededOrFailed()
         pool:
-          vmImage: 'Ubuntu 16.04'
+          vmImage: 'Ubuntu 20.04'
         strategy:
           matrix:
-            18.04 Clang:
-              CONTAINER: env1804
+            20.04 Clang:
+              CONTAINER: env2004
               CC: clang
               CXX: clang++
               INDEX_SIGNED: OFF
@@ -122,37 +142,27 @@ stages:
     displayName: Build MSVC
     dependsOn: formatting
     jobs:
-      - job: vs2017
-        displayName: Windows VS2017 Build
+      - job: Windows
+        displayName: Windows Build
         pool:
-          vmImage: 'vs2017-win2016'
+          vmImage: 'windows-2019'
         strategy:
           matrix:
             x86:
+              CONTAINER: winx86
               PLATFORM: 'x86'
               ARCHITECTURE: 'x86'
-              GENERATOR: 'Visual Studio 15 2017'
+              GENERATOR: '"Visual Studio 16 2019" -A Win32'
             x64:
+              CONTAINER: winx64
               PLATFORM: 'x64'
               ARCHITECTURE: 'x86_amd64'
-              GENERATOR: 'Visual Studio 15 2017 Win64'
+              GENERATOR: '"Visual Studio 16 2019" -A x64'
+        container: $[ variables['CONTAINER'] ]
         timeoutInMinutes: 0
         variables:
-          BUILD_DIR: '$(Agent.WorkFolder)\build'
+          BUILD_DIR: 'c:\build'
           CONFIGURATION: 'Release'
-          VCPKG_ROOT: 'C:\vcpkg'
+          VCPKG_ROOT: 'c:\vcpkg'
         steps:
           - template: build/windows.yaml
-
-  - stage: documentation
-    displayName: Documentation
-    dependsOn: []
-    jobs:
-      - template: documentation.yaml
-
-  - stage: tutorials
-    displayName: Tutorials
-    dependsOn: build_gcc
-    jobs:
-      - template: tutorials.yaml
-      
diff --git a/.ci/azure-pipelines/build/macos.yaml b/.ci/azure-pipelines/build/macos.yaml
index 985aaa35..36631166 100644
--- a/.ci/azure-pipelines/build/macos.yaml
+++ b/.ci/azure-pipelines/build/macos.yaml
@@ -3,7 +3,8 @@ steps:
     # find the commit hash on a quick non-forced update too
     fetchDepth: 10
   - script: |
-      brew install cmake pkg-config boost eigen flann glew libusb qhull vtk glew qt5 libpcap libomp brewsci/science/openni
+      brew install cmake pkg-config boost eigen flann glew libusb qhull vtk glew qt5 libpcap libomp google-benchmark
+      brew install brewsci/science/openni
       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'
@@ -19,6 +20,7 @@ steps:
         -DPCL_ONLY_CORE_POINT_TYPES=ON \
         -DBUILD_simulation=ON \
         -DBUILD_global_tests=ON \
+        -DBUILD_benchmarks=ON \
         -DBUILD_examples=ON \
         -DBUILD_tools=ON \
         -DBUILD_apps=ON \
diff --git a/.ci/azure-pipelines/build/ubuntu.yaml b/.ci/azure-pipelines/build/ubuntu.yaml
index 559dfddd..961e34ea 100644
--- a/.ci/azure-pipelines/build/ubuntu.yaml
+++ b/.ci/azure-pipelines/build/ubuntu.yaml
@@ -1,4 +1,6 @@
 steps:
+  - script: (nohup Xvfb :99 -screen 0 1280x1024x24 -nolisten tcp -nolisten unix &)
+    displayName: "Start Xvfb for DISPLAY=$(DISPLAY)"
   - checkout: self
     # find the commit hash on a quick non-forced update too
     fetchDepth: 10
@@ -12,6 +14,7 @@ steps:
       -DBUILD_simulation=ON \
       -DBUILD_surface_on_nurbs=ON \
       -DBUILD_global_tests=ON \
+      -DBUILD_benchmarks=ON \
       -DBUILD_examples=ON \
       -DBUILD_tools=ON \
       -DBUILD_apps=ON \
diff --git a/.ci/azure-pipelines/build/ubuntu_indices.yaml b/.ci/azure-pipelines/build/ubuntu_indices.yaml
index 28f8340d..fd2e8366 100644
--- a/.ci/azure-pipelines/build/ubuntu_indices.yaml
+++ b/.ci/azure-pipelines/build/ubuntu_indices.yaml
@@ -10,11 +10,6 @@ steps:
       -DPCL_ONLY_CORE_POINT_TYPES=ON \
       -DPCL_INDEX_SIGNED=$INDEX_SIGNED \
       -DPCL_INDEX_SIZE=$INDEX_SIZE \
-      -DBUILD_geometry=OFF \
-      -DBUILD_tools=OFF \
-      -DBUILD_kdtree=OFF \
-      -DBUILD_ml=OFF \
-      -DBUILD_octree=OFF \
       -DBUILD_global_tests=ON 
       # Temporary fix to ensure no tests are skipped
       cmake $(Build.SourcesDirectory)
diff --git a/.ci/azure-pipelines/build/windows.yaml b/.ci/azure-pipelines/build/windows.yaml
index b2a884c0..c9839847 100644
--- a/.ci/azure-pipelines/build/windows.yaml
+++ b/.ci/azure-pipelines/build/windows.yaml
@@ -3,43 +3,32 @@ steps:
     # 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%
+      mkdir %BUILD_DIR% && cd %BUILD_DIR% && dir
       cmake $(Build.SourcesDirectory) ^
-        -G"%GENERATOR%" ^
+        -G%GENERATOR% ^
+        -DVCPKG_TARGET_TRIPLET=%PLATFORM%-windows-rel ^
         -DCMAKE_BUILD_TYPE="MinSizeRel" ^
-        -DCMAKE_TOOLCHAIN_FILE=%VCPKG_ROOT%\scripts\buildsystems\vcpkg.cmake ^
+        -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_benchmarks=ON ^
         -DBUILD_tools=OFF ^
-        -DBUILD_surface_on_nurbs=ON
+        -DBUILD_surface_on_nurbs=ON ^
+        -DPCL_DISABLE_VISUALIZATION_TESTS=ON
     displayName: 'CMake Configuration'
-  - script: cd %BUILD_DIR% && cmake --build . --config %CONFIGURATION%
+  - script: |
+      cd %BUILD_DIR% && cmake --build . --config %CONFIGURATION%
     displayName: 'Build Library'
-  - script: cd %BUILD_DIR% && cmake --build . --target tests --config %CONFIGURATION%
+  - 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'
+      searchFolder: '$(BUILD_DIR)'
     condition: succeededOrFailed()
 
diff --git a/.ci/azure-pipelines/docs-pipeline.yaml b/.ci/azure-pipelines/docs-pipeline.yaml
new file mode 100644
index 00000000..e9c787cb
--- /dev/null
+++ b/.ci/azure-pipelines/docs-pipeline.yaml
@@ -0,0 +1,46 @@
+trigger:
+  paths:
+    include:
+    - doc
+
+pr:
+  paths:
+    include:
+    - doc
+
+resources:
+  pipelines:
+    - pipeline: Build-CI
+      source: Build
+      trigger:
+        stages:
+        - build_gcc
+  containers:
+    - container: fmt # for formatting.yaml
+      image: pointcloudlibrary/fmt
+    - container: doc # for documentation.yaml
+      image: pointcloudlibrary/doc
+    - container: env1804 # for tutorials.yaml
+      image: pointcloudlibrary/env:18.04
+
+stages:
+  - stage: formatting
+    displayName: Formatting
+    # if docs pipeline triggered by build_gcc stage,
+    # the formatting stage has already run, thus it
+    # won't run for a second time here.
+    condition: ne(variables['Build.Reason'], 'ResourceTrigger')
+    jobs:
+      - template: formatting.yaml
+
+  - stage: documentation
+    displayName: Documentation
+    condition: in(dependencies.formatting.result, 'Succeeded', 'SucceededWithIssues', 'Skipped')
+    jobs:
+      - template: documentation.yaml
+
+  - stage: tutorials
+    displayName: Tutorials
+    condition: in(dependencies.documentation.result, 'Succeeded', 'SucceededWithIssues')
+    jobs:
+      - template: tutorials.yaml
diff --git a/.ci/azure-pipelines/documentation.yaml b/.ci/azure-pipelines/documentation.yaml
index 9eaacba3..9cfcbfb8 100644
--- a/.ci/azure-pipelines/documentation.yaml
+++ b/.ci/azure-pipelines/documentation.yaml
@@ -2,7 +2,7 @@ jobs:
   - job: documentation
     displayName: Generate Documentation
     pool:
-      vmImage: 'Ubuntu 16.04'
+      vmImage: 'Ubuntu 20.04'
     container: doc
     variables:
       BUILD_DIR: '$(Agent.BuildDirectory)/build'
diff --git a/.ci/azure-pipelines/env.yml b/.ci/azure-pipelines/env.yml
index d3c6f7aa..00ca0a80 100644
--- a/.ci/azure-pipelines/env.yml
+++ b/.ci/azure-pipelines/env.yml
@@ -9,12 +9,14 @@ trigger:
   paths:
     include:
     - .dev/docker/env/Dockerfile
+    - .dev/docker/windows
     - .ci/azure-pipelines/env.yml
 
 pr:
   paths:
     include:
     - .dev/docker/env/Dockerfile
+    - .dev/docker/windows
     - .ci/azure-pipelines/env.yml
 
 schedules:
@@ -32,32 +34,39 @@ variables:
   dockerHubID: "pointcloudlibrary"
 
 jobs:
-- job: BuildAndPush
+- job: BuildAndPushUbuntu
   timeoutInMinutes: 360
   displayName: "Env"
   pool:
     vmImage: 'ubuntu-latest'
   strategy:
     matrix:
-      Ubuntu 16.04:
-        CUDA_VERSION: 9.2
-        UBUNTU_DISTRO: 16.04
-        USE_CUDA: false
-        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
+        TAG: 18.04
       Ubuntu 20.04:
-        CUDA_VERSION: 11
+        CUDA_VERSION: 11.2.1
         UBUNTU_DISTRO: 20.04
         VTK_VERSION: 7
-        # nvidia-cuda docker image has not been released for 20.04 yet
+        USE_CUDA: true
+        TAG: 20.04
+      Ubuntu 20.10:
+        CUDA_VERSION: 11.2.1
+        UBUNTU_DISTRO: 20.10
+        VTK_VERSION: 7
+        # nvidia-cuda docker image has not been released for 20.10 yet
+        USE_CUDA: ""
+        TAG: 20.10
+      Ubuntu 21.04:
+        CUDA_VERSION: 11.2.1
+        UBUNTU_DISTRO: 21.04
+        VTK_VERSION: 9
+        # nvidia-cuda docker image has not been released for 21.04 yet
         USE_CUDA: ""
-        tag: 20.04
+        TAG: 21.04
   steps:
   - task: Docker@2
     displayName: "Build docker image"
@@ -69,12 +78,12 @@ jobs:
         --build-arg UBUNTU_DISTRO=$(UBUNTU_DISTRO)
         --build-arg USE_CUDA=$(USE_CUDA)
         --build-arg VTK_VERSION=$(VTK_VERSION)
-        -t $(dockerHubID)/env:$(tag)
+        -t $(dockerHubID)/env:$(TAG)
       dockerfile: '$(Build.SourcesDirectory)/.dev/docker/env/Dockerfile'
-      tags: "$(tag)"
+      tags: "$(TAG)"
   - script: |
       set -x
-      docker run --rm -v "$(Build.SourcesDirectory)":/pcl $(dockerHubID)/env:$(tag) bash -c ' \
+      docker run --rm -v "$(Build.SourcesDirectory)":/pcl $(dockerHubID)/env:$(TAG) bash -c ' \
       mkdir /pcl/build && cd /pcl/build && \
       cmake /pcl \
         -DCMAKE_BUILD_TYPE="Release" \
@@ -89,6 +98,57 @@ jobs:
       command: push
       containerRegistry: $(dockerHub)
       repository: $(dockerHubID)/env
-      tags: "$(tag)"
+      tags: "$(TAG)"
+      condition: and(eq(variables['Build.Repository.Name'], 'PointCloudLibrary/pcl'),
+                     eq(variables['Build.SourceBranch'], 'refs/heads/master'))
+- job: BuildAndPushWindows
+  timeoutInMinutes: 360
+  displayName: "Env"
+  pool:
+    vmImage: 'windows-2019'
+  strategy:
+    matrix:
+      Winx86:
+        PLATFORM: x86
+        TAG: winx86
+        GENERATOR: "'Visual Studio 16 2019' -A Win32"
+        VCPKGCOMMIT: 2bc10eae2fb0b8c7c098325c4e9d82aa5d0329d9
+      Winx64:
+        PLATFORM: x64
+        TAG: winx64
+        GENERATOR: "'Visual Studio 16 2019' -A x64"
+        VCPKGCOMMIT: master
+  steps:
+  - task: Docker@2
+    displayName: "Build docker image"
+    inputs:
+      command: build
+      arguments: |
+        --no-cache
+        --build-arg PLATFORM=$(PLATFORM)
+        --build-arg VCPKGCOMMIT=$(VCPKGCOMMIT)
+        -t $(dockerHubID)/env:$(TAG)
+      dockerfile: '$(Build.SourcesDirectory)/.dev/docker/windows/Dockerfile'
+      tags: "$(TAG)"
+      
+  - script: >
+      docker run --rm -v "$(Build.SourcesDirectory)":c:\pcl $(dockerHubID)/env:$(TAG) 
+      powershell -command "mkdir c:\pcl\build; cd c:\pcl\build; 
+      cmake c:\pcl -G$(GENERATOR) 
+      -DVCPKG_TARGET_TRIPLET=$(PLATFORM)-windows-rel 
+      -DCMAKE_BUILD_TYPE='Release' 
+      -DCMAKE_TOOLCHAIN_FILE=c:\vcpkg\scripts\buildsystems\vcpkg.cmake 
+      -DPCL_ONLY_CORE_POINT_TYPES=ON 
+      -DBUILD_io:BOOL=OFF 
+      -DBUILD_kdtree:BOOL=OFF;
+      cmake --build . "
+    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
index 0033e72f..342e56ff 100644
--- a/.ci/azure-pipelines/formatting.yaml
+++ b/.ci/azure-pipelines/formatting.yaml
@@ -2,7 +2,7 @@ jobs:
   - job: formatting
     displayName: Check code formatting
     pool:
-      vmImage: 'Ubuntu 16.04'
+      vmImage: 'Ubuntu 20.04'
     container: fmt
     steps:
       - checkout: self
diff --git a/.ci/azure-pipelines/release.yaml b/.ci/azure-pipelines/release.yaml
index 225c846f..b088cf29 100644
--- a/.ci/azure-pipelines/release.yaml
+++ b/.ci/azure-pipelines/release.yaml
@@ -48,6 +48,20 @@ stages:
     timeoutInMinutes: 360
     pool:
       vmImage: 'ubuntu-latest'
+    strategy:
+      matrix:
+        INT32:
+          PCL_INDEX_SIGNED: true
+          PCL_INDEX_SIZE: 32
+        UINT32:
+          PCL_INDEX_SIGNED: false
+          PCL_INDEX_SIZE: 32
+        INT64:
+          PCL_INDEX_SIGNED: true
+          PCL_INDEX_SIZE: 64
+        UINT64:
+          PCL_INDEX_SIGNED: false
+          PCL_INDEX_SIZE: 64
     variables:
       tag: "release"
     steps:
@@ -61,6 +75,8 @@ stages:
         arguments: |
           --no-cache
           -t $(dockerHubID)/release:$(tag)
+          --build-arg PCL_INDEX_SIGNED=$(PCL_INDEX_SIGNED)
+          --build-arg PCL_INDEX_SIZE=$(PCL_INDEX_SIZE)
         dockerfile: '$(Build.SourcesDirectory)/.dev/docker/release/Dockerfile'
         tags: "$(tag)"
 - stage: ROS
diff --git a/.ci/azure-pipelines/tutorials.yaml b/.ci/azure-pipelines/tutorials.yaml
index 05922c47..6ca14276 100644
--- a/.ci/azure-pipelines/tutorials.yaml
+++ b/.ci/azure-pipelines/tutorials.yaml
@@ -2,8 +2,8 @@ jobs:
   - job: tutorials
     displayName: Building Tutorials
     pool:
-      vmImage: 'Ubuntu 16.04'
-    container: env1604
+      vmImage: 'Ubuntu 20.04'
+    container: env1804
     timeoutInMinutes: 0
     variables:
       BUILD_DIR: '$(Agent.BuildDirectory)/build'
diff --git a/.ci/scripts/build_tutorials.sh b/.ci/scripts/build_tutorials.sh
index 169128ec..c26d02d9 100755
--- a/.ci/scripts/build_tutorials.sh
+++ b/.ci/scripts/build_tutorials.sh
@@ -73,7 +73,7 @@ for DIRECTORY in "$SOURCE_DIR"/*/ ; do
     TUTORIAL_BUILD_DIR="$BUILD_DIR/$NAME"
     mkdir -p "$TUTORIAL_BUILD_DIR" && cd "$TUTORIAL_BUILD_DIR" || exit
     echo "Configuring tutorial: $NAME"
-    if ! cmake "$TUTORIAL_SOURCE_DIR" -DPCL_DIR="$INSTALL_DIR" -DCMAKE_CXX_FLAGS="-Werror"; then
+    if ! cmake "$TUTORIAL_SOURCE_DIR" -DPCL_DIR="$INSTALL_DIR" -DCMAKE_CXX_FLAGS="-Wall -Wextra -Wpedantic -Werror"; then
       STATUS="cmake error"
     else
       echo "Building tutorial: $NAME"
diff --git a/.dev/docker/env/Dockerfile b/.dev/docker/env/Dockerfile
index 6e06fb78..e2dcb662 100644
--- a/.dev/docker/env/Dockerfile
+++ b/.dev/docker/env/Dockerfile
@@ -14,6 +14,7 @@ ENV DEBIAN_FRONTEND=noninteractive
 
 RUN apt-get update \
  && apt-get install -y \
+      xvfb \
       cmake \
       g++ \
       clang \
@@ -26,6 +27,7 @@ RUN apt-get update \
       libflann-dev \
       libglew-dev \
       libgtest-dev \
+      libbenchmark-dev \
       libopenni-dev \
       libopenni2-dev \
       libproj-dev \
diff --git a/.dev/docker/release/Dockerfile b/.dev/docker/release/Dockerfile
index b7afd81b..8b156a98 100644
--- a/.dev/docker/release/Dockerfile
+++ b/.dev/docker/release/Dockerfile
@@ -3,6 +3,9 @@ FROM debian:testing
 ARG VTK_VERSION=7
 ENV DEBIAN_FRONTEND=noninteractive
 
+ARG PCL_INDEX_SIGNED=true
+ARG PCL_INDEX_SIZE=32
+
 # Add sources so we can just install build-dependencies of PCL
 RUN sed -i 's/^deb \(.*\)$/deb \1\ndeb-src \1/' /etc/apt/sources.list \
  && apt update \
@@ -57,6 +60,8 @@ RUN cd \
     -DBUILD_tools=ON -DBUILD_tracking=ON -DBUILD_visualization=ON       \
     -DBUILD_apps_cloud_composer=OFF -DBUILD_apps_modeler=ON             \
     -DBUILD_apps_point_cloud_editor=ON -DBUILD_apps_in_hand_scanner=ON  \
+    -DPCL_INDEX_SIGNED=${PCL_INDEX_SIGNED} \
+    -DPCL_INDEX_SIZE=${PCL_INDEX_SIZE} \
  && make install -j2 \
  && cd \
  && rm -fr pcl
diff --git a/.dev/docker/windows/Dockerfile b/.dev/docker/windows/Dockerfile
new file mode 100644
index 00000000..6901105d
--- /dev/null
+++ b/.dev/docker/windows/Dockerfile
@@ -0,0 +1,47 @@
+# escape=`
+
+FROM mcr.microsoft.com/windows/servercore:ltsc2019
+
+# Use "--build-arg platform=x64" for 64 bit or x86 for 32 bit.
+ARG PLATFORM
+
+# Use to set specific commit to checkout
+ARG VCPKGCOMMIT
+
+# Download channel for fixed install.
+ARG CHANNEL_BASE_URL=https://aka.ms/vs/16/release
+
+ADD $CHANNEL_BASE_URL/channel C:\TEMP\VisualStudio.chman
+
+SHELL ["powershell", "-Command", "$ErrorActionPreference = 'Stop'; $ProgressPreference = 'SilentlyContinue';"]
+
+# Download and install Build Tools for Visual Studio 2019 for native desktop
+RUN wget $Env:CHANNEL_BASE_URL/vs_buildtools.exe -OutFile 'C:\TEMP\vs_buildtools.exe'; `
+    Start-Process -FilePath C:\TEMP\vs_buildtools.exe -ArgumentList `
+    "--quiet",                                                      `
+    "--norestart",                                                  `
+    "--nocache",                                                    `
+    "--installPath",                                                `
+    "C:\BuildTools",                                                `
+    "--wait",                                                       `
+    "--channelUri",                                                 `
+    "C:\TEMP\VisualStudio.chman",                                   `
+    "--installChannelUri",                                          `
+    "C:\TEMP\VisualStudio.chman",                                   `
+    "--add",                                                        `
+    "Microsoft.VisualStudio.Workload.VCTools",                      `
+    "--includeRecommended"                                          `
+    -Wait -PassThru;                                                `
+    del c:\temp\vs_buildtools.exe;                                  
+
+RUN iex ((New-Object System.Net.WebClient).DownloadString('https://chocolatey.org/install.ps1'));        `
+    choco install cmake git --installargs 'ADD_CMAKE_TO_PATH=System' -y --no-progress
+
+RUN git clone https://github.com/microsoft/vcpkg.git; cd vcpkg; git checkout $Env:VCPKGCOMMIT;
+
+# To explicit set VCPKG to only build Release version of the libraries.
+COPY $PLATFORM'-windows-rel.cmake' 'c:\vcpkg\triplets\'$PLATFORM'-windows-rel.cmake'
+
+RUN cd .\vcpkg;                                                `
+    .\bootstrap-vcpkg.bat;                                            `
+    .\vcpkg install boost flann eigen3 qhull vtk[qt,opengl] gtest benchmark --triplet $Env:PLATFORM-windows-rel --clean-after-build;
diff --git a/.dev/docker/windows/x64-windows-rel.cmake b/.dev/docker/windows/x64-windows-rel.cmake
new file mode 100644
index 00000000..f6c40253
--- /dev/null
+++ b/.dev/docker/windows/x64-windows-rel.cmake
@@ -0,0 +1,4 @@
+set(VCPKG_TARGET_ARCHITECTURE x64)
+set(VCPKG_CRT_LINKAGE dynamic)
+set(VCPKG_LIBRARY_LINKAGE dynamic)
+set(VCPKG_BUILD_TYPE release)
diff --git a/.dev/docker/windows/x86-windows-rel.cmake b/.dev/docker/windows/x86-windows-rel.cmake
new file mode 100644
index 00000000..0a277bdb
--- /dev/null
+++ b/.dev/docker/windows/x86-windows-rel.cmake
@@ -0,0 +1,4 @@
+set(VCPKG_TARGET_ARCHITECTURE x86)
+set(VCPKG_CRT_LINKAGE dynamic)
+set(VCPKG_LIBRARY_LINKAGE dynamic)
+set(VCPKG_BUILD_TYPE release)
diff --git a/.dev/format.sh b/.dev/format.sh
index 6acff827..dba26190 100755
--- a/.dev/format.sh
+++ b/.dev/format.sh
@@ -8,7 +8,7 @@
 
 format() {
     # don't use a directory with whitespace
-    local whitelist="apps/3d_rec_framework apps/modeler 2d ml octree simulation stereo tracking"
+    local whitelist="apps/3d_rec_framework apps/include apps/modeler apps/src benchmarks 2d geometry ml octree simulation stereo tracking registration gpu/containers"
 
     local PCL_DIR="${2}"
     local formatter="${1}"
diff --git a/2d/CMakeLists.txt b/2d/CMakeLists.txt
index 8bd5411e..437d23fb 100644
--- a/2d/CMakeLists.txt
+++ b/2d/CMakeLists.txt
@@ -31,8 +31,6 @@ set(impl_incs
 )
 
 if(${VTK_FOUND})
-  set(VTK_USE_FILE "${VTK_USE_FILE}" CACHE INTERNAL "VTK_USE_FILE")
-  include("${VTK_USE_FILE}")
   set(VTK_IO_TARGET_LINK_LIBRARIES vtkCommon vtkWidgets vtkIO vtkImaging)
 endif()
 
diff --git a/CHANGES.md b/CHANGES.md
index 533d35f4..5fd55b71 100644
--- a/CHANGES.md
+++ b/CHANGES.md
@@ -1,5 +1,227 @@
 # ChangeList
 
+## = 1.12.0 (2021.07.07) =
+
+PCL 1.12.0 enables custom index size and type, from `int16_t` to `uint64_t`, allowing
+users to have as small or large clouds as they wish. 1.12 also comes with improved
+support for VTK and CUDA, along with making existing functionality more user friendly.
+
+This is all on top of the usual bug-fixes and performance improvements across the board
+
+### Notable changes
+
+**New features** *added to PCL*
+
+* **[sample_consensus]** Add SIMD implementations to some countWithinDistance functions [[#3519](https://github.com/PointCloudLibrary/pcl/pull/3519)]
+* **[io]** Enable Real Sense 2 grabber for all platforms [[#4471](https://github.com/PointCloudLibrary/pcl/pull/4471)]
+* **[visualization]** add ellipsoid shape to pcl_visualizer [[#4531](https://github.com/PointCloudLibrary/pcl/pull/4531)]
+* **[common]** Add `constexpr` to static member functions for point types, add macro for `if constexpr` [[#4735](https://github.com/PointCloudLibrary/pcl/pull/4735)]
+* **[ci]** Use windows docker image in CI. [[#4426](https://github.com/PointCloudLibrary/pcl/pull/4426)]
+* **[common]** Add pcl log stream macros [[#4595](https://github.com/PointCloudLibrary/pcl/pull/4595)]
+
+**Deprecation** *of public APIs, scheduled to be removed after two minor releases*
+
+* **[common]** Modify index type for vertices [[#4256](https://github.com/PointCloudLibrary/pcl/pull/4256)]
+* **[gpu]** Add square distances to GPU knnSearch API [[#4322](https://github.com/PointCloudLibrary/pcl/pull/4322)]
+* **[gpu]** Add square distances to ApproxNearestSearch [[#4340](https://github.com/PointCloudLibrary/pcl/pull/4340)]
+* Deprecate unused ease-of-internal-use headers [[#4367](https://github.com/PointCloudLibrary/pcl/pull/4367)]
+* **[registration]** Deprecate `TransformationEstimationDQ` in favor of `TransformationEstimationDualQuaternion` [[#4425](https://github.com/PointCloudLibrary/pcl/pull/4425)]
+* **[segmentation]** Deprecate unused `max_label` in `extractLabeledEuclideanClusters` [[#4105](https://github.com/PointCloudLibrary/pcl/pull/4105)]
+* **[surface]** MLS: correct typo in `principle` by using `principal` instead [[#4505](https://github.com/PointCloudLibrary/pcl/pull/4505)]
+* Deprecate unneeded meta-headers [[#4628](https://github.com/PointCloudLibrary/pcl/pull/4628)]
+* **[apps][tracking]** pyramidal klt: switch keypoints_status_ to int vector [[#4681](https://github.com/PointCloudLibrary/pcl/pull/4681)]
+* Properly remove remaining items deprecated for version 1.12, deprecate `uniform_sampling.h` [[#4688](https://github.com/PointCloudLibrary/pcl/pull/4688)]
+* **[recognition]** Add deprecation for incorrectly installed headers [[#4650](https://github.com/PointCloudLibrary/pcl/pull/4650)]
+
+**Removal** *of the public APIs deprecated in previous releases*
+
+* Remove deprecated items as scheduled in preparation of v1.12 (except `concatenatePointCloud`) [[#4341](https://github.com/PointCloudLibrary/pcl/pull/4341)]
+* **[apps]** Remove unused code in persistence_utils.h [[#4500](https://github.com/PointCloudLibrary/pcl/pull/4500)]
+* Properly remove remaining items deprecated for version 1.12, deprecate `uniform_sampling.h` [[#4688](https://github.com/PointCloudLibrary/pcl/pull/4688)]
+
+**Behavior changes** *in classes, apps, or tools*
+
+* **[registration]** Don't move, or copy ICP [[#4167](https://github.com/PointCloudLibrary/pcl/pull/4167)]
+* **[common]** Fix PointXYZRGBA ctor, set A as 255 by default [[#4799](https://github.com/PointCloudLibrary/pcl/pull/4799)]
+
+**API changes** *that did not go through the proper deprecation and removal cycle*
+
+* **[common]** modify index type for PCLImage [[#4257](https://github.com/PointCloudLibrary/pcl/pull/4257)]
+* **[registration]** Don't move, or copy ICP [[#4167](https://github.com/PointCloudLibrary/pcl/pull/4167)]
+* **[kdtree]** KdTree: handle 0 or negative k for nearestKSearch [[#4430](https://github.com/PointCloudLibrary/pcl/pull/4430)]
+* **[common]** Use `std::array` instead of C-array for ColorLUT [[#4489](https://github.com/PointCloudLibrary/pcl/pull/4489)]
+* **[tracking]** Use SFINAE instead of relying on macro `PCL_TRACKING_NORMAL_SUPPORTED` [[#4643](https://github.com/PointCloudLibrary/pcl/pull/4643)]
+* **[gpu]** Export and template extract clusters [[#4196](https://github.com/PointCloudLibrary/pcl/pull/4196)]
+* **[common]** Added `namespace pcl` to free functions: `aligned_{malloc/free}` [[#4742](https://github.com/PointCloudLibrary/pcl/pull/4742)]
+
+**ABI changes** *that are still API compatible*
+
+* **[registration]** Refactoring and Bugfix of NDT [[#4180](https://github.com/PointCloudLibrary/pcl/pull/4180)]
+* **[common]** modify index types for PCLPointCloud2 [[#4199](https://github.com/PointCloudLibrary/pcl/pull/4199)]
+* **[common]** Modify index type for vertices [[#4256](https://github.com/PointCloudLibrary/pcl/pull/4256)]
+* **[common]** Modify index type for PCLPointField [[#4228](https://github.com/PointCloudLibrary/pcl/pull/4228)]
+* **[surface]** Enabled multithreading in Poisson surface reconstruction [[#4332](https://github.com/PointCloudLibrary/pcl/pull/4332)]
+* **[io]** Allow file_io to read large point clouds depending on PCL config [[#4331](https://github.com/PointCloudLibrary/pcl/pull/4331)]
+* **[sample_consensus]** Allow user to apply arbitrary constraint on models in sample consensus [[#4260](https://github.com/PointCloudLibrary/pcl/pull/4260)]
+* **[tracking]** Use SFINAE instead of relying on macro `PCL_TRACKING_NORMAL_SUPPORTED` [[#4643](https://github.com/PointCloudLibrary/pcl/pull/4643)]
+* **[features]** Move the init of static variables to library load time [[#4640](https://github.com/PointCloudLibrary/pcl/pull/4640)]
+* **[octree]** Octree2BufBase: Fix bug that contents from previous buffer appear in current buffer [[#4642](https://github.com/PointCloudLibrary/pcl/pull/4642)]
+
+### Changes grouped by module
+
+#### CMake:
+
+* Update `pcl_find_boost` to allow compilation with Boost 1.74 [[#4330](https://github.com/PointCloudLibrary/pcl/pull/4330)]
+* Variable needs to be expanded when checking for `EXT_DEPS` [[#4353](https://github.com/PointCloudLibrary/pcl/pull/4353)]
+* Update pcl_find_cuda.cmake to contain all supported architectures [[#4400](https://github.com/PointCloudLibrary/pcl/pull/4400)]
+* Add support for VTK 9 [[#4262](https://github.com/PointCloudLibrary/pcl/pull/4262)]
+* Refactor cmake find script of libusb [[#4483](https://github.com/PointCloudLibrary/pcl/pull/4483)]
+* Add AVX for windows [[#4598](https://github.com/PointCloudLibrary/pcl/pull/4598)]
+* Add SSE definitions for SSE 4.1 and 4.2 [[#4596](https://github.com/PointCloudLibrary/pcl/pull/4596)]
+
+#### libpcl_common:
+
+* **[ABI break]** modify index types for PCLPointCloud2 [[#4199](https://github.com/PointCloudLibrary/pcl/pull/4199)]
+* **[API break]** modify index type for PCLImage [[#4257](https://github.com/PointCloudLibrary/pcl/pull/4257)]
+* **[ABI break][deprecation]** Modify index type for vertices [[#4256](https://github.com/PointCloudLibrary/pcl/pull/4256)]
+* **[ABI break]** Modify index type for PCLPointField [[#4228](https://github.com/PointCloudLibrary/pcl/pull/4228)]
+* Allow PCL_DEPRECATED to detect and help remove deprecations before release [[#4336](https://github.com/PointCloudLibrary/pcl/pull/4336)]
+* Allow conversion of PointCloud with more than 32-bit size rows/columns [[#4343](https://github.com/PointCloudLibrary/pcl/pull/4343)]
+* Improve routing for `transformPointCloud` [[#4398](https://github.com/PointCloudLibrary/pcl/pull/4398)]
+* Correct typo in `transformPlane` [[#4396](https://github.com/PointCloudLibrary/pcl/pull/4396)]
+* **[API break]** Use `std::array` instead of C-array for ColorLUT [[#4489](https://github.com/PointCloudLibrary/pcl/pull/4489)]
+* Set header in two toPCLPointCloud2 functions [[#4538](https://github.com/PointCloudLibrary/pcl/pull/4538)]
+* Add more operators to `PointCloud` to prevent perf regression in refactoring [[#4397](https://github.com/PointCloudLibrary/pcl/pull/4397)]
+* Make sure that organized point clouds are still organized after transformPointCloud [[#4488](https://github.com/PointCloudLibrary/pcl/pull/4488)]
+* **[API break]** Added `namespace pcl` to free functions: `aligned_{malloc/free}` [[#4742](https://github.com/PointCloudLibrary/pcl/pull/4742)]
+* **[new feature]** Add `constexpr` to static member functions for point types, add macro for `if constexpr` [[#4735](https://github.com/PointCloudLibrary/pcl/pull/4735)]
+* Fix `PolygonMesh::concatenate` and its unit test [[#4745](https://github.com/PointCloudLibrary/pcl/pull/4745)]
+* **[behavior change]** Fix PointXYZRGBA ctor, set A as 255 by default [[#4799](https://github.com/PointCloudLibrary/pcl/pull/4799)]
+* Remove pseudo-template-instantiations in eigen.h to reduce compilation time [[#4788](https://github.com/PointCloudLibrary/pcl/pull/4788)]
+* **[new feature]** Add pcl log stream macros [[#4595](https://github.com/PointCloudLibrary/pcl/pull/4595)]
+
+#### libpcl_features:
+
+* **[ABI break]** Move the init of static variables to library load time [[#4640](https://github.com/PointCloudLibrary/pcl/pull/4640)]
+* Use correct cloud for checking finite-ness in fpfh [[#4720](https://github.com/PointCloudLibrary/pcl/pull/4720)]
+
+#### libpcl_filters:
+
+* Improve performance of median filter by using `nth_element` [[#4360](https://github.com/PointCloudLibrary/pcl/pull/4360)]
+* Fix the covariance calculation as suggested by @zxd123 [[#4466](https://github.com/PointCloudLibrary/pcl/pull/4466)]
+* Filters: fix wrong initialization of covariance in VoxelGridCovariance [[#4556](https://github.com/PointCloudLibrary/pcl/pull/4556)]
+* Fix application of setMinimumPointsNumberPerVoxel for PCLPointCloud2 implementation of VoxelGrid [[#4389](https://github.com/PointCloudLibrary/pcl/pull/4389)]
+* Adding tests for CropHull and using hull_cloud instead of input in getHullCloudRange [[#3976](https://github.com/PointCloudLibrary/pcl/pull/3976)]
+
+#### libpcl_gpu:
+
+* **[deprecation]** Add square distances to GPU knnSearch API [[#4322](https://github.com/PointCloudLibrary/pcl/pull/4322)]
+* **[deprecation]** Add square distances to ApproxNearestSearch [[#4340](https://github.com/PointCloudLibrary/pcl/pull/4340)]
+* **[API break]** Export and template extract clusters [[#4196](https://github.com/PointCloudLibrary/pcl/pull/4196)]
+* Update support for CUDA arch in CMake and `convertSMVer2Cores` [[#4748](https://github.com/PointCloudLibrary/pcl/pull/4748)]
+* Add ability to download contiguous chunk of memory to host using `Device{Array,Memory}` [[#4741](https://github.com/PointCloudLibrary/pcl/pull/4741)]
+* Speeding up GPU clustering using smarter download strategy and memory allocations [[#4677](https://github.com/PointCloudLibrary/pcl/pull/4677)]
+
+#### libpcl_io:
+
+* **[ABI break]** Allow file_io to read large point clouds depending on PCL config [[#4331](https://github.com/PointCloudLibrary/pcl/pull/4331)]
+* Improve PCD read performance (more than 50%) by reusing `istringstream` [[#4339](https://github.com/PointCloudLibrary/pcl/pull/4339)]
+* **[new feature]** Enable Real Sense 2 grabber for all platforms [[#4471](https://github.com/PointCloudLibrary/pcl/pull/4471)]
+* Throw error if the device bluffs about its capability [[#4141](https://github.com/PointCloudLibrary/pcl/pull/4141)]
+* Fix crash in Dinast Grabber due to bad initialization of device handle [[#4484](https://github.com/PointCloudLibrary/pcl/pull/4484)]
+* PLY face definition accepts uint fields as well [[#4492](https://github.com/PointCloudLibrary/pcl/pull/4492)]
+* Prevent segfault in vtk2mesh [[#4581](https://github.com/PointCloudLibrary/pcl/pull/4581)]
+* Prevent exception in PCDReader for misformed PCD files [[#4566](https://github.com/PointCloudLibrary/pcl/pull/4566)]
+* Enable arbitary size Indices for Octree module [[#4350](https://github.com/PointCloudLibrary/pcl/pull/4350)]
+* Fix addition of Carriage Return to PCD files. [[#4727](https://github.com/PointCloudLibrary/pcl/pull/4727)]
+* Support Ensenso SDK 3.0 for ensenso_grabber [[#4751](https://github.com/PointCloudLibrary/pcl/pull/4751)]
+* Specify no face elements in PLY files (from point cloud) to make them interoperable with VTK [[#4774](https://github.com/PointCloudLibrary/pcl/pull/4774)]
+
+#### libpcl_kdtree:
+
+* **[API break]** KdTree: handle 0 or negative k for nearestKSearch [[#4430](https://github.com/PointCloudLibrary/pcl/pull/4430)]
+
+#### libpcl_ml:
+
+* Fix un-initialized centroids bug (k-means) [[#4570](https://github.com/PointCloudLibrary/pcl/pull/4570)]
+
+#### libpcl_octree:
+
+* Enable arbitary size Indices for Octree module [[#4350](https://github.com/PointCloudLibrary/pcl/pull/4350)]
+* Fix problems in octree search functions when using dynamic depth [[#4657](https://github.com/PointCloudLibrary/pcl/pull/4657)]
+* **[ABI break]** Octree2BufBase: Fix bug that contents from previous buffer appear in current buffer [[#4642](https://github.com/PointCloudLibrary/pcl/pull/4642)]
+
+#### libpcl_outofcore:
+
+* Fix compile issue due to missing include under MSVC 2019 [[#4755](https://github.com/PointCloudLibrary/pcl/pull/4755)]
+
+#### libpcl_recognition:
+
+* **[deprecation]** Add deprecation for incorrectly installed headers [[#4650](https://github.com/PointCloudLibrary/pcl/pull/4650)]
+
+#### libpcl_registration:
+
+* **[ABI break]** Refactoring and Bugfix of NDT [[#4180](https://github.com/PointCloudLibrary/pcl/pull/4180)]
+* **[API break][behavior change]** Don't move, or copy ICP [[#4167](https://github.com/PointCloudLibrary/pcl/pull/4167)]
+* **[deprecation]** Deprecate `TransformationEstimationDQ` in favor of `TransformationEstimationDualQuaternion` [[#4425](https://github.com/PointCloudLibrary/pcl/pull/4425)]
+* Fix force no recompute [[#4535](https://github.com/PointCloudLibrary/pcl/pull/4535)]
+* Skip non-finite points for Pyramid Feature Matching [[#4711](https://github.com/PointCloudLibrary/pcl/pull/4711)]
+
+#### libpcl_sample_consensus:
+
+* **[ABI break]** Allow user to apply arbitrary constraint on models in sample consensus [[#4260](https://github.com/PointCloudLibrary/pcl/pull/4260)]
+* Improve logging errors during sample consensus model registration [[#4381](https://github.com/PointCloudLibrary/pcl/pull/4381)]
+* **[new feature]** Add SIMD implementations to some countWithinDistance functions [[#3519](https://github.com/PointCloudLibrary/pcl/pull/3519)]
+* Faster sample consensus functions [[#4424](https://github.com/PointCloudLibrary/pcl/pull/4424)]
+* Fix and improve MLESAC [[#4575](https://github.com/PointCloudLibrary/pcl/pull/4575)]
+* Improve logging in module `sample_consensus` [[#4261](https://github.com/PointCloudLibrary/pcl/pull/4261)]
+
+#### libpcl_search:
+
+* Faster organized search [[#4496](https://github.com/PointCloudLibrary/pcl/pull/4496)]
+* Add access to boxSearch [[#4282](https://github.com/PointCloudLibrary/pcl/pull/4282)]
+
+#### libpcl_segmentation:
+
+* **[deprecation]** Deprecate unused `max_label` in `extractLabeledEuclideanClusters` [[#4105](https://github.com/PointCloudLibrary/pcl/pull/4105)]
+* Fix the dotproduct calculation in `extractEuclideanClusters` for smooth surfaces [[#4162](https://github.com/PointCloudLibrary/pcl/pull/4162)]
+* Make euclidean clustering with normals faster [[#4551](https://github.com/PointCloudLibrary/pcl/pull/4551)]
+
+#### libpcl_surface:
+
+* **[ABI break]** Enabled multithreading in Poisson surface reconstruction [[#4332](https://github.com/PointCloudLibrary/pcl/pull/4332)]
+* Add stdlib header for malloc in poisson (bugfix for gcc-5) [[#4376](https://github.com/PointCloudLibrary/pcl/pull/4376)]
+* Always update counter and prevent overflow access in poisson4 octree [[#4316](https://github.com/PointCloudLibrary/pcl/pull/4316)]
+* Add missing include to nurbs_solve_umfpack.cpp [[#4571](https://github.com/PointCloudLibrary/pcl/pull/4571)]
+* **[deprecation]** MLS: correct typo in `principle` by using `principal` instead [[#4505](https://github.com/PointCloudLibrary/pcl/pull/4505)]
+
+#### libpcl_visualization:
+
+* Add support for VTK 9 [[#4262](https://github.com/PointCloudLibrary/pcl/pull/4262)]
+* **[new feature]** add ellipsoid shape to pcl_visualizer [[#4531](https://github.com/PointCloudLibrary/pcl/pull/4531)]
+
+#### PCL Apps:
+
+* **[removal]** Remove unused code in persistence_utils.h [[#4500](https://github.com/PointCloudLibrary/pcl/pull/4500)]
+* **[deprecation]** pyramidal klt: switch keypoints_status_ to int vector [[#4681](https://github.com/PointCloudLibrary/pcl/pull/4681)]
+
+#### PCL Docs:
+
+* Update documentation to be coherent with the style guide [[#4771](https://github.com/PointCloudLibrary/pcl/pull/4771)]
+
+#### PCL Tutorials:
+
+* Replace PassThrough with removeNaNFromPointCloud in 3 tutorials  [[#4760](https://github.com/PointCloudLibrary/pcl/pull/4760)]
+
+#### PCL Tools:
+
+* Fix virtual scanner [[#4730](https://github.com/PointCloudLibrary/pcl/pull/4730)]
+
+#### CI:
+
+* Make windows build on c:\ drive to fix out-of-disk-space errors [[#4382](https://github.com/PointCloudLibrary/pcl/pull/4382)]
+* **[new feature]** Use windows docker image in CI. [[#4426](https://github.com/PointCloudLibrary/pcl/pull/4426)]
+
 ## = 1.11.1 (13.08.2020) =
 
 Apart from the usual serving of bug-fixes and speed improvements, PCL 1.11.1 brings in
diff --git a/CMakeLists.txt b/CMakeLists.txt
index e5aa7f40..63b38df6 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.11.1)
+project(PCL VERSION 1.12.0)
 string(TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER)
 
 ### ---[ Find universal dependencies
@@ -90,18 +90,25 @@ if(PCL_ENABLE_SSE AND "${CMAKE_CXX_FLAGS}" STREQUAL "${CMAKE_CXX_FLAGS_DEFAULT}"
   PCL_CHECK_FOR_SSE()
 endif()
 
+# check for AVX flags for windows
+if(PCL_ENABLE_AVX AND "${CMAKE_CXX_FLAGS}" STREQUAL "${CMAKE_CXX_FLAGS_DEFAULT}")
+  include("${PCL_SOURCE_DIR}/cmake/pcl_find_avx.cmake")
+  PCL_CHECK_FOR_AVX()
+endif()
+
 # ---[ Unix/Darwin/Windows specific flags
 if(CMAKE_COMPILER_IS_GNUCXX)
   if("${CMAKE_CXX_FLAGS}" STREQUAL "${CMAKE_CXX_FLAGS_DEFAULT}")
     if (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 7)
-      set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wabi=11")
+      string(APPEND CMAKE_CXX_FLAGS " -Wabi=11")
     else()
-      set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wabi")
-    endif()
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wno-unknown-pragmas -fno-strict-aliasing -Wno-format-extra-args -Wno-sign-compare -Wno-invalid-offsetof -Wno-conversion ${SSE_FLAGS_STR}")
-    if(PCL_WARNINGS_ARE_ERRORS)
-      set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Werror")
+      string(APPEND CMAKE_CXX_FLAGS " -Wabi")
     endif()
+    string(APPEND CMAKE_CXX_FLAGS " -Wall -Wextra -Wno-unknown-pragmas -fno-strict-aliasing -Wno-format-extra-args -Wno-sign-compare -Wno-invalid-offsetof -Wno-conversion ${SSE_FLAGS}")
+  endif()
+
+  if(PCL_WARNINGS_ARE_ERRORS)
+    string(APPEND CMAKE_CXX_FLAGS " -Werror -fno-strict-aliasing")
   endif()
 
   if("${CMAKE_SHARED_LINKER_FLAGS}" STREQUAL "" AND NOT CMAKE_SYSTEM_NAME STREQUAL "Darwin")
@@ -110,10 +117,10 @@ if(CMAKE_COMPILER_IS_GNUCXX)
 
   if(WIN32)
     if(PCL_SHARED_LIBS)
-      set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--export-all-symbols -Wl,--enable-auto-import")
+      string(APPEND CMAKE_SHARED_LINKER_FLAGS " -Wl,--export-all-symbols -Wl,--enable-auto-import")
       if(MINGW)
         add_definitions("-DBOOST_THREAD_USE_LIB")
-        set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--allow-multiple-definition")
+        string(APPEND CMAKE_SHARED_LINKER_FLAGS " -Wl,--allow-multiple-definition")
       endif()
     else()
       add_definitions("-DBOOST_LIB_DIAGNOSTIC -DBOOST_THREAD_USE_LIB")
@@ -127,20 +134,20 @@ if(CMAKE_COMPILER_IS_MSVC)
   add_compile_options(/bigobj)
   
   if("${CMAKE_CXX_FLAGS}" STREQUAL "${CMAKE_CXX_FLAGS_DEFAULT}")
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /fp:precise /wd4800 /wd4521 /wd4251 /wd4275 /wd4305 /wd4355 ${SSE_FLAGS_STR}")
+    string(APPEND CMAKE_CXX_FLAGS " /fp:precise /wd4800 /wd4521 /wd4251 /wd4275 /wd4305 /wd4355 ${SSE_FLAGS} ${AVX_FLAGS}")
 
     # Add extra code generation/link optimizations
     if(CMAKE_MSVC_CODE_LINK_OPTIMIZATION AND (NOT BUILD_CUDA) AND (NOT BUILD_GPU))
-      set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} /GL")
-      set(CMAKE_SHARED_LINKER_FLAGS_RELEASE "${CMAKE_SHARED_LINKER_FLAGS_RELEASE} /LTCG /OPT:REF")
-      set(CMAKE_EXE_LINKER_FLAGS_RELEASE "${CMAKE_EXE_LINKER_FLAGS_RELEASE} /LTCG")
+      string(APPEND CMAKE_CXX_FLAGS_RELEASE " /GL")
+      string(APPEND CMAKE_SHARED_LINKER_FLAGS_RELEASE " /LTCG /OPT:REF")
+      string(APPEND CMAKE_EXE_LINKER_FLAGS_RELEASE " /LTCG")
     else()
       message("Global optimizations /GL has been turned off, as it doesn't work with nvcc/thrust")
     endif()
     # /MANIFEST:NO") # please, don't disable manifest generation, otherwise crash at start for vs2008
 
     if(PCL_WARNINGS_ARE_ERRORS)
-      set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /WX")
+      string(APPEND CMAKE_CXX_FLAGS " /WX")
     endif()
 
     include(ProcessorCount)
@@ -150,11 +157,11 @@ if(CMAKE_COMPILER_IS_MSVC)
       # Usage of COMPILE_LANGUAGE generator expression for MSVC in add_compile_options requires at least CMake 3.11, see https://gitlab.kitware.com/cmake/cmake/issues/17435	  
       if(MSVC_MP EQUAL 0)
         # MSVC_MP is 0 in case the information cannot be determined by ProcessorCount => fallback
-        set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /MP")
-        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP")
+        string(APPEND CMAKE_C_FLAGS " /MP")
+        string(APPEND CMAKE_CXX_FLAGS " /MP")
       elseif(MSVC_MP GREATER 1)
-        set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /MP${MSVC_MP}")
-        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP${MSVC_MP}")
+        string(APPEND CMAKE_C_FLAGS " /MP${MSVC_MP}")
+        string(APPEND CMAKE_CXX_FLAGS " /MP${MSVC_MP}")
       endif()      
     else()      
       if(MSVC_MP EQUAL 0)
@@ -168,8 +175,8 @@ if(CMAKE_COMPILER_IS_MSVC)
   endif()
 
   if(CMAKE_GENERATOR STREQUAL "Ninja")
-    set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /FS")
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /FS")
+    string(APPEND CMAKE_C_FLAGS " /FS")
+    string(APPEND CMAKE_CXX_FLAGS " /FS")
   endif()
 endif()
 
@@ -189,7 +196,7 @@ if(CMAKE_COMPILER_IS_CLANG)
   if("${CMAKE_CXX_FLAGS}" STREQUAL "")
     set(CMAKE_CXX_FLAGS "-ftemplate-depth=1024 -Qunused-arguments -Wno-invalid-offsetof ${SSE_FLAGS_STR}") # Unfortunately older Clang versions do not have this: -Wno-unnamed-type-template-args
     if(APPLE AND WITH_CUDA AND CUDA_FOUND)
-      set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -stdlib=libstdc++")
+      string(APPEND CMAKE_CXX_FLAGS " -stdlib=libstdc++")
     endif()
   endif()
   set(CLANG_LIBRARIES "stdc++")
@@ -263,8 +270,8 @@ if(WITH_OPENMP)
   find_package(OpenMP COMPONENTS C CXX)
 endif()
 if(OpenMP_FOUND)
-  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+  string(APPEND CMAKE_C_FLAGS " ${OpenMP_C_FLAGS}")
+  string(APPEND CMAKE_CXX_FLAGS " ${OpenMP_CXX_FLAGS}")
   if(${CMAKE_VERSION} VERSION_LESS "3.7")
     message(STATUS "Found OpenMP")
   else()
@@ -281,8 +288,8 @@ if(OpenMP_FOUND)
       set(OPENMP_DLL VCOMP140)
     endif()
     if(OPENMP_DLL)
-      set(CMAKE_SHARED_LINKER_FLAGS_DEBUG "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} /DELAYLOAD:${OPENMP_DLL}D.dll")
-      set(CMAKE_SHARED_LINKER_FLAGS_RELEASE "${CMAKE_SHARED_LINKER_FLAGS_RELEASE} /DELAYLOAD:${OPENMP_DLL}.dll")
+      string(APPEND CMAKE_SHARED_LINKER_FLAGS_DEBUG " /DELAYLOAD:${OPENMP_DLL}D.dll")
+      string(APPEND CMAKE_SHARED_LINKER_FLAGS_RELEASE " /DELAYLOAD:${OPENMP_DLL}.dll")
     else()
       message(WARNING "Delay loading flag for OpenMP DLL is invalid.")
     endif()
@@ -299,18 +306,15 @@ find_package(Eigen 3.1 REQUIRED)
 include_directories(SYSTEM ${EIGEN_INCLUDE_DIRS})
 
 # FLANN (required)
-if(NOT PCL_SHARED_LIBS OR ((WIN32 AND NOT MINGW) AND NOT PCL_BUILD_WITH_FLANN_DYNAMIC_LINKING_WIN32))
-  set(FLANN_USE_STATIC ON)
+find_package(FLANN 1.9.1 REQUIRED)
+if(NOT (${FLANN_LIBRARY_TYPE} MATCHES ${PCL_FLANN_REQUIRED_TYPE}) AND NOT (${PCL_FLANN_REQUIRED_TYPE} MATCHES "DONTCARE"))
+  message(FATAL_ERROR "Flann was selected with ${PCL_FLANN_REQUIRED_TYPE} but found as ${FLANN_LIBRARY_TYPE}")
 endif()
-find_package(FLANN 1.7.0 REQUIRED)
 
-# libusb-1.0
+# libusb
 option(WITH_LIBUSB "Build USB RGBD-Camera drivers" TRUE)
 if(WITH_LIBUSB)
-  find_package(libusb-1.0)
-  if(LIBUSB_1_FOUND)
-    include_directories(SYSTEM "${LIBUSB_1_INCLUDE_DIR}")
-  endif()
+  find_package(libusb)
 endif()
 
 # Dependencies for different grabbers
@@ -363,89 +367,44 @@ if(WITH_CUDA)
   include("${PCL_SOURCE_DIR}/cmake/pcl_find_cuda.cmake")
 endif()
 
-option(WITH_QT "Build QT Front-End" TRUE)
-if(WITH_QT)
-  find_package(Qt5 COMPONENTS Concurrent OpenGL Widgets QUIET)
-endif()
 
-# Find VTK
+# Reset VTK_FOUND to off
+set(VTK_FOUND OFF)
+# Find VTK - VTK has to be found before Qt, otherwise it can overwrite Qt variables
 option(WITH_VTK "Build VTK-Visualizations" TRUE)
-if(WITH_VTK AND NOT ANDROID)
-  set(PCL_VTK_COMPONENTS
-    vtkChartsCore
-    vtkCommonCore
-    vtkCommonDataModel
-    vtkCommonExecutionModel
-    vtkFiltersCore
-    vtkFiltersExtraction
-    vtkFiltersModeling
-    vtkImagingCore
-    vtkImagingSources
-    vtkInteractionStyle
-    vtkInteractionWidgets
-    vtkIOCore
-    vtkIOGeometry
-    vtkIOImage
-    vtkIOLegacy
-    vtkIOPLY
-    vtkRenderingAnnotation
-    vtkRenderingLOD
-    vtkViewsContext2D
-  )
-  find_package(VTK COMPONENTS ${PCL_VTK_COMPONENTS})
-  if(VTK_FOUND AND ("${VTK_VERSION}" VERSION_LESS 6.2))
-    message(WARNING "The minimum required version of VTK is 6.2, but found ${VTK_VERSION}")
-    set(VTK_FOUND FALSE)
+if(WITH_VTK)
+  if(ANDROID)
+    message(WARNING "VTK is not supported on Android.")
+  else()
+    include("${PCL_SOURCE_DIR}/cmake/pcl_find_vtk.cmake")
   endif()
+endif()
 
-  if(VTK_FOUND)
-    if(NOT DEFINED VTK_RENDERING_BACKEND)
-      # On old VTK versions this variable does not exist. In this case it is
-      # safe to assume OpenGL backend
-      set(VTK_RENDERING_BACKEND "OpenGL")
-    endif()
-    list(APPEND PCL_VTK_COMPONENTS vtkRenderingContext${VTK_RENDERING_BACKEND})
-
-    if(WITH_QT)
-      if(";${VTK_MODULES_ENABLED};" MATCHES ";vtkGUISupportQt;" AND ";${VTK_MODULES_ENABLED};" MATCHES ";vtkRenderingQt;")
-        set(QVTK_FOUND ON)
-        list(APPEND PCL_VTK_COMPONENTS vtkRenderingQt vtkGUISupportQt)
-      else()
-        unset(QVTK_FOUND)
-      endif()
+#VTK can depend on Qt and search for its required version, so search after so we can overwrite Qt5_FOUND if the version we require is not found.
+option(WITH_QT "Build QT Front-End" TRUE)
+if(WITH_QT)
+  find_package(Qt5 5.9.5 COMPONENTS Concurrent OpenGL Widgets)
+  set(QT_DISABLE_PRECATED_BEFORE_VAL "0x050900")
+  
+  if(Qt5_FOUND)
+    message(STATUS "Qt5 version: ${Qt5_VERSION}")
+    set(QT5_FOUND ${Qt5_FOUND})
+    #Set Cmake Auto features to skip .hh files
+    if(POLICY CMP0100)
+      cmake_policy(SET CMP0100 OLD)
     endif()
+    
+    get_property(core_def TARGET Qt5::Core PROPERTY INTERFACE_COMPILE_DEFINITIONS)
+    list(APPEND core_def "QT_DISABLE_DEPRECATED_BEFORE=${QT_DISABLE_PRECATED_BEFORE_VAL}")
+    set_property(TARGET Qt5::Core PROPERTY INTERFACE_COMPILE_DEFINITIONS ${core_def})
 
-    find_package(VTK COMPONENTS ${PCL_VTK_COMPONENTS})
-
-    message(STATUS "VTK_MAJOR_VERSION ${VTK_MAJOR_VERSION}, rendering backend: ${VTK_RENDERING_BACKEND}")
-    if(PCL_SHARED_LIBS OR (NOT (PCL_SHARED_LIBS) AND NOT (VTK_BUILD_SHARED_LIBS)))
-      if(VTK_USE_FILE)
-        include(${VTK_USE_FILE})
-      endif()
-      message(STATUS "VTK found (include: ${VTK_INCLUDE_DIRS}, libs: ${VTK_LIBRARIES}")
-      if(APPLE)
-        option(VTK_USE_COCOA "Use Cocoa for VTK render windows" ON)
-        mark_as_advanced(VTK_USE_COCOA)
-      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()
-    else()
-      set(VTK_FOUND OFF)
-      message("Warning: You are to build PCL in STATIC but VTK is SHARED!")
-      message("Warning: VTK disabled!")
-    endif()
+  else()
+    message(STATUS "Qt5 is not found.")
   endif()
 else()
-  set(VTK_FOUND OFF)
+  set(Qt5_FOUND FALSE)
 endif()
 
-
 #Find PCAP
 option(WITH_PCAP "pcap file capabilities in Velodyne HDL driver" TRUE)
 if(WITH_PCAP)
diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md
index 16e64b7a..c940ed3b 100644
--- a/CONTRIBUTING.md
+++ b/CONTRIBUTING.md
@@ -24,7 +24,7 @@ restrictions:
   * [Discord Server](https://discord.gg/JFFMAXS) for live chat with
   other members of the PCL community and casual discussions
 
-<!-- 
+<!--
   * Mailing list: The [PCL Google Group](https://groups.google.com/forum/#!forum/point-cloud-library)
 -->
 
@@ -42,7 +42,7 @@ commits.
 **Please ask first** before embarking on any significant pull request (e.g.
 implementing features, refactoring code), otherwise you risk spending a lot of
 time working on something that the project's developers might not want to merge
-into the project. Please read the [tutorial on writing a new PCL class](http://pointclouds.org/documentation/tutorials/writing_new_classes.php#writing-new-classes) if you want to contribute a
+into the project. Please read the [tutorial on writing a new PCL class](https://pcl.readthedocs.io/projects/tutorials/en/latest/writing_new_classes.html) if you want to contribute a
 brand new feature.
 
 If you are new to Git, GitHub, or contributing to an open-source project, you
@@ -55,7 +55,7 @@ may want to consult the [step-by-step guide on preparing and submitting a pull r
 Please use the following checklist to make sure that your contribution is well
 prepared for merging into PCL:
 
-1. Source code adheres to the coding conventions described in [PCL Style Guide](http://pointclouds.org/documentation/advanced/pcl_style_guide.php).
+1. Source code adheres to the coding conventions described in [PCL Style Guide](http://pcl.readthedocs.io/projects/advanced/en/latest/pcl_style_guide.html).
    But if you modify existing code, do not change/fix style in the lines that
    are not related to your contribution.
 
@@ -114,40 +114,12 @@ of each `.h` and `.cpp` file:
 
 ```cpp
 /*
- * Software License Agreement (BSD License)
+ * SPDX-License-Identifier: BSD-3-Clause
  *
  *  Point Cloud Library (PCL) - www.pointclouds.org
- *  Copyright (c) 2014-, 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.
+ *  Copyright (c) 2014-, Open Perception Inc.
  *
+ *  All rights reserved
  */
 ```
 
diff --git a/PCLConfig.cmake.in b/PCLConfig.cmake.in
index 349ad7c4..a1283a81 100644
--- a/PCLConfig.cmake.in
+++ b/PCLConfig.cmake.in
@@ -101,20 +101,19 @@ macro(find_boost)
   endif()
   set(Boost_ADDITIONAL_VERSIONS
     "@Boost_MAJOR_VERSION@.@Boost_MINOR_VERSION@.@Boost_SUBMINOR_VERSION@" "@Boost_MAJOR_VERSION@.@Boost_MINOR_VERSION@"
-    "1.73.0" "1.73" "1.72.0" "1.72" "1.71.0" "1.71" "1.70.0" "1.70"
-    "1.69.0" "1.69" "1.68.0" "1.68" "1.67.0" "1.67" "1.66.0" "1.66" "1.65.1" "1.65.0" "1.65"
-    "1.64.0" "1.64" "1.63.0" "1.63" "1.62.0" "1.62" "1.61.0" "1.61" "1.60.0" "1.60"
-    "1.59.0" "1.59" "1.58.0" "1.58" "1.57.0" "1.57" "1.56.0" "1.56" "1.55.0" "1.55")
+    "1.76.0" "1.76" "1.75.0" "1.75" 
+    "1.74.0" "1.74" "1.73.0" "1.73" "1.72.0" "1.72" "1.71.0" "1.71" "1.70.0" "1.70"
+    "1.69.0" "1.69" "1.68.0" "1.68" "1.67.0" "1.67" "1.66.0" "1.66" "1.65.1" "1.65.0" "1.65")
   # Disable the config mode of find_package(Boost)
   set(Boost_NO_BOOST_CMAKE ON)
-  find_package(Boost 1.55.0 ${QUIET_} COMPONENTS @PCLCONFIG_AVAILABLE_BOOST_MODULES@)
+  find_package(Boost 1.65.0 ${QUIET_} COMPONENTS @PCLCONFIG_AVAILABLE_BOOST_MODULES@)
 
   set(BOOST_FOUND ${Boost_FOUND})
   set(BOOST_INCLUDE_DIRS "${Boost_INCLUDE_DIR}")
   set(BOOST_LIBRARY_DIRS "${Boost_LIBRARY_DIRS}")
   set(BOOST_LIBRARIES ${Boost_LIBRARIES})
   if(WIN32 AND NOT MINGW)
-    set(BOOST_DEFINITIONS ${BOOST_DEFINITIONS} -DBOOST_ALL_NO_LIB)
+    string(APPEND BOOST_DEFINITIONS -DBOOST_ALL_NO_LIB)
   endif()
 endmacro()
 
@@ -126,7 +125,6 @@ macro(find_eigen)
     get_filename_component(EIGEN_ROOT "@EIGEN_INCLUDE_DIRS@" ABSOLUTE)
   endif()
   find_package(Eigen 3.1)
-  set(EIGEN_DEFINITIONS ${EIGEN_DEFINITIONS})
 endmacro()
 
 #remove this as soon as qhull is shipped with FindQhull.cmake
@@ -255,17 +253,7 @@ macro(find_VTK)
 endmacro()
 
 macro(find_libusb)
-  if(NOT WIN32)
-    find_path(LIBUSB_1_INCLUDE_DIR
-      NAMES libusb-1.0/libusb.h
-      PATHS /usr/include /usr/local/include /opt/local/include /sw/include
-      PATH_SUFFIXES libusb-1.0)
-
-    find_library(LIBUSB_1_LIBRARY
-      NAMES usb-1.0
-      PATHS /usr/lib /usr/local/lib /opt/local/lib /sw/lib)
-    find_package_handle_standard_args(libusb-1.0 LIBUSB_1_LIBRARY LIBUSB_1_INCLUDE_DIR)
-  endif()
+  find_package(libusb)
 endmacro()
 
 macro(find_glew)
@@ -310,7 +298,7 @@ macro(find_external_library _component _lib _is_optional)
     find_rssdk2()
   elseif("${_lib}" STREQUAL "vtk")
     find_VTK()
-  elseif("${_lib}" STREQUAL "libusb-1.0")
+  elseif("${_lib}" STREQUAL "libusb")
     find_libusb()
   elseif("${_lib}" STREQUAL "glew")
     find_glew()
@@ -396,6 +384,9 @@ file(TO_CMAKE_PATH "${PCL_DIR}" PCL_DIR)
 if(WIN32 AND NOT MINGW)
 # PCLConfig.cmake is installed to PCL_ROOT/cmake
   get_filename_component(PCL_ROOT "${PCL_DIR}" PATH)
+  if(EXISTS "${PCL_ROOT}/3rdParty")
+    set(PCL_ALL_IN_ONE_INSTALLER ON)
+  endif()
 else()
 # PCLConfig.cmake is installed to PCL_ROOT/share/pcl-x.y
   get_filename_component(PCL_ROOT "${CMAKE_CURRENT_LIST_DIR}/../.." ABSOLUTE)
@@ -407,17 +398,11 @@ if(EXISTS "${PCL_ROOT}/include/pcl-${PCL_VERSION_MAJOR}.${PCL_VERSION_MINOR}/pcl
   # pcl_message("Found a PCL installation")
   set(PCL_CONF_INCLUDE_DIR "${PCL_ROOT}/include/pcl-${PCL_VERSION_MAJOR}.${PCL_VERSION_MINOR}")
   set(PCL_LIBRARY_DIRS "${PCL_ROOT}/@LIB_INSTALL_DIR@")
-  if(EXISTS "${PCL_ROOT}/3rdParty")
-    set(PCL_ALL_IN_ONE_INSTALLER ON)
-  endif()
 elseif(EXISTS "${PCL_ROOT}/include/pcl/pcl_config.h")
   # Found a non-standard (likely ANDROID) PCL installation
   # pcl_message("Found a PCL installation")
   set(PCL_CONF_INCLUDE_DIR "${PCL_ROOT}/include")
   set(PCL_LIBRARY_DIRS "${PCL_ROOT}/lib")
-  if(EXISTS "${PCL_ROOT}/3rdParty")
-    set(PCL_ALL_IN_ONE_INSTALLER ON)
-  endif()
 elseif(EXISTS "${PCL_DIR}/include/pcl/pcl_config.h")
   # Found PCLConfig.cmake in a build tree of PCL
   # pcl_message("PCL found into a build tree.")
@@ -529,6 +514,7 @@ foreach(component ${PCL_TO_FIND_COMPONENTS})
     #pcl_message("No include directory found for pcl_${component}.")
   endif()
 
+  set(FPHSA_NAME_MISMATCHED 1) # Suppress warnings, see https://cmake.org/cmake/help/v3.17/module/FindPackageHandleStandardArgs.html
   # Skip find_library for header only modules
   list(FIND pcl_header_only_components ${component} _is_header_only)
   if(_is_header_only EQUAL -1)
@@ -565,6 +551,7 @@ foreach(component ${PCL_TO_FIND_COMPONENTS})
     find_package_handle_standard_args(PCL_${COMPONENT} DEFAULT_MSG
       PCL_${COMPONENT}_INCLUDE_DIR)
   endif()
+  unset(FPHSA_NAME_MISMATCHED)
 
   if(PCL_${COMPONENT}_FOUND)
     if(NOT "${PCL_${COMPONENT}_INCLUDE_DIRS}" STREQUAL "")
diff --git a/README.md b/README.md
index 9ae34aae..b8c83e4d 100644
--- a/README.md
+++ b/README.md
@@ -5,7 +5,7 @@
 [![Release][release-image]][releases]
 [![License][license-image]][license]
 
-[release-image]: https://img.shields.io/badge/release-1.11.1-green.svg?style=flat
+[release-image]: https://img.shields.io/badge/release-1.12.0-green.svg?style=flat
 [releases]: https://github.com/PointCloudLibrary/pcl/releases
 
 [license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat
@@ -21,19 +21,22 @@ If you really need access to the old website, please use [the copy made by the i
 Continuous integration
 ----------------------
 [ci-latest-build]: https://dev.azure.com/PointCloudLibrary/pcl/_build/latest?definitionId=9&branchName=master
-[ci-ubuntu-16.04]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20GCC&jobName=Ubuntu&configuration=Ubuntu%2016.04%20GCC&label=Ubuntu%2016.04%20GCC
-[ci-ubuntu-18.04]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20Clang&jobName=Ubuntu&configuration=Ubuntu%2018.04%20Clang&label=Ubuntu%2018.04%20Clang
-[ci-ubuntu-20.04]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20GCC&jobName=Ubuntu&configuration=Ubuntu%2020.04%20GCC&label=Ubuntu%2020.04%20GCC
-[ci-windows-x86]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20MSVC&jobName=Windows%20VS2017%20Build&configuration=Windows%20VS2017%20Build%20x86&label=Windows%20VS2017%20x86
-[ci-windows-x64]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20MSVC&jobName=Windows%20VS2017%20Build&configuration=Windows%20VS2017%20Build%20x64&label=Windows%20VS2017%20x64
+[ci-ubuntu-18.04]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20GCC&jobName=Ubuntu&configuration=Ubuntu%2018.04%20GCC&label=Ubuntu%2018.04%20GCC
+[ci-ubuntu-20.04]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20Clang&jobName=Ubuntu&configuration=Ubuntu%2020.04%20Clang&label=Ubuntu%2020.04%20Clang
+[ci-ubuntu-20.10]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20GCC&jobName=Ubuntu&configuration=Ubuntu%2020.10%20GCC&label=Ubuntu%2020.10%20GCC
+[ci-windows-x86]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20MSVC&jobName=Windows%20Build&configuration=Windows%20Build%20x86&label=Windows%20VS2019%20x86
+[ci-windows-x64]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20MSVC&jobName=Windows%20Build&configuration=Windows%20Build%20x64&label=Windows%20VS2019%20x64
 [ci-macos-10.14]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20Clang&jobName=macOS&configuration=macOS%20Mojave%2010.14&label=macOS%20Mojave%2010.14
 [ci-macos-10.15]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20Clang&jobName=macOS&configuration=macOS%20Catalina%2010.15&label=macOS%20Catalina%2010.15
+[ci-docs]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/Documentation?branchName=master
+[ci-latest-docs]: https://dev.azure.com/PointCloudLibrary/pcl/_build/latest?definitionId=14&branchName=master
 
 Build Platform           | Status
 ------------------------ | ------------------------------------------------------------------------------------------------- |
-Ubuntu                   | [![Status][ci-ubuntu-16.04]][ci-latest-build] <br> [![Status][ci-ubuntu-18.04]][ci-latest-build]                              <br> [![Status][ci-ubuntu-20.04]][ci-latest-build]                                                |
+Ubuntu                   | [![Status][ci-ubuntu-18.04]][ci-latest-build] <br> [![Status][ci-ubuntu-20.04]][ci-latest-build]                              <br> [![Status][ci-ubuntu-20.10]][ci-latest-build]                                                |
 Windows                  | [![Status][ci-windows-x86]][ci-latest-build]  <br> [![Status][ci-windows-x64]][ci-latest-build]   |
 macOS                    | [![Status][ci-macos-10.14]][ci-latest-build]  <br> [![Status][ci-macos-10.15]][ci-latest-build]   |
+Documentation            | [![Status][ci-docs]][ci-latest-docs] |
 
 Community
 ---------
@@ -97,3 +100,18 @@ for Q&A as well as support for troubleshooting, installation and debugging. Do
 remember to tag your questions with the tag `point-cloud-library`.
 * [Discord Server](https://discord.gg/JFFMAXS) for live chat with
 other members of the PCL community and casual discussions
+
+Citation
+--------
+We encourage other researchers to cite PCL if they use PCL or its components for their work or baselines. The bibtex entry for the same is
+```
+@InProceedings{Rusu_ICRA2011_PCL,
+  author    = {Radu Bogdan Rusu and Steve Cousins},
+  title     = {{3D is here: Point Cloud Library (PCL)}},
+  booktitle = {{IEEE International Conference on Robotics and Automation (ICRA)}},
+  month     = {May 9-13},
+  year      = {2011},
+  address   = {Shanghai, China},
+  publisher = {IEEE}
+}
+```
diff --git a/apps/3d_rec_framework/CMakeLists.txt b/apps/3d_rec_framework/CMakeLists.txt
index ec6c56d8..219c4def 100644
--- a/apps/3d_rec_framework/CMakeLists.txt
+++ b/apps/3d_rec_framework/CMakeLists.txt
@@ -1,25 +1,7 @@
 set(SUBSUBSYS_NAME 3d_rec_framework)
 set(SUBSUBSYS_DESC "3D recognition framework")
 set(SUBSUBSYS_DEPS common geometry io filters sample_consensus segmentation visualization kdtree features surface octree registration keypoints tracking search recognition ml)
-
-# Find VTK
-if(NOT VTK_FOUND)
-  set(DEFAULT AUTO_OFF)
-  set(REASON "VTK was not found.")
-else()
-  set(DEFAULT TRUE)
-  set(REASON)
-  include("${VTK_USE_FILE}")
-endif()
-
-# OpenNI found?
-if(NOT WITH_OPENNI)
-  set(DEFAULT AUTO_OFF)
-  set(REASON "OpenNI was not found or was disabled by the user.")
-elseif(NOT ${DEFAULT} STREQUAL "AUTO_OFF")
-  set(DEFAULT TRUE)
-  set(REASON)
-endif()
+set(SUBSUBSYS_EXT_DEPS vtk openni)
 
 # Default to not building for now
 if(${DEFAULT} STREQUAL "TRUE")
@@ -27,7 +9,7 @@ if(${DEFAULT} STREQUAL "TRUE")
 endif()
 
 PCL_SUBSUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" "${SUBSUBSYS_DESC}" ${DEFAULT} "${REASON}")
-PCL_SUBSUBSYS_DEPEND(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" DEPS ${SUBSUBSYS_DEPS} EXT_DEPS vtk openni)
+PCL_SUBSUBSYS_DEPEND(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" DEPS ${SUBSUBSYS_DEPS} EXT_DEPS ${SUBSUBSYS_EXT_DEPS})
 
 if(NOT build)
   return()
@@ -112,10 +94,6 @@ target_link_libraries("${LIB_NAME}" pcl_apps pcl_common pcl_io pcl_filters pcl_v
 
 if(WITH_OPENNI)
   target_link_libraries("${LIB_NAME}" ${OPENNI_LIBRARIES})
-  if(NOT WIN32)
-    find_package(libusb-1.0 REQUIRED)
-    target_link_libraries("${LIB_NAME}" ${LIBUSB_1_LIBRARIES})
-  endif()
 endif()
 
 PCL_MAKE_PKGCONFIG(${LIB_NAME} COMPONENT ${SUBSUBSYS_NAME} DESC ${SUBSUBSYS_DESC})
diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/cvfh_estimator.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/cvfh_estimator.h
index 97583935..471f647f 100644
--- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/cvfh_estimator.h
+++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/cvfh_estimator.h
@@ -8,7 +8,6 @@
 #pragma once
 
 #include <pcl/apps/3d_rec_framework/feature_wrapper/global/global_estimator.h>
-#include <pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h>
 #include <pcl/features/cvfh.h>
 #include <pcl/surface/mls.h>
 
@@ -133,7 +132,7 @@ public:
 
     for (std::size_t i = 0; i < cvfh_signatures.size(); i++) {
       pcl::PointCloud<FeatureT> vfh_signature;
-      vfh_signature.points.resize(1);
+      vfh_signature.resize(1);
       vfh_signature.width = vfh_signature.height = 1;
       for (int d = 0; d < 308; ++d)
         vfh_signature[0].histogram[d] = cvfh_signatures[i].histogram[d];
diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/ourcvfh_estimator.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/ourcvfh_estimator.h
index 85e5c264..621702ae 100644
--- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/ourcvfh_estimator.h
+++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/ourcvfh_estimator.h
@@ -162,7 +162,7 @@ public:
 
     for (const auto& point : cvfh_signatures.points) {
       pcl::PointCloud<FeatureT> vfh_signature;
-      vfh_signature.points.resize(1);
+      vfh_signature.resize(1);
       vfh_signature.width = vfh_signature.height = 1;
       for (int d = 0; d < 308; ++d)
         vfh_signature[0].histogram[d] = point.histogram[d];
diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/fpfh_local_estimator.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/fpfh_local_estimator.h
index c4106401..efcda555 100644
--- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/fpfh_local_estimator.h
+++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/fpfh_local_estimator.h
@@ -8,7 +8,6 @@
 #pragma once
 
 #include <pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h>
-#include <pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h>
 #include <pcl/features/fpfh.h>
 
 namespace pcl {
diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h
index d78c4eec..c37229b9 100644
--- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h
+++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h
@@ -77,7 +77,7 @@ private:
   using KeypointExtractor<PointInT>::input_;
   using KeypointExtractor<PointInT>::radius_;
   float sampling_density_;
-  std::shared_ptr<std::vector<std::vector<int>>> neighborhood_indices_;
+  std::shared_ptr<std::vector<pcl::Indices>> neighborhood_indices_;
   std::shared_ptr<std::vector<std::vector<float>>> neighborhood_dist_;
 
   void
@@ -92,12 +92,12 @@ private:
       tree.reset(new pcl::search::KdTree<PointInT>(false));
     tree->setInputCloud(input);
 
-    neighborhood_indices_.reset(new std::vector<std::vector<int>>);
+    neighborhood_indices_.reset(new std::vector<pcl::Indices>);
     neighborhood_indices_->resize(keypoints_cloud->size());
     neighborhood_dist_.reset(new std::vector<std::vector<float>>);
     neighborhood_dist_->resize(keypoints_cloud->size());
 
-    filtered_keypoints.points.resize(keypoints_cloud->size());
+    filtered_keypoints.resize(keypoints_cloud->size());
     int good = 0;
 
     for (std::size_t i = 0; i < keypoints_cloud->size(); i++) {
diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/shot_local_estimator.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/shot_local_estimator.h
index 9fef4505..31664672 100644
--- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/shot_local_estimator.h
+++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/shot_local_estimator.h
@@ -10,7 +10,6 @@
 #include <pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h>
 #include <pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h>
 #include <pcl/features/shot.h>
-#include <pcl/io/pcd_io.h>
 
 namespace pcl {
 namespace rec_3d_framework {
diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/shot_local_estimator_omp.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/shot_local_estimator_omp.h
index 3b86d64a..51fbf34b 100644
--- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/shot_local_estimator_omp.h
+++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/shot_local_estimator_omp.h
@@ -10,7 +10,6 @@
 #include <pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h>
 #include <pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h>
 #include <pcl/features/shot_omp.h>
-#include <pcl/io/pcd_io.h>
 
 namespace pcl {
 namespace rec_3d_framework {
diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h
index b1a7f539..a8f49baa 100644
--- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h
+++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h
@@ -30,9 +30,8 @@ class PreProcessorAndNormalEstimator {
     KdTreeInPtr tree = pcl::make_shared<pcl::KdTreeFLANN<PointInT>>(false);
     tree->setInputCloud(input);
 
-    std::vector<int> nn_indices(9);
+    pcl::Indices nn_indices(9);
     std::vector<float> nn_distances(9);
-    std::vector<int> src_indices;
 
     float sum_distances = 0.0;
     std::vector<float> avg_distances(input->size());
@@ -198,7 +197,7 @@ public:
         }
 
         if (j != static_cast<pcl::index_t>(out->size())) {
-          PCL_ERROR("Contain nans...");
+          PCL_ERROR("Contain nans...\n");
         }
 
         out->points.resize(j);
diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pc_source/mesh_source.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pc_source/mesh_source.h
index 8c3238dd..6e171361 100644
--- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pc_source/mesh_source.h
+++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pc_source/mesh_source.h
@@ -10,7 +10,6 @@
 #include <pcl/apps/3d_rec_framework/pc_source/source.h>
 #include <pcl/apps/3d_rec_framework/utils/vtk_model_sampling.h>
 #include <pcl/apps/render_views_tesselated_sphere.h>
-#include <pcl/io/io.h>
 #include <pcl/io/pcd_io.h>
 
 #include <vtkTransformPolyDataFilter.h>
@@ -81,9 +80,8 @@ public:
   void
   loadOrGenerate(std::string& dir, std::string& model_path, ModelT& model)
   {
-    std::stringstream pathmodel;
-    pathmodel << dir << "/" << model.class_ << "/" << model.id_;
-    bf::path trained_dir = pathmodel.str();
+    const std::string pathmodel = dir + '/' + model.class_ + '/' + model.id_;
+    bf::path trained_dir = pathmodel;
 
     model.views_.reset(new std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>);
     model.poses_.reset(
@@ -95,7 +93,6 @@ public:
     if (bf::exists(trained_dir)) {
       // load views, poses and self-occlusions
       std::vector<std::string> view_filenames;
-      int number_of_views = 0;
       for (const auto& dir_entry : bf::directory_iterator(trained_dir)) {
         // check if its a directory, then get models in it
         if (!(bf::is_directory(dir_entry))) {
@@ -112,17 +109,14 @@ public:
 
           if (extension == "pcd" && strs_[0] == "view") {
             view_filenames.push_back((dir_entry.path().filename()).string());
-
-            number_of_views++;
           }
         }
       }
 
       for (const auto& view_filename : view_filenames) {
-        std::stringstream view_file;
-        view_file << pathmodel.str() << "/" << view_filename;
+        const std::string view_file = pathmodel + '/' + view_filename;
         typename pcl::PointCloud<PointInT>::Ptr cloud(new pcl::PointCloud<PointInT>());
-        pcl::io::loadPCDFile(view_file.str(), *cloud);
+        pcl::io::loadPCDFile(view_file, *cloud);
 
         model.views_->push_back(cloud);
 
@@ -135,19 +129,17 @@ public:
         boost::replace_all(file_replaced2, ".pcd", ".txt");
 
         // read pose as well
-        std::stringstream pose_file;
-        pose_file << pathmodel.str() << "/" << file_replaced1;
+        const std::string pose_file = pathmodel + '/' + file_replaced1;
 
         Eigen::Matrix4f pose;
-        PersistenceUtils::readMatrixFromFile(pose_file.str(), pose);
+        PersistenceUtils::readMatrixFromFile(pose_file, pose);
 
         model.poses_->push_back(pose);
 
         // read entropy as well
-        std::stringstream entropy_file;
-        entropy_file << pathmodel.str() << "/" << file_replaced2;
+        const std::string entropy_file = pathmodel + '/' + file_replaced2;
         float entropy = 0;
-        PersistenceUtils::readFloatFromFile(entropy_file.str(), entropy);
+        PersistenceUtils::readFloatFromFile(entropy_file, entropy);
         model.self_occlusions_->push_back(entropy);
       }
     }
@@ -201,26 +193,23 @@ public:
         model.self_occlusions_->push_back(entropies[i]);
       }
 
-      std::stringstream direc;
-      direc << dir << "/" << model.class_ << "/" << model.id_;
+      const std::string direc = dir + '/' + model.class_ + '/' + model.id_;
       this->createClassAndModelDirectories(dir, model.class_, model.id_);
 
       for (std::size_t i = 0; i < model.views_->size(); i++) {
         // save generated model for future use
-        std::stringstream path_view;
-        path_view << direc.str() << "/view_" << i << ".pcd";
-        pcl::io::savePCDFileBinary(path_view.str(), *(model.views_->at(i)));
+        const std::string path_view = direc + "/view_" + std::to_string(i) + ".pcd";
+        pcl::io::savePCDFileBinary(path_view, *(model.views_->at(i)));
 
-        std::stringstream path_pose;
-        path_pose << direc.str() << "/pose_" << i << ".txt";
+        const std::string path_pose = direc + "/pose_" + std::to_string(i) + ".txt";
 
-        pcl::rec_3d_framework::PersistenceUtils::writeMatrixToFile(path_pose.str(),
+        pcl::rec_3d_framework::PersistenceUtils::writeMatrixToFile(path_pose,
                                                                    model.poses_->at(i));
 
-        std::stringstream path_entropy;
-        path_entropy << direc.str() << "/entropy_" << i << ".txt";
+        const std::string path_entropy =
+            direc + "/entropy_" + std::to_string(i) + ".txt";
         pcl::rec_3d_framework::PersistenceUtils::writeFloatToFile(
-            path_entropy.str(), model.self_occlusions_->at(i));
+            path_entropy, model.self_occlusions_->at(i));
       }
 
       loadOrGenerate(dir, model_path, model);
diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pc_source/registered_views_source.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pc_source/registered_views_source.h
index 06319ec4..1699f85d 100644
--- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pc_source/registered_views_source.h
+++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pc_source/registered_views_source.h
@@ -8,7 +8,7 @@
 #pragma once
 
 #include <pcl/apps/3d_rec_framework/pc_source/source.h>
-#include <pcl/io/io.h>
+#include <pcl/common/io.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/visualization/pcl_visualizer.h>
 
@@ -100,9 +100,8 @@ public:
   void
   loadOrGenerate(std::string& dir, std::string& model_path, ModelT& model)
   {
-    std::stringstream pathmodel;
-    pathmodel << dir << "/" << model.class_ << "/" << model.id_;
-    bf::path trained_dir = pathmodel.str();
+    const std::string pathmodel = dir + '/' + model.class_ + '/' + model.id_;
+    const bf::path trained_dir = pathmodel;
 
     model.views_.reset(new std::vector<typename pcl::PointCloud<PointInT>::Ptr>);
     model.poses_.reset(
@@ -112,7 +111,6 @@ public:
     if (bf::exists(trained_dir)) {
       // load views and poses
       std::vector<std::string> view_filenames;
-      int number_of_views = 0;
       for (const auto& dir_entry : bf::directory_iterator(trained_dir)) {
         // check if its a directory, then get models in it
         if (!(bf::is_directory(*itr))) {
@@ -129,7 +127,6 @@ public:
 
           if (extension == "pcd" && strs_[0] == "view") {
             view_filenames.push_back((dir_entry.path().filename()).string());
-            number_of_views++;
           }
         }
       }
@@ -138,10 +135,9 @@ public:
           poses_to_assemble_;
 
       for (std::size_t i = 0; i < view_filenames.size(); i++) {
-        std::stringstream view_file;
-        view_file << pathmodel.str() << "/" << view_filenames[i];
+        const std::string view_file = pathmodel + '/' + view_filenames[i];
         typename pcl::PointCloud<PointInT>::Ptr cloud(new pcl::PointCloud<PointInT>());
-        pcl::io::loadPCDFile(view_file.str(), *cloud);
+        pcl::io::loadPCDFile(view_file, *cloud);
 
         model.views_->push_back(cloud);
 
@@ -150,10 +146,9 @@ public:
         boost::replace_all(file_replaced1, ".pcd", ".txt");
 
         // read pose as well
-        std::stringstream pose_file;
-        pose_file << pathmodel.str() << "/" << file_replaced1;
+        const std::string pose_file = pathmodel + '/' + file_replaced1;
         Eigen::Matrix4f pose;
-        PersistenceUtils::readMatrixFromFile(pose_file.str(), pose);
+        PersistenceUtils::readMatrixFromFile(pose_file, pose);
 
         if (pose_files_order_ != 0) {
           Eigen::Matrix4f pose_trans = pose.transpose();
@@ -175,8 +170,7 @@ public:
     else {
 
       // we just need to copy the views to the training directory
-      std::stringstream direc;
-      direc << dir << "/" << model.class_ << "/" << model.id_;
+      const std::string direc = dir + '/' + model.class_ + '/' << model.id_;
       createClassAndModelDirectories(dir, model.class_, model.id_);
 
       std::vector<std::string> view_filenames;
@@ -186,23 +180,20 @@ public:
       std::cout << view_filenames.size() << std::endl;
 
       for (std::size_t i = 0; i < view_filenames.size(); i++) {
-        std::stringstream view_file;
-        view_file << model_path << "/" << view_filenames[i];
+        const std::string view_file = model_path + '/' + view_filenames[i];
         typename pcl::PointCloud<PointInT>::Ptr cloud(new pcl::PointCloud<PointInT>());
-        pcl::io::loadPCDFile(view_file.str(), *cloud);
+        pcl::io::loadPCDFile(view_file, *cloud);
 
-        std::cout << view_file.str() << std::endl;
+        std::cout << view_file << std::endl;
 
-        std::stringstream path_view;
-        path_view << direc.str() << "/view_" << i << ".pcd";
-        pcl::io::savePCDFileBinary(path_view.str(), *cloud);
+        const std::string path_view = direc + "/view_" + std::to_string(i) + ".pcd";
+        pcl::io::savePCDFileBinary(path_view, *cloud);
 
-        std::string file_replaced1(view_file.str());
-        boost::replace_all(file_replaced1, view_prefix_, "pose");
-        boost::replace_all(file_replaced1, ".pcd", ".txt");
+        boost::replace_all(view_file, view_prefix_, "pose");
+        boost::replace_all(view_file, ".pcd", ".txt");
 
         Eigen::Matrix4f pose;
-        PersistenceUtils::readMatrixFromFile(file_replaced1, pose);
+        PersistenceUtils::readMatrixFromFile(view_file, pose);
 
         std::cout << pose << std::endl;
 
@@ -213,10 +204,8 @@ public:
           std::cout << pose << std::endl;
         }
 
-        std::stringstream path_pose;
-        path_pose << direc.str() << "/pose_" << i << ".txt";
-        pcl::rec_3d_framework::PersistenceUtils::writeMatrixToFile(path_pose.str(),
-                                                                   pose);
+        const std::string path_pose = direc + "/pose_" + std::to_string(i) + ".txt";
+        pcl::rec_3d_framework::PersistenceUtils::writeMatrixToFile(path_pose, pose);
       }
 
       loadOrGenerate(dir, model_path, model);
@@ -245,7 +234,7 @@ public:
       // check if its a directory, then get models in it
       if (bf::is_directory(dir_entry)) {
         std::string so_far =
-            rel_path_so_far + (dir_entry.path().filename()).string() + "/";
+            rel_path_so_far + (dir_entry.path().filename()).string() + '/';
         bf::path curr_path = dir_entry.path();
 
         if (isleafDirectory(curr_path)) {
@@ -289,14 +278,7 @@ public:
         m.id_ = strs[0];
       }
       else {
-        std::stringstream ss;
-        for (int i = 0; i < (static_cast<int>(strs.size()) - 1); i++) {
-          ss << strs[i];
-          if (i != (static_cast<int>(strs.size()) - 1))
-            ss << "/";
-        }
-
-        m.class_ = ss.str();
+        m.class_ = boost::algorithm::join(strs, '/');
         m.id_ = strs[strs.size() - 1];
       }
 
@@ -305,10 +287,8 @@ public:
       // load views, poses and self-occlusions for those that exist
       // generate otherwise
 
-      std::stringstream model_path;
-      model_path << path_ << "/" << files[i];
-      std::string path_model = model_path.str();
-      loadOrGenerate(training_dir, path_model, m);
+      const std::string model_path = path_ + '/' + files[i];
+      loadOrGenerate(training_dir, model_path, m);
 
       models_->push_back(m);
     }
diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pc_source/source.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pc_source/source.h
index a3f45ee4..91b4cd9b 100644
--- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pc_source/source.h
+++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pc_source/source.h
@@ -8,9 +8,7 @@
 #pragma once
 
 #include <pcl/apps/3d_rec_framework/utils/persistence_utils.h>
-#include <pcl/common/transforms.h>
 #include <pcl/filters/voxel_grid.h>
-#include <pcl/io/pcd_io.h>
 
 #include <boost/algorithm/string.hpp>
 #include <boost/filesystem.hpp>
@@ -93,20 +91,9 @@ protected:
                             std::string& id,
                             std::string& classname)
   {
-
-    std::vector<std::string> strs;
-    boost::split(strs, filename, boost::is_any_of("/\\"));
-    std::string name = strs[strs.size() - 1];
-
-    std::stringstream ss;
-    for (int i = 0; i < (static_cast<int>(strs.size()) - 1); i++) {
-      ss << strs[i];
-      if (i != (static_cast<int>(strs.size()) - 1))
-        ss << "/";
-    }
-
-    classname = ss.str();
-    id = name.substr(0, name.length() - 4);
+    const bf::path path = filename;
+    classname = path.parent_path().string() + '/';
+    id = path.stem().string();
   }
 
   void
@@ -122,22 +109,7 @@ protected:
                                  std::string& class_str,
                                  std::string& id_str)
   {
-    std::vector<std::string> strs;
-    boost::split(strs, class_str, boost::is_any_of("/\\"));
-
-    std::stringstream ss;
-    ss << training_dir << "/";
-    for (const auto& str : strs) {
-      ss << str << "/";
-      bf::path trained_dir = ss.str();
-      if (!bf::exists(trained_dir))
-        bf::create_directory(trained_dir);
-    }
-
-    ss << id_str;
-    bf::path trained_dir = ss.str();
-    if (!bf::exists(trained_dir))
-      bf::create_directory(trained_dir);
+    bf::create_directories(training_dir + '/' + class_str + '/' + id_str);
   }
 
 public:
@@ -174,7 +146,7 @@ public:
       // check if its a directory, then get models in it
       if (bf::is_directory(dir_entry)) {
         std::string so_far =
-            rel_path_so_far + (dir_entry.path().filename()).string() + "/";
+            rel_path_so_far + (dir_entry.path().filename()).string() + '/';
 
         bf::path curr_path = dir_entry.path();
         getModelsInDirectory(curr_path, so_far, relative_paths, ext);
@@ -239,19 +211,13 @@ public:
   bool
   modelAlreadyTrained(ModelT m, std::string& base_dir, std::string& descr_name)
   {
-    std::stringstream dir;
-    dir << base_dir << "/" << m.class_ << "/" << m.id_ << "/" << descr_name;
-    bf::path desc_dir = dir.str();
-    std::cout << dir.str() << std::endl;
-    return bf::exists(desc_dir);
+    return bf::exists(getModelDescriptorDir(m, base_dir, descr_name));
   }
 
   std::string
   getModelDescriptorDir(ModelT m, std::string& base_dir, std::string& descr_name)
   {
-    std::stringstream dir;
-    dir << base_dir << "/" << m.class_ << "/" << m.id_ << "/" << descr_name;
-    return dir.str();
+    return base_dir + '/' + m.class_ + '/' + m.id_ + '/' + descr_name;
   }
 
   void
diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_classifier.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_classifier.h
index 981cc5d6..d6554df8 100644
--- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_classifier.h
+++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_classifier.h
@@ -9,7 +9,6 @@
 
 #include <pcl/apps/3d_rec_framework/feature_wrapper/global/global_estimator.h>
 #include <pcl/apps/3d_rec_framework/pc_source/source.h>
-#include <pcl/common/common.h>
 
 #include <flann/flann.hpp>
 
@@ -34,7 +33,7 @@ public:
   classify() = 0;
 
   virtual void
-  setIndices(std::vector<int>& indices) = 0;
+  setIndices(pcl::Indices& indices) = 0;
 
   virtual void
   setInputCloud(const PointInTPtr& cloud) = 0;
@@ -92,7 +91,7 @@ protected:
   flann::Index<DistT>* flann_index_;
   std::vector<flann_model> flann_models_;
 
-  std::vector<int> indices_;
+  pcl::Indices indices_;
 
   // load features from disk and create flann structure
   void
@@ -186,7 +185,7 @@ public:
   }
 
   void
-  setIndices(std::vector<int>& indices) override
+  setIndices(pcl::Indices& indices) override
   {
     indices_ = indices;
   }
diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_recognizer_crh.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_recognizer_crh.h
index fb597bb1..ff3bde4e 100644
--- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_recognizer_crh.h
+++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_recognizer_crh.h
@@ -95,7 +95,7 @@ protected:
       poses_cache_;
   std::map<std::pair<std::string, int>, Eigen::Vector3f> centroids_cache_;
 
-  std::vector<int> indices_;
+  pcl::Indices indices_;
 
   // load features from disk and create flann structure
   void
@@ -216,7 +216,7 @@ public:
   }
 
   void
-  setIndices(std::vector<int>& indices)
+  setIndices(pcl::Indices& indices)
   {
     indices_ = indices;
   }
diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_recognizer_cvfh.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_recognizer_cvfh.h
index 82e9cc34..54c5076c 100644
--- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_recognizer_cvfh.h
+++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_recognizer_cvfh.h
@@ -130,7 +130,7 @@ protected:
       poses_cache_;
   std::map<std::pair<std::string, int>, Eigen::Vector3f> centroids_cache_;
 
-  std::vector<int> indices_;
+  pcl::Indices indices_;
 
   bool compute_scale_;
 
@@ -295,7 +295,7 @@ public:
   }
 
   void
-  setIndices(std::vector<int>& indices)
+  setIndices(pcl::Indices& indices)
   {
     indices_ = indices;
   }
diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_classifier.hpp b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_classifier.hpp
index ac54b86a..0592be86 100644
--- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_classifier.hpp
+++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_classifier.hpp
@@ -181,30 +181,28 @@ pcl::rec_3d_framework::GlobalNNPipeline<Distance, PointInT, FeatureT>::initializ
         if (!bf::exists(desc_dir))
           bf::create_directory(desc_dir);
 
-        std::stringstream path_view;
-        path_view << path << "/view_" << v << ".pcd";
-        pcl::io::savePCDFileBinary(path_view.str(), *processed);
-
-        std::stringstream path_pose;
-        path_pose << path << "/pose_" << v << ".txt";
-        PersistenceUtils::writeMatrixToFile(path_pose.str(),
-                                            models->at(i).poses_->at(v));
-
-        std::stringstream path_entropy;
-        path_entropy << path << "/entropy_" << v << ".txt";
-        PersistenceUtils::writeFloatToFile(path_entropy.str(),
+        const std::string path_view = path + "/view_" + std::to_string(v) + ".pcd";
+        pcl::io::savePCDFileBinary(path_view, *processed);
+
+        const std::string path_pose = path + "/pose_" + std::to_string(v) + ".txt";
+        PersistenceUtils::writeMatrixToFile(path_pose, models->at(i).poses_->at(v));
+
+        const std::string path_entropy =
+            path + "/entropy_" + std::to_string(v) + ".txt";
+        PersistenceUtils::writeFloatToFile(path_entropy,
                                            models->at(i).self_occlusions_->at(v));
 
         // save signatures and centroids to disk
         for (std::size_t j = 0; j < signatures.size(); j++) {
-          std::stringstream path_centroid;
-          path_centroid << path << "/centroid_" << v << "_" << j << ".txt";
+          const std::string path_centroid = path + "/centroid_" + std::to_string(v) +
+                                            "_" + std::to_string(j) + ".txt";
           Eigen::Vector3f centroid(centroids[j][0], centroids[j][1], centroids[j][2]);
-          PersistenceUtils::writeCentroidToFile(path_centroid.str(), centroid);
+          PersistenceUtils::writeCentroidToFile(path_centroid, centroid);
 
-          std::stringstream path_descriptor;
-          path_descriptor << path << "/descriptor_" << v << "_" << j << ".pcd";
-          pcl::io::savePCDFileBinary(path_descriptor.str(), signatures[j]);
+          const std::string path_descriptor = path + "/descriptor_" +
+                                              std::to_string(v) + "_" +
+                                              std::to_string(j) + ".pcd";
+          pcl::io::savePCDFileBinary(path_descriptor, signatures[j]);
         }
       }
     }
diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_recognizer_crh.hpp b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_recognizer_crh.hpp
index 5d5052a0..d6ca7cd4 100644
--- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_recognizer_crh.hpp
+++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_recognizer_crh.hpp
@@ -34,11 +34,11 @@ pcl::rec_3d_framework::GlobalNNCRHRecognizer<Distance, PointInT, FeatureT>::getP
     }
   }
 
-  std::stringstream dir;
-  std::string path = source_->getModelDescriptorDir(model, training_dir_, descr_name_);
-  dir << path << "/pose_" << view_id << ".txt";
+  const std::string path =
+      source_->getModelDescriptorDir(model, training_dir_, descr_name_);
+  const std::string dir = path + "/pose_" + std::to_string(view_id) + ".txt";
 
-  PersistenceUtils::readMatrixFromFile(dir.str(), pose_matrix);
+  PersistenceUtils::readMatrixFromFile(dir, pose_matrix);
 }
 
 template <template <class> class Distance, typename PointInT, typename FeatureT>
@@ -48,11 +48,12 @@ pcl::rec_3d_framework::GlobalNNCRHRecognizer<Distance, PointInT, FeatureT>::getC
 {
 
   hist.reset(new CRHPointCloud);
-  std::stringstream dir;
-  std::string path = source_->getModelDescriptorDir(model, training_dir_, descr_name_);
-  dir << path << "/crh_" << view_id << "_" << d_id << ".pcd";
+  const std::string path =
+      source_->getModelDescriptorDir(model, training_dir_, descr_name_);
+  const std::string dir = path + "/centroid_" + std::to_string(view_id) + '_' +
+                          std::to_string(d_id) + ".pcd";
 
-  pcl::io::loadPCDFile(dir.str(), *hist);
+  pcl::io::loadPCDFile(dir, *hist);
 }
 
 template <template <class> class Distance, typename PointInT, typename FeatureT>
@@ -60,11 +61,12 @@ void
 pcl::rec_3d_framework::GlobalNNCRHRecognizer<Distance, PointInT, FeatureT>::getCentroid(
     ModelT& model, int view_id, int d_id, Eigen::Vector3f& centroid)
 {
-  std::stringstream dir;
-  std::string path = source_->getModelDescriptorDir(model, training_dir_, descr_name_);
-  dir << path << "/centroid_" << view_id << "_" << d_id << ".txt";
+  const std::string path =
+      source_->getModelDescriptorDir(model, training_dir_, descr_name_);
+  const std::string dir = path + "/centroid_" + std::to_string(view_id) + '_' +
+                          std::to_string(d_id) + ".txt";
 
-  PersistenceUtils::getCentroidFromFile(dir.str(), centroid);
+  PersistenceUtils::getCentroidFromFile(dir, centroid);
 }
 
 template <template <class> class Distance, typename PointInT, typename FeatureT>
@@ -73,10 +75,10 @@ pcl::rec_3d_framework::GlobalNNCRHRecognizer<Distance, PointInT, FeatureT>::getV
     ModelT& model, int view_id, PointInTPtr& view)
 {
   view.reset(new pcl::PointCloud<PointInT>);
-  std::stringstream dir;
-  std::string path = source_->getModelDescriptorDir(model, training_dir_, descr_name_);
-  dir << path << "/view_" << view_id << ".pcd";
-  pcl::io::loadPCDFile(dir.str(), *view);
+  const std::string path =
+      source_->getModelDescriptorDir(model, training_dir_, descr_name_);
+  const std::string dir = path + "/view_" + std::to_string(view_id) + ".pcd";
+  pcl::io::loadPCDFile(dir, *view);
 }
 
 template <template <class> class Distance, typename PointInT, typename FeatureT>
@@ -122,11 +124,11 @@ pcl::rec_3d_framework::GlobalNNCRHRecognizer<Distance, PointInT, FeatureT>::
 
         if (use_cache_) {
 
-          std::stringstream dir_pose;
-          dir_pose << path << "/pose_" << descr_model.view_id << ".txt";
+          const std::string dir_pose =
+              path + "/pose_" + std::to_string(descr_model.view_id) + ".txt";
 
           Eigen::Matrix4f pose_matrix;
-          PersistenceUtils::readMatrixFromFile(dir_pose.str(), pose_matrix);
+          PersistenceUtils::readMatrixFromFile(dir_pose, pose_matrix);
 
           std::pair<std::string, int> pair_model_view =
               std::make_pair(models->at(i).id_, descr_model.view_id);
@@ -415,18 +417,15 @@ pcl::rec_3d_framework::GlobalNNCRHRecognizer<Distance, PointInT, FeatureT>::init
         if (!bf::exists(desc_dir))
           bf::create_directory(desc_dir);
 
-        std::stringstream path_view;
-        path_view << path << "/view_" << v << ".pcd";
-        pcl::io::savePCDFileBinary(path_view.str(), *processed);
+        const std::string path_view = path + "/view_" + std::to_string(v) + ".pcd";
+        pcl::io::savePCDFileBinary(path_view, *processed);
 
-        std::stringstream path_pose;
-        path_pose << path << "/pose_" << v << ".txt";
-        PersistenceUtils::writeMatrixToFile(path_pose.str(),
-                                            models->at(i).poses_->at(v));
+        const std::string path_pose = path + "/pose_" + std::to_string(v) + ".txt";
+        PersistenceUtils::writeMatrixToFile(path_pose, models->at(i).poses_->at(v));
 
-        std::stringstream path_entropy;
-        path_entropy << path << "/entropy_" << v << ".txt";
-        PersistenceUtils::writeFloatToFile(path_entropy.str(),
+        const std::string path_entropy =
+            path + "/entropy_" + std::to_string(v) + ".txt";
+        PersistenceUtils::writeFloatToFile(path_entropy,
                                            models->at(i).self_occlusions_->at(v));
 
         std::vector<CRHPointCloud::Ptr> crh_histograms;
@@ -434,18 +433,19 @@ pcl::rec_3d_framework::GlobalNNCRHRecognizer<Distance, PointInT, FeatureT>::init
 
         // save signatures and centroids to disk
         for (std::size_t j = 0; j < signatures.size(); j++) {
-          std::stringstream path_centroid;
-          path_centroid << path << "/centroid_" << v << "_" << j << ".txt";
+          const std::string path_centroid = path + "/centroid_" + std::to_string(v) +
+                                            '_' + std::to_string(j) + ".txt";
           Eigen::Vector3f centroid(centroids[j][0], centroids[j][1], centroids[j][2]);
-          PersistenceUtils::writeCentroidToFile(path_centroid.str(), centroid);
+          PersistenceUtils::writeCentroidToFile(path_centroid, centroid);
 
-          std::stringstream path_descriptor;
-          path_descriptor << path << "/descriptor_" << v << "_" << j << ".pcd";
-          pcl::io::savePCDFileBinary(path_descriptor.str(), signatures[j]);
+          const std::string path_descriptor = path + "/descriptor_" +
+                                              std::to_string(v) + '_' +
+                                              std::to_string(j) + ".pcd";
+          pcl::io::savePCDFileBinary(path_descriptor, signatures[j]);
 
-          std::stringstream path_roll;
-          path_roll << path << "/crh_" << v << "_" << j << ".pcd";
-          pcl::io::savePCDFileBinary(path_roll.str(), *crh_histograms[j]);
+          const std::string path_roll =
+              path + "/crh_" + std::to_string(v) + '_' + std::to_string(j) + ".pcd";
+          pcl::io::savePCDFileBinary(path_roll, *crh_histograms[j]);
         }
       }
     }
diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_recognizer_cvfh.hpp b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_recognizer_cvfh.hpp
index e95fbf2a..b891d95e 100644
--- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_recognizer_cvfh.hpp
+++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_recognizer_cvfh.hpp
@@ -34,11 +34,11 @@ pcl::rec_3d_framework::GlobalNNCVFHRecognizer<Distance, PointInT, FeatureT>::get
     }
   }
 
-  std::stringstream dir;
-  std::string path = source_->getModelDescriptorDir(model, training_dir_, descr_name_);
-  dir << path << "/pose_" << view_id << ".txt";
+  const std::string path =
+      source_->getModelDescriptorDir(model, training_dir_, descr_name_);
+  const std::string dir = path + "/pose_" + std::to_string(view_id) + ".txt";
 
-  PersistenceUtils::readMatrixFromFile(dir.str(), pose_matrix);
+  PersistenceUtils::readMatrixFromFile(dir, pose_matrix);
 }
 
 template <template <class> class Distance, typename PointInT, typename FeatureT>
@@ -46,15 +46,14 @@ bool
 pcl::rec_3d_framework::GlobalNNCVFHRecognizer<Distance, PointInT, FeatureT>::
     getRollPose(ModelT& model, int view_id, int d_id, Eigen::Matrix4f& pose_matrix)
 {
+  const std::string path =
+      source_->getModelDescriptorDir(model, training_dir_, descr_name_);
+  const std::string dir = path + "/roll_trans_" + std::to_string(view_id) + '_' +
+                          std::to_string(d_id) + ".txt";
 
-  std::stringstream dir;
-  std::string path = source_->getModelDescriptorDir(model, training_dir_, descr_name_);
-
-  dir << path << "/roll_trans_" << view_id << "_" << d_id << ".txt";
-
-  bf::path file_path = dir.str();
+  const bf::path file_path = dir;
   if (bf::exists(file_path)) {
-    PersistenceUtils::readMatrixFromFile(dir.str(), pose_matrix);
+    PersistenceUtils::readMatrixFromFile(dir, pose_matrix);
     return true;
   }
   return false;
@@ -65,11 +64,12 @@ void
 pcl::rec_3d_framework::GlobalNNCVFHRecognizer<Distance, PointInT, FeatureT>::
     getCentroid(ModelT& model, int view_id, int d_id, Eigen::Vector3f& centroid)
 {
-  std::stringstream dir;
-  std::string path = source_->getModelDescriptorDir(model, training_dir_, descr_name_);
-  dir << path << "/centroid_" << view_id << "_" << d_id << ".txt";
+  const std::string path =
+      source_->getModelDescriptorDir(model, training_dir_, descr_name_);
+  const std::string dir = path + "/centroid_" + std::to_string(view_id) + '_' +
+                          std::to_string(d_id) + ".txt";
 
-  PersistenceUtils::getCentroidFromFile(dir.str(), centroid);
+  PersistenceUtils::getCentroidFromFile(dir, centroid);
 }
 
 template <template <class> class Distance, typename PointInT, typename FeatureT>
@@ -78,10 +78,9 @@ pcl::rec_3d_framework::GlobalNNCVFHRecognizer<Distance, PointInT, FeatureT>::get
     ModelT& model, int view_id, PointInTPtr& view)
 {
   view.reset(new pcl::PointCloud<PointInT>);
-  std::stringstream dir;
   std::string path = source_->getModelDescriptorDir(model, training_dir_, descr_name_);
-  dir << path << "/view_" << view_id << ".pcd";
-  pcl::io::loadPCDFile(dir.str(), *view);
+  std::string dir = path + "/view_" + std::to_string(view_id) + ".pcd";
+  pcl::io::loadPCDFile(dir, *view);
 }
 
 template <template <class> class Distance, typename PointInT, typename FeatureT>
@@ -154,12 +153,11 @@ pcl::rec_3d_framework::GlobalNNCVFHRecognizer<Distance, PointInT, FeatureT>::
         flann_models_.push_back(descr_model);
 
         if (use_cache_) {
-
-          std::stringstream dir_pose;
-          dir_pose << path << "/pose_" << descr_model.view_id << ".txt";
+          const std::string dir_pose =
+              path + "/pose_" + std::to_string(descr_model.view_id) + ".txt";
 
           Eigen::Matrix4f pose_matrix;
-          PersistenceUtils::readMatrixFromFile(dir_pose.str(), pose_matrix);
+          PersistenceUtils::readMatrixFromFile(dir_pose, pose_matrix);
 
           std::pair<std::string, int> pair_model_view =
               std::make_pair(models->at(i).id_, descr_model.view_id);
@@ -617,36 +615,34 @@ pcl::rec_3d_framework::GlobalNNCVFHRecognizer<Distance, PointInT, FeatureT>::ini
         if (!bf::exists(desc_dir))
           bf::create_directory(desc_dir);
 
-        std::stringstream path_view;
-        path_view << path << "/view_" << v << ".pcd";
-        pcl::io::savePCDFileBinary(path_view.str(), *processed);
+        const std::string path_view = path + "/view_" + std::to_string(v) + ".pcd";
+        pcl::io::savePCDFileBinary(path_view, *processed);
 
-        std::stringstream path_pose;
-        path_pose << path << "/pose_" << v << ".txt";
-        PersistenceUtils::writeMatrixToFile(path_pose.str(),
-                                            models->at(i).poses_->at(v));
+        const std::string path_pose = path + "/pose_" + std::to_string(v) + ".txt";
+        PersistenceUtils::writeMatrixToFile(path_pose, models->at(i).poses_->at(v));
 
-        std::stringstream path_entropy;
-        path_entropy << path << "/entropy_" << v << ".txt";
-        PersistenceUtils::writeFloatToFile(path_entropy.str(),
+        const std::string path_entropy =
+            path + "/entropy_" + std::to_string(v) + ".txt";
+        PersistenceUtils::writeFloatToFile(path_entropy,
                                            models->at(i).self_occlusions_->at(v));
 
         // save signatures and centroids to disk
         for (std::size_t j = 0; j < signatures.size(); j++) {
           if (valid_trans[j]) {
-            std::stringstream path_centroid;
-            path_centroid << path << "/centroid_" << v << "_" << j << ".txt";
+            const std::string path_centroid = path + "/centroid_" + std::to_string(v) +
+                                              '_' + std::to_string(j) + ".txt";
             Eigen::Vector3f centroid(centroids[j][0], centroids[j][1], centroids[j][2]);
-            PersistenceUtils::writeCentroidToFile(path_centroid.str(), centroid);
+            PersistenceUtils::writeCentroidToFile(path_centroid, centroid);
 
-            std::stringstream path_descriptor;
-            path_descriptor << path << "/descriptor_" << v << "_" << j << ".pcd";
-            pcl::io::savePCDFileBinary(path_descriptor.str(), signatures[j]);
+            const std::string path_descriptor = path + "/descriptor_" +
+                                                std::to_string(v) + '_' +
+                                                std::to_string(j) + ".pcd";
+            pcl::io::savePCDFileBinary(path_descriptor, signatures[j]);
 
             // save roll transform
-            std::stringstream path_pose;
-            path_pose << path << "/roll_trans_" << v << "_" << j << ".txt";
-            PersistenceUtils::writeMatrixToFile(path_pose.str(), transforms[j]);
+            const std::string path_pose = path + "/roll_trans_" + std::to_string(v) +
+                                          '_' + std::to_string(j) + ".txt";
+            PersistenceUtils::writeMatrixToFile(path_pose, transforms[j]);
           }
         }
       }
diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/local_recognizer.hpp b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/local_recognizer.hpp
index db173fb0..0bd6b9d6 100644
--- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/local_recognizer.hpp
+++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/local_recognizer.hpp
@@ -37,18 +37,17 @@ pcl::rec_3d_framework::LocalRecognitionPipeline<Distance, PointInT, FeatureT>::
         descr_model.view_id = atoi(strs[1].c_str());
 
         if (use_cache_) {
-
-          std::stringstream dir_keypoints;
           std::string path =
               source_->getModelDescriptorDir(models->at(i), training_dir_, descr_name_);
-          dir_keypoints << path << "/keypoint_indices_" << descr_model.view_id
-                        << ".pcd";
+          const std::string dir_keypoints = path + "/keypoint_indices_" +
+                                            std::to_string(descr_model.view_id) +
+                                            ".pcd";
 
-          std::stringstream dir_pose;
-          dir_pose << path << "/pose_" << descr_model.view_id << ".txt";
+          const std::string dir_pose =
+              path + "/pose_" + std::to_string(descr_model.view_id) + ".txt";
 
           Eigen::Matrix4f pose_matrix;
-          PersistenceUtils::readMatrixFromFile(dir_pose.str(), pose_matrix);
+          PersistenceUtils::readMatrixFromFile(dir_pose, pose_matrix);
 
           std::pair<std::string, int> pair_model_view =
               std::make_pair(models->at(i).id_, descr_model.view_id);
@@ -57,7 +56,7 @@ pcl::rec_3d_framework::LocalRecognitionPipeline<Distance, PointInT, FeatureT>::
           // load keypoints and save them to cache
           typename pcl::PointCloud<PointInT>::Ptr keypoints(
               new pcl::PointCloud<PointInT>());
-          pcl::io::loadPCDFile(dir_keypoints.str(), *keypoints);
+          pcl::io::loadPCDFile(dir_keypoints, *keypoints);
           keypoints_cache_[pair_model_view] = keypoints;
         }
 
@@ -155,31 +154,27 @@ pcl::rec_3d_framework::LocalRecognitionPipeline<Distance, PointInT, FeatureT>::
           if (!bf::exists(desc_dir))
             bf::create_directory(desc_dir);
 
-          std::stringstream path_view;
-          path_view << path << "/view_" << v << ".pcd";
-          pcl::io::savePCDFileBinary(path_view.str(), *processed);
+          const std::string path_view = path = "/view_" + std::to_string(v) + ".pcd";
+          pcl::io::savePCDFileBinary(path_view, *processed);
 
-          std::stringstream path_pose;
-          path_pose << path << "/pose_" << v << ".txt";
-          PersistenceUtils::writeMatrixToFile(path_pose.str(),
-                                              models->at(i).poses_->at(v));
+          const std::string path_pose = path + "/pose_" + std::to_string(v) + ".txt";
+          PersistenceUtils::writeMatrixToFile(path_pose, models->at(i).poses_->at(v));
 
           if (v < models->at(i).self_occlusions_->size()) {
-            std::stringstream path_entropy;
-            path_entropy << path << "/entropy_" << v << ".txt";
-            PersistenceUtils::writeFloatToFile(path_entropy.str(),
+            const std::string path_entropy =
+                path + "/entropy_" + std::to_string(v) + ".txt";
+            PersistenceUtils::writeFloatToFile(path_entropy,
                                                models->at(i).self_occlusions_->at(v));
           }
 
           // save keypoints and signatures to disk
-          std::stringstream keypoints_sstr;
-          keypoints_sstr << path << "/keypoint_indices_" << v << ".pcd";
-
-          pcl::io::savePCDFileBinary(keypoints_sstr.str(), *keypoints_pointcloud);
+          const std::string keypoints_sstr =
+              path + "/keypoint_indices_" + std::to_string(v) + ".pcd";
+          pcl::io::savePCDFileBinary(keypoints_sstr, *keypoints_pointcloud);
 
-          std::stringstream path_descriptor;
-          path_descriptor << path << "/descriptor_" << v << ".pcd";
-          pcl::io::savePCDFileBinary(path_descriptor.str(), *signatures);
+          const std::string path_descriptor =
+              path + "/descriptor_" + std::to_string(v) + ".pcd";
+          pcl::io::savePCDFileBinary(path_descriptor, *signatures);
         }
       }
     }
@@ -476,11 +471,11 @@ pcl::rec_3d_framework::LocalRecognitionPipeline<Distance, PointInT, FeatureT>::g
     }
   }
 
-  std::stringstream dir;
-  std::string path = source_->getModelDescriptorDir(model, training_dir_, descr_name_);
-  dir << path << "/pose_" << view_id << ".txt";
+  const std::string path =
+      source_->getModelDescriptorDir(model, training_dir_, descr_name_);
+  const std::string dir = path + "/pose_" + std::to_string(view_id) + ".txt";
 
-  PersistenceUtils::readMatrixFromFile(dir.str(), pose_matrix);
+  PersistenceUtils::readMatrixFromFile(dir, pose_matrix);
 }
 
 template <template <class> class Distance, typename PointInT, typename FeatureT>
@@ -502,9 +497,10 @@ pcl::rec_3d_framework::LocalRecognitionPipeline<Distance, PointInT, FeatureT>::
     }
   }
 
-  std::stringstream dir;
-  std::string path = source_->getModelDescriptorDir(model, training_dir_, descr_name_);
-  dir << path << "/keypoint_indices_" << view_id << ".pcd";
+  const std::string path =
+      source_->getModelDescriptorDir(model, training_dir_, descr_name_);
+  const std::string dir =
+      path + "/keypoint_indices_" + std::to_string(view_id) + ".pcd";
 
-  pcl::io::loadPCDFile(dir.str(), *keypoints_cloud);
+  pcl::io::loadPCDFile(dir, *keypoints_cloud);
 }
diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/local_recognizer.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/local_recognizer.h
index b7826c78..99fa9f7c 100644
--- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/local_recognizer.h
+++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/local_recognizer.h
@@ -9,7 +9,6 @@
 
 #include <pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h>
 #include <pcl/apps/3d_rec_framework/pc_source/source.h>
-#include <pcl/common/common.h>
 #include <pcl/recognition/cg/correspondence_grouping.h>
 #include <pcl/recognition/hv/hypotheses_verification.h>
 #include <pcl/visualization/pcl_visualizer.h>
@@ -81,7 +80,7 @@ class PCL_EXPORTS LocalRecognitionPipeline {
   flann::Index<DistT>* flann_index_;
   std::vector<flann_model> flann_models_;
 
-  std::vector<int> indices_;
+  pcl::Indices indices_;
 
   bool use_cache_;
   std::map<std::pair<std::string, int>,
@@ -185,10 +184,8 @@ class PCL_EXPORTS LocalRecognitionPipeline {
       p_scene.getVector4fMap() =
           (*keypoints_pointcloud)[correspondences[kk].index_match].getVector4fMap();
 
-      std::stringstream line_name;
-      line_name << "line_" << kk;
-
-      vis_corresp_.addLine<pcl::PointXYZ, pcl::PointXYZ>(p_scene, p, line_name.str());
+      const std::string line_name = "line_" + std::to_string(kk);
+      vis_corresp_.addLine<pcl::PointXYZ, pcl::PointXYZ>(p_scene, p, line_name);
     }
 
     vis_corresp_.spin();
@@ -245,7 +242,7 @@ public:
   }
 
   void
-  setIndices(std::vector<int>& indices)
+  setIndices(pcl::Indices& indices)
   {
     indices_ = indices;
   }
diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/utils/metrics.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/utils/metrics.h
index 27cbd816..c258952a 100644
--- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/utils/metrics.h
+++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/utils/metrics.h
@@ -9,9 +9,6 @@
 
 #include <pcl/common/utils.h> // pcl::utils::ignore
 
-#include <cmath>
-#include <cstdlib>
-
 namespace Metrics {
 
 using ::std::abs;
diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/utils/persistence_utils.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/utils/persistence_utils.h
index 2e3fa759..a3c58124 100644
--- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/utils/persistence_utils.h
+++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/utils/persistence_utils.h
@@ -87,31 +87,6 @@ getViewId(std::string id)
   return strs[strs.size() - 2];
 }
 
-inline bool
-readFloatFromFile(const std::string& dir, std::string file, float& value)
-{
-
-  std::vector<std::string> strs;
-  boost::split(strs, file, boost::is_any_of("/"));
-
-  std::string str;
-  for (std::size_t i = 0; i < (strs.size() - 1); i++) {
-    str += strs[i] + "/";
-  }
-
-  std::stringstream matrix_file;
-  matrix_file << dir << "/" << str << "entropy_" << getViewId(file) << ".txt";
-
-  std::ifstream in;
-  in.open(matrix_file.str().c_str(), std::ifstream::in);
-
-  char linebuf[1024];
-  in.getline(linebuf, 1024);
-  value = static_cast<float>(atof(linebuf));
-
-  return true;
-}
-
 inline bool
 readFloatFromFile(const std::string& file, float& value)
 {
@@ -126,53 +101,6 @@ readFloatFromFile(const std::string& file, float& value)
   return true;
 }
 
-inline bool
-readMatrixFromFile(std::string dir, std::string file, Eigen::Matrix4f& matrix)
-{
-
-  // get the descriptor name from dir
-  std::vector<std::string> path;
-  boost::split(path, dir, boost::is_any_of("/"));
-
-  std::string dname = path[path.size() - 1];
-  std::string file_replaced;
-  for (std::size_t i = 0; i < (path.size() - 1); i++) {
-    file_replaced += path[i] + "/";
-  }
-
-  boost::split(path, file, boost::is_any_of("/"));
-  std::string id;
-
-  for (std::size_t i = 0; i < (path.size() - 1); i++) {
-    id += path[i];
-    if (i < (path.size() - 1)) {
-      id += "/";
-    }
-  }
-
-  boost::split(path, file, boost::is_any_of("/"));
-  std::string filename = path[path.size() - 1];
-
-  std::stringstream matrix_file;
-  matrix_file << file_replaced << id << "/" << dname << "/pose_" << getViewId(file)
-              << ".txt";
-
-  std::ifstream in;
-  in.open(matrix_file.str().c_str(), std::ifstream::in);
-
-  char linebuf[1024];
-  in.getline(linebuf, 1024);
-  std::string line(linebuf);
-  std::vector<std::string> strs_2;
-  boost::split(strs_2, line, boost::is_any_of(" "));
-
-  for (int i = 0; i < 16; i++) {
-    matrix(i % 4, i / 4) = static_cast<float>(atof(strs_2[i].c_str()));
-  }
-
-  return true;
-}
-
 inline bool
 readMatrixFromFile(const std::string& file, Eigen::Matrix4f& matrix)
 {
@@ -192,61 +120,6 @@ readMatrixFromFile(const std::string& file, Eigen::Matrix4f& matrix)
 
   return true;
 }
-
-inline bool
-readMatrixFromFile2(const std::string& file, Eigen::Matrix4f& matrix)
-{
-
-  std::ifstream in;
-  in.open(file.c_str(), std::ifstream::in);
-
-  char linebuf[1024];
-  in.getline(linebuf, 1024);
-  std::string line(linebuf);
-  std::vector<std::string> strs_2;
-  boost::split(strs_2, line, boost::is_any_of(" "));
-
-  for (int i = 0; i < 16; i++) {
-    matrix(i / 4, i % 4) = static_cast<float>(atof(strs_2[i].c_str()));
-  }
-
-  return true;
-}
-
-template <typename PointInT>
-inline void
-getPointCloudFromFile(std::string dir,
-                      std::string file,
-                      typename pcl::PointCloud<PointInT>::Ptr& cloud)
-{
-
-  // get the descriptor name from dir
-  std::vector<std::string> path;
-  boost::split(path, dir, boost::is_any_of("/"));
-
-  std::string dname = path[path.size() - 1];
-  std::string file_replaced;
-  for (std::size_t i = 0; i < (path.size() - 1); i++) {
-    file_replaced += path[i] + "/";
-  }
-
-  boost::split(path, file, boost::is_any_of("/"));
-  std::string id;
-
-  for (std::size_t i = 0; i < (path.size() - 1); i++) {
-    id += path[i];
-    if (i < (path.size() - 1)) {
-      id += "/";
-    }
-  }
-
-  std::stringstream view_file;
-  view_file << file_replaced << id << "/" << dname << "/view_" << getViewId(file)
-            << ".pcd";
-
-  pcl::io::loadPCDFile(view_file.str(), *cloud);
-}
-
 } // namespace PersistenceUtils
 } // namespace rec_3d_framework
 } // namespace pcl
diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/utils/vtk_model_sampling.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/utils/vtk_model_sampling.h
index e58ad34a..5f55fcbd 100644
--- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/utils/vtk_model_sampling.h
+++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/utils/vtk_model_sampling.h
@@ -7,7 +7,7 @@
 
 #pragma once
 
-#include <pcl/common/common.h>
+#include <pcl/visualization/vtk/pcl_vtk_compatibility.h>
 
 #include <vtkCellArray.h>
 #include <vtkPLYReader.h>
@@ -74,7 +74,7 @@ randPSurface(vtkPolyData* polydata,
 
   double A[3], B[3], C[3];
   vtkIdType npts = 0;
-  vtkIdType* ptIds = nullptr;
+  vtkCellPtsPtr ptIds = nullptr;
   polydata->GetCellPoints(el, npts, ptIds);
 
   if (ptIds == nullptr)
@@ -98,7 +98,9 @@ uniform_sampling(const vtkSmartPointer<vtkPolyData>& polydata,
   double p1[3], p2[3], p3[3], totalArea = 0;
   std::vector<double> cumulativeAreas(cells->GetNumberOfCells(), 0);
   std::size_t i = 0;
-  vtkIdType npts = 0, *ptIds = nullptr;
+  vtkIdType npts = 0;
+  vtkCellPtsPtr ptIds = nullptr;
+
   for (cells->InitTraversal(); cells->GetNextCell(npts, ptIds); i++) {
     polydata->GetPoint(ptIds[0], p1);
     polydata->GetPoint(ptIds[1], p2);
diff --git a/apps/3d_rec_framework/src/tools/global_classification.cpp b/apps/3d_rec_framework/src/tools/global_classification.cpp
index 0b5522cc..500bb59f 100644
--- a/apps/3d_rec_framework/src/tools/global_classification.cpp
+++ b/apps/3d_rec_framework/src/tools/global_classification.cpp
@@ -70,29 +70,26 @@ segmentAndClassify(
     vis.addPointCloud<OpenNIFrameSource::PointT>(frame, "frame");
 
     for (std::size_t i = 0; i < previous_cluster_size; i++) {
-      std::stringstream cluster_name;
-      cluster_name << "cluster_" << i;
-      vis.removePointCloud(cluster_name.str());
+      std::string cluster_name = "cluster_" + std::to_string(i);
+      vis.removePointCloud(cluster_name);
 
-      cluster_name << "_ply_model_";
-      vis.removeShape(cluster_name.str());
+      cluster_name += "_ply_model_";
+      vis.removeShape(cluster_name);
     }
 
     for (std::size_t i = 0; i < previous_categories_size; i++) {
-      std::stringstream cluster_text;
-      cluster_text << "cluster_" << i << "_text";
-      vis.removeText3D(cluster_text.str());
+      const std::string cluster_text = "cluster_" + std::to_string(i) + "_text";
+      vis.removeText3D(cluster_text);
     }
 
     previous_categories_size = 0;
     float dist_ = 0.03f;
 
     for (std::size_t i = 0; i < clusters.size(); i++) {
-      std::stringstream cluster_name;
-      cluster_name << "cluster_" << i;
+      const std::string cluster_name = "cluster_" + std::to_string(i);
       pcl::visualization::PointCloudColorHandlerRandom<pcl::PointXYZ> random_handler(
           clusters[i]);
-      vis.addPointCloud<pcl::PointXYZ>(clusters[i], random_handler, cluster_name.str());
+      vis.addPointCloud<pcl::PointXYZ>(clusters[i], random_handler, cluster_name);
 
       global.setInputCloud(xyz_points);
       global.setIndices(indices[i].indices);
@@ -117,9 +114,9 @@ segmentAndClassify(
         prob_str.precision(1);
         prob_str << categories[kk] << " [" << conf[kk] << "]";
 
-        std::stringstream cluster_text;
-        cluster_text << "cluster_" << previous_categories_size << "_text";
-        vis.addText3D(prob_str.str(), pos, text_scale, 1, 0, 1, cluster_text.str(), 0);
+        const std::string cluster_text =
+            "cluster_" + std::to_string(previous_categories_size) + "_text";
+        vis.addText3D(prob_str.str(), pos, text_scale, 1, 0, 1, cluster_text, 0);
         previous_categories_size++;
       }
     }
diff --git a/apps/3d_rec_framework/src/tools/local_recognition_mian_dataset.cpp b/apps/3d_rec_framework/src/tools/local_recognition_mian_dataset.cpp
index 4c90da64..b7427680 100644
--- a/apps/3d_rec_framework/src/tools/local_recognition_mian_dataset.cpp
+++ b/apps/3d_rec_framework/src/tools/local_recognition_mian_dataset.cpp
@@ -10,10 +10,10 @@
 #include <pcl/apps/3d_rec_framework/feature_wrapper/local/shot_local_estimator_omp.h>
 #include <pcl/apps/3d_rec_framework/pc_source/mesh_source.h>
 #include <pcl/apps/3d_rec_framework/pipeline/local_recognizer.h>
+#include <pcl/common/transforms.h> // for transformPointCloud
 #include <pcl/console/parse.h>
 #include <pcl/recognition/cg/correspondence_grouping.h>
 #include <pcl/recognition/cg/geometric_consistency.h>
-#include <pcl/recognition/cg/hough_3d.h>
 #include <pcl/recognition/hv/greedy_verification.h>
 #include <pcl/recognition/hv/hv_go.h>
 #include <pcl/recognition/hv/hv_papazov.h>
@@ -105,11 +105,10 @@ recognizeAndVisualize(
         if ((std::size_t)scene != i)
           continue;
 
-      std::stringstream file;
-      file << ply_files_dir.string() << files[i];
+      const std::string file = ply_files_dir.string() + files[i];
 
       typename pcl::PointCloud<PointT>::Ptr scene(new pcl::PointCloud<PointT>());
-      pcl::io::loadPCDFile(file.str(), *scene);
+      pcl::io::loadPCDFile(file, *scene);
 
       local.setVoxelSizeICP(0.005f);
       local.setInputCloud(scene);
@@ -118,18 +117,16 @@ recognizeAndVisualize(
         local.recognize();
       }
 
-      std::stringstream scene_name;
-      scene_name << "Scene " << (i + 1);
+      const std::string scene_name = "Scene " + std::to_string(i + 1);
       vis.addPointCloud<PointT>(scene, "scene_cloud");
-      vis.addText(scene_name.str(), 1, 30, 24, 1, 0, 0, "scene_text");
+      vis.addText(scene_name, 1, 30, 24, 1, 0, 0, "scene_text");
 
       // visualize results
       auto models = local.getModels();
       auto transforms = local.getTransforms();
 
       for (std::size_t j = 0; j < models->size(); j++) {
-        std::stringstream name;
-        name << "cloud_" << j;
+        const std::string name = "cloud_" + std::to_string(j);
 
         ConstPointInTPtr model_cloud = models->at(j).getAssembled(0.0025f);
         typename pcl::PointCloud<PointT>::Ptr model_aligned(
@@ -162,7 +159,7 @@ recognizeAndVisualize(
 
         pcl::visualization::PointCloudColorHandlerCustom<PointT> random_handler(
             model_aligned, r, g, b);
-        vis.addPointCloud<PointT>(model_aligned, random_handler, name.str());
+        vis.addPointCloud<PointT>(model_aligned, random_handler, name);
       }
 
       vis.spin();
@@ -170,9 +167,8 @@ recognizeAndVisualize(
       vis.removePointCloud("scene_cloud");
       vis.removeShape("scene_text");
       for (std::size_t j = 0; j < models->size(); j++) {
-        std::stringstream name;
-        name << "cloud_" << j;
-        vis.removePointCloud(name.str());
+        const std::string name = "cloud_" + std::to_string(j);
+        vis.removePointCloud(name);
       }
     }
   }
@@ -185,27 +181,24 @@ recognizeAndVisualize(
     local.initialize();
 
     for (std::size_t i = 0; i < files.size(); i++) {
-      std::stringstream file;
-      file << ply_files_dir.string() << files[i];
+      const std::string file = ply_files_dir.string() + files[i];
 
       typename pcl::PointCloud<PointT>::Ptr scene(new pcl::PointCloud<PointT>());
-      pcl::io::loadPCDFile(file.str(), *scene);
+      pcl::io::loadPCDFile(file, *scene);
 
       local.setInputCloud(scene);
       local.recognize();
 
-      std::stringstream scene_name;
-      scene_name << "Scene " << (i + 1);
+      const std::string scene_name = "Scene " + std::to_string(i + 1);
       vis.addPointCloud<PointT>(scene, "scene_cloud");
-      vis.addText(scene_name.str(), 1, 30, 24, 1, 0, 0, "scene_text");
+      vis.addText(scene_name, 1, 30, 24, 1, 0, 0, "scene_text");
 
       // visualize results
       auto models = local.getModels();
       auto transforms = local.getTransforms();
 
       for (std::size_t j = 0; j < models->size(); j++) {
-        std::stringstream name;
-        name << "cloud_" << j;
+        const std::string name = "cloud_" + std::to_string(j);
 
         ConstPointInTPtr model_cloud = models->at(j).getAssembled(0.0025f);
         typename pcl::PointCloud<PointT>::Ptr model_aligned(
@@ -214,7 +207,7 @@ recognizeAndVisualize(
 
         pcl::visualization::PointCloudColorHandlerRandom<PointT> random_handler(
             model_aligned);
-        vis.addPointCloud<PointT>(model_aligned, random_handler, name.str());
+        vis.addPointCloud<PointT>(model_aligned, random_handler, name);
       }
 
       vis.spin();
@@ -222,9 +215,8 @@ recognizeAndVisualize(
       vis.removePointCloud("scene_cloud");
       vis.removeShape("scene_text");
       for (std::size_t j = 0; j < models->size(); j++) {
-        std::stringstream name;
-        name << "cloud_" << j;
-        vis.removePointCloud(name.str());
+        const std::string name = "cloud_" + std::to_string(j);
+        vis.removePointCloud(name);
       }
     }
   }
diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt
index 853dac2d..722c7728 100644
--- a/apps/CMakeLists.txt
+++ b/apps/CMakeLists.txt
@@ -1,16 +1,22 @@
 set(SUBSYS_NAME apps)
 set(SUBSYS_DESC "Application examples/samples that show how PCL works")
 set(SUBSYS_DEPS common geometry io filters sample_consensus segmentation visualization kdtree features surface octree registration keypoints tracking search recognition ml stereo 2d)
+set(SUBSYS_OPT_DEPS openni vtk Qt5)
 
 set(DEFAULT FALSE)
 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 openni vtk)
+PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS ${SUBSYS_OPT_DEPS})
 
 if(NOT build)
   return()
 endif()
 
+#Enable cmakes QT auto features.
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_AUTORCC ON)
+set(CMAKE_AUTOUIC ON)
+
 # to be filled with all targets the apps subsystem
 set(PCL_APPS_ALL_TARGETS)
 
@@ -28,14 +34,12 @@ target_link_libraries(pcl_pyramid_surface_matching pcl_common pcl_io pcl_feature
 PCL_ADD_EXECUTABLE(pcl_statistical_multiscale_interest_region_extraction_example COMPONENT ${SUBSYS_NAME} SOURCES src/statistical_multiscale_interest_region_extraction_example.cpp)
 target_link_libraries(pcl_statistical_multiscale_interest_region_extraction_example pcl_common pcl_io pcl_features pcl_filters)
 
-if(LIBUSB_1_FOUND)
+if(LIBUSB_FOUND)
   PCL_ADD_EXECUTABLE(pcl_dinast_grabber COMPONENT ${SUBSYS_NAME} SOURCES src/dinast_grabber_example.cpp)
   target_link_libraries(pcl_dinast_grabber pcl_common pcl_visualization pcl_io)
 endif()
 
 if(VTK_FOUND)
-  include("${VTK_USE_FILE}")
-
   set(incs "include/pcl/${SUBSYS_NAME}/render_views_tesselated_sphere.h")
   set(srcs "src/render_views_tesselated_sphere.cpp")
 
@@ -60,6 +64,10 @@ if(VTK_FOUND)
   if(QHULL_FOUND)
     PCL_ADD_EXECUTABLE(pcl_pcd_select_object_plane COMPONENT ${SUBSYS_NAME} SOURCES src/pcd_select_object_plane.cpp)
     target_link_libraries(pcl_pcd_select_object_plane pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_features pcl_surface)
+    #TODO: Update when CMAKE 3.10 is available
+    if(NOT (${VTK_VERSION} VERSION_LESS 9.0))
+      target_link_libraries(pcl_pcd_select_object_plane VTK::FiltersGeometry)
+    endif()
   endif()
 
   PCL_ADD_EXECUTABLE(pcl_pcd_organized_edge_detection COMPONENT ${SUBSYS_NAME} SOURCES src/pcd_organized_edge_detection.cpp)
@@ -74,17 +82,37 @@ if(VTK_FOUND)
   PCL_ADD_EXECUTABLE(pcl_stereo_ground_segmentation COMPONENT ${SUBSYS_NAME} SOURCES src/stereo_ground_segmentation.cpp)
   target_link_libraries(pcl_stereo_ground_segmentation pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_features pcl_stereo)
 
-  if(Qt5_FOUND AND QVTK_FOUND)
+  if(Qt5_FOUND AND HAVE_QVTK)
     # Manual registration demo
-    QT5_WRAP_UI(manual_registration_ui src/manual_registration/manual_registration.ui)
-    QT5_WRAP_CPP(manual_registration_moc include/pcl/apps/manual_registration.h OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED -DBOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION)
-    PCL_ADD_EXECUTABLE(pcl_manual_registration COMPONENT ${SUBSYS_NAME} SOURCES ${manual_registration_ui} ${manual_registration_moc} src/manual_registration/manual_registration.cpp BUNDLE)
+    PCL_ADD_EXECUTABLE(pcl_manual_registration
+      COMPONENT
+      ${SUBSYS_NAME}
+      SOURCES
+      include/pcl/apps/manual_registration.h
+      src/manual_registration/manual_registration.cpp
+      src/manual_registration/manual_registration.ui
+      BUNDLE)
+      
     target_link_libraries(pcl_manual_registration pcl_common pcl_io pcl_visualization pcl_segmentation pcl_features pcl_surface Qt5::Widgets)
+    #TODO: Update when CMAKE 3.10 is available
+    if(NOT (${VTK_VERSION} VERSION_LESS 9.0))
+      target_link_libraries(pcl_manual_registration VTK::GUISupportQt)
+    endif()
 
-    QT5_WRAP_UI(pcd_video_player_ui src/pcd_video_player/pcd_video_player.ui)
-    QT5_WRAP_CPP(pcd_video_player_moc include/pcl/apps/pcd_video_player.h OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED -DBOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION)
-    PCL_ADD_EXECUTABLE(pcl_pcd_video_player COMPONENT ${SUBSYS_NAME} SOURCES ${pcd_video_player_ui} ${pcd_video_player_moc} src/pcd_video_player/pcd_video_player.cpp BUNDLE)
+    PCL_ADD_EXECUTABLE(pcl_pcd_video_player
+      COMPONENT
+      ${SUBSYS_NAME}
+      SOURCES
+      include/pcl/apps/pcd_video_player.h
+      src/pcd_video_player/pcd_video_player.cpp
+      src/pcd_video_player/pcd_video_player.ui
+      BUNDLE)
+    
     target_link_libraries(pcl_pcd_video_player pcl_common pcl_io pcl_visualization pcl_segmentation pcl_features pcl_surface Qt5::Widgets)
+    #TODO: Update when CMAKE 3.10 is available
+    if(NOT (${VTK_VERSION} VERSION_LESS 9.0))
+      target_link_libraries(pcl_pcd_video_player VTK::GUISupportQt)
+    endif()
   endif()
 
   if(WITH_OPENNI)
@@ -138,18 +166,39 @@ if(VTK_FOUND)
     PCL_ADD_EXECUTABLE(pcl_openni_face_detector COMPONENT ${SUBSYS_NAME} SOURCES src/face_detection//openni_face_detection.cpp src/face_detection//openni_frame_source.cpp BUNDLE)
     target_link_libraries(pcl_openni_face_detector pcl_features pcl_recognition pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_surface pcl_keypoints pcl_ml pcl_search pcl_kdtree ${VTK_LIBRARIES})
 
-    if(Qt5_FOUND AND QVTK_FOUND)
-    # OpenNI Passthrough application demo
-    QT5_WRAP_UI(openni_passthrough_ui src/openni_passthrough.ui)
-    QT5_WRAP_CPP(openni_passthrough_moc include/pcl/apps/openni_passthrough.h OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED -DBOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION)
-    PCL_ADD_EXECUTABLE(pcl_openni_passthrough COMPONENT ${SUBSYS_NAME} SOURCES ${openni_passthrough_ui} ${openni_passthrough_moc} src/openni_passthrough.cpp)
-    target_link_libraries(pcl_openni_passthrough pcl_common pcl_io pcl_filters pcl_visualization Qt5::Widgets)
-
-    # OpenNI Organized Connected Component application demo
-    QT5_WRAP_UI(organized_segmentation_demo_ui src/organized_segmentation_demo.ui)
-    QT5_WRAP_CPP(organized_segmentation_demo_moc include/pcl/apps/organized_segmentation_demo.h OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED -DBOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION)
-    PCL_ADD_EXECUTABLE(pcl_organized_segmentation_demo COMPONENT ${SUBSYS_NAME} SOURCES ${organized_segmentation_demo_ui} ${organized_segmentation_demo_moc} src/organized_segmentation_demo.cpp BUNDLE)
-    target_link_libraries(pcl_organized_segmentation_demo pcl_common pcl_io pcl_visualization pcl_segmentation pcl_features pcl_surface Qt5::Widgets)
+    if(Qt5_FOUND AND HAVE_QVTK)
+      # OpenNI Passthrough application demo
+      PCL_ADD_EXECUTABLE(pcl_openni_passthrough
+        COMPONENT
+          ${SUBSYS_NAME}
+        SOURCES
+          include/pcl/apps/openni_passthrough.h
+          src/openni_passthrough.cpp
+          src/openni_passthrough.ui)
+          
+      target_link_libraries(pcl_openni_passthrough pcl_common pcl_io pcl_filters pcl_visualization Qt5::Widgets)
+      #TODO: Update when CMAKE 3.10 is available
+      if(NOT (${VTK_VERSION} VERSION_LESS 9.0))
+        target_link_libraries(pcl_openni_passthrough VTK::GUISupportQt)
+      endif()
+
+      list(APPEND CMAKE_AUTOUIC_SEARCH_PATHS "src")
+
+      # OpenNI Organized Connected Component application demo
+      PCL_ADD_EXECUTABLE(pcl_organized_segmentation_demo
+        COMPONENT
+          ${SUBSYS_NAME}
+        SOURCES
+          include/pcl/apps/organized_segmentation_demo_qt.h
+          include/pcl/apps/organized_segmentation_demo.h
+          src/organized_segmentation_demo.cpp
+          src/organized_segmentation_demo.ui
+        BUNDLE)
+      target_link_libraries(pcl_organized_segmentation_demo pcl_common pcl_io pcl_visualization pcl_segmentation pcl_features pcl_surface Qt5::Widgets)
+      #TODO: Update when CMAKE 3.10 is available
+      if(NOT (${VTK_VERSION} VERSION_LESS 9.0))
+        target_link_libraries(pcl_organized_segmentation_demo VTK::GUISupportQt)
+      endif()      
     endif()
 
     if(QHULL_FOUND)
diff --git a/apps/cloud_composer/CMakeLists.txt b/apps/cloud_composer/CMakeLists.txt
index d70e4bb5..4b006ee2 100644
--- a/apps/cloud_composer/CMakeLists.txt
+++ b/apps/cloud_composer/CMakeLists.txt
@@ -6,28 +6,10 @@
 set(SUBSUBSYS_NAME cloud_composer)
 set(SUBSUBSYS_DESC "Cloud Composer - Application for Manipulating Point Clouds")
 set(SUBSUBSYS_DEPS common io visualization filters apps)
-
-# Find VTK
-if(NOT VTK_FOUND)
-  set(DEFAULT AUTO_OFF)
-  set(REASON "VTK was not found.")
-else()
-  set(DEFAULT TRUE)
-  set(REASON)
-  include("${VTK_USE_FILE}")
-endif()
-
-# QT5 Found?
-if(NOT Qt5_FOUND)
-  set(DEFAULT AUTO_OFF)
-  set(REASON "Qt5 was not found.")
-elseif(NOT ${DEFAULT} STREQUAL "AUTO_OFF")
-  set(DEFAULT TRUE)
-  set(REASON)
-endif()
+set(SUBSUBSYS_EXT_DEPS vtk Qt5)
 
 # QVTK?
-if(NOT QVTK_FOUND)
+if(NOT HAVE_QVTK)
   set(DEFAULT AUTO_OFF)
   set(REASON "Cloud composer requires QVTK")
 elseif(NOT ${DEFAULT} STREQUAL "AUTO_OFF")
@@ -41,7 +23,7 @@ if("${DEFAULT}" STREQUAL "TRUE")
 endif()
 
 PCL_SUBSUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" "${SUBSUBSYS_DESC}" ${DEFAULT} "${REASON}")
-PCL_SUBSUBSYS_DEPEND(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" DEPS ${SUBSUBSYS_DEPS})
+PCL_SUBSUBSYS_DEPEND(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" DEPS ${SUBSUBSYS_DEPS} EXT_DEPS ${SUBSUBSYS_EXT_DEPS})
 
 PCL_ADD_DOC(${SUBSUBSYS_NAME})
 
@@ -90,9 +72,16 @@ set(INTERFACE_SOURCES
 # Build pcl_cc_tool_interface as static library, to fix issue mentioned in #2708
 set(PCL_LIB_TYPE_ORIGIN ${PCL_LIB_TYPE})
 set(PCL_LIB_TYPE STATIC)
-QT5_WRAP_CPP(INTERFACE_HEADERS_MOC ${INTERFACE_HEADERS} OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED -DBOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION)
-PCL_ADD_LIBRARY(pcl_cc_tool_interface COMPONENT ${SUBSUBSYS_NAME} SOURCES ${INTERFACE_HEADERS} ${INTERFACE_SOURCES} ${INTERFACE_HEADERS_MOC})
-target_link_libraries(pcl_cc_tool_interface pcl_common pcl_filters pcl_search pcl_visualization ${VTK_LIBRARIES} Qt5::Widgets)
+
+PCL_ADD_LIBRARY(pcl_cc_tool_interface COMPONENT ${SUBSUBSYS_NAME} SOURCES ${INTERFACE_HEADERS} ${INTERFACE_SOURCES})
+
+set(vtk_libs ${VTK_LIBRARIES})
+#TODO: Update when CMAKE 3.10 is available
+if (NOT (${VTK_VERSION} VERSION_LESS 9.0))
+  set(vtk_libs VTK::GUISupportQt)
+endif()
+target_link_libraries(pcl_cc_tool_interface pcl_common pcl_filters pcl_search pcl_visualization Qt5::Widgets ${vtk_libs})
+
 set(PCL_LIB_TYPE ${PCL_LIB_TYPE_ORIGIN})
 
 if(APPLE)
@@ -108,7 +97,6 @@ set(incs
   "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/item_inspector.h"
   "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/merge_selection.h"
   "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/project_model.h"
-  "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/properties_model.h"
   "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/signal_multiplexer.h"
   "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/toolbox_model.h"
   "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/transform_clouds.h"
@@ -141,14 +129,14 @@ set(impl_incs
 set(uis src/cloud_composer_main_window.ui)
 set(resources resources/resources.qrc)
 
-QT5_WRAP_UI(cloud_composer_ui ${uis})
-QT5_WRAP_CPP(cloud_composer_moc ${incs} OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED -DBOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION)
-QT5_ADD_RESOURCES(resource_srcs ${resources})
+list(APPEND CMAKE_AUTOUIC_SEARCH_PATHS "src")
 
 set(EXE_NAME "pcl_${SUBSUBSYS_NAME}")
-PCL_ADD_EXECUTABLE(${EXE_NAME} COMPONENT ${SUBSUBSYS_NAME} SOURCES ${cloud_composer_ui} ${cloud_composer_moc} ${srcs} ${resource_srcs} ${impl_incs})
+PCL_ADD_EXECUTABLE(${EXE_NAME} COMPONENT ${SUBSUBSYS_NAME} SOURCES ${uis} ${incs} ${srcs} ${resources} ${impl_incs})
 target_link_libraries("${EXE_NAME}" pcl_cc_tool_interface pcl_common pcl_io pcl_visualization pcl_filters Qt5::Widgets)
 
+
+
 # Install include files
 PCL_ADD_INCLUDES("${SUBSUBSYS_NAME}" "${SUBSUBSYS_NAME}" ${incs} ${item_incs} ${selector_incs})
 PCL_ADD_INCLUDES("${SUBSUBSYS_NAME}" "${SUBSUBSYS_NAME}/impl" ${impl_incs})
diff --git a/apps/cloud_composer/ComposerTool.cmake b/apps/cloud_composer/ComposerTool.cmake
index 6ba49c8d..5261da76 100644
--- a/apps/cloud_composer/ComposerTool.cmake
+++ b/apps/cloud_composer/ComposerTool.cmake
@@ -3,20 +3,18 @@ function(define_composer_tool TOOL_NAME TOOL_SOURCES TOOL_HEADERS DEPS)
 
   project(pcl_cc_tool_${TOOL_NAME})
 
-  #message("Making plugin " pcl_cc_tool_${TOOL_NAME})
-  QT5_WRAP_CPP(TOOL_HEADERS_MOC ${TOOL_HEADERS} OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED -DBOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION)
   set(TOOL_TARGET pcl_cc_tool_${TOOL_NAME})
-  # message("Files:"  ${TOOL_SOURCES} ${TOOL_HEADERS_MOC})
-  PCL_ADD_LIBRARY(${TOOL_TARGET} COMPONENT ${SUBSYS_NAME} SOURCES ${TOOL_SOURCES} ${TOOL_HEADERS} ${TOOL_HEADERS_MOC})
+  PCL_ADD_LIBRARY(${TOOL_TARGET} COMPONENT ${SUBSYS_NAME} SOURCES ${TOOL_SOURCES} ${TOOL_HEADERS})
+  
   if(WIN32)
     set_target_properties (${TOOL_TARGET} PROPERTIES RUNTIME_OUTPUT_DIRECTORY_DEBUG ${CLOUD_COMPOSER_PLUGIN_DIR}
                                                     RUNTIME_OUTPUT_DIRECTORY_RELEASE ${CLOUD_COMPOSER_PLUGIN_DIR})
   else()
     set_target_properties (${TOOL_TARGET} PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CLOUD_COMPOSER_PLUGIN_DIR})
   endif()
+  
   add_definitions(${QT_DEFINITIONS})
   add_definitions(-DQT_PLUGIN)
-  add_definitions(-DQT_NO_DEBUG)
   add_definitions(-DQT_SHARED)
 
   target_link_libraries(${TOOL_TARGET} pcl_cc_tool_interface pcl_common pcl_io ${DEPS} Qt5::Widgets)
diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/cloud_composer.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/cloud_composer.h
index ec7e0be1..9fbfdcc5 100644
--- a/apps/cloud_composer/include/pcl/apps/cloud_composer/cloud_composer.h
+++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/cloud_composer.h
@@ -42,6 +42,7 @@
 #include <pcl/point_types.h>
 
 #include <ui_cloud_composer_main_window.h>
+
 class QTreeView;
 
 namespace pcl
diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/cloud_view.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/cloud_view.h
index 19c91ca7..27780fa5 100644
--- a/apps/cloud_composer/include/pcl/apps/cloud_composer/cloud_view.h
+++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/cloud_view.h
@@ -42,8 +42,15 @@
 #include <vtkEventQtSlotConnect.h>
 
 #include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/visualization/qvtk_compatibility.h>
 #include <pcl/apps/cloud_composer/point_selectors/interactor_style_switch.h>
 
+
+#include <vtkSmartPointer.h>
+#include <vtkOrientationMarkerWidget.h>
+#include <vtkAxesActor.h>
+#include <vtkVersion.h>
+
 class QItemSelection;
 class QStandardItem;
 
@@ -67,11 +74,12 @@ namespace pcl
       
       void 
       setModel (ProjectModel* new_model);
+      
       ProjectModel* 
       getModel () const { return model_; }
       
-      QVTKWidget* 
-      getQVTK() const {return qvtk_; }
+      PCLQVTKWidget*
+      getQVTK() const { return qvtk_; }
       
       pcl::visualization::PCLVisualizer::Ptr
       getPCLVisualizer () const { return vis_; }
@@ -141,7 +149,9 @@ namespace pcl
       
       pcl::visualization::PCLVisualizer::Ptr vis_;
       ProjectModel* model_;
-      QVTKWidget* qvtk_;
+
+      PCLQVTKWidget* qvtk_;
+
       vtkSmartPointer<InteractorStyleSwitch> style_switch_;
       
       vtkSmartPointer<vtkOrientationMarkerWidget> axes_widget_;
diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/items/cloud_item.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/items/cloud_item.h
index debc9a32..a54f7fb2 100644
--- a/apps/cloud_composer/include/pcl/apps/cloud_composer/items/cloud_item.h
+++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/items/cloud_item.h
@@ -40,10 +40,17 @@
 #include <QDebug>
 
 #include <pcl/apps/cloud_composer/items/cloud_composer_item.h>
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/PCLPointCloud2.h>
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
-#include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/search/kdtree.h>
+#include <pcl/visualization/point_cloud_geometry_handlers.h>
+#include <pcl/visualization/point_cloud_color_handlers.h>
+#include <pcl/visualization/pcl_visualizer.h>
+
 
 //Typedefs to make things sane
 using GeometryHandler = pcl::visualization::PointCloudGeometryHandler<pcl::PCLPointCloud2>;
diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/items/fpfh_item.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/items/fpfh_item.h
index 42feb563..5f3ac452 100644
--- a/apps/cloud_composer/include/pcl/apps/cloud_composer/items/fpfh_item.h
+++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/items/fpfh_item.h
@@ -41,10 +41,9 @@
 #include <pcl/features/fpfh.h>
 
 #include <pcl/apps/cloud_composer/items/cloud_composer_item.h>
+#include <pcl/visualization/qvtk_compatibility.h>
 #include <pcl/visualization/pcl_plotter.h>
 
-class QVTKWidget;
-
 namespace pcl
 {
   namespace cloud_composer
@@ -74,7 +73,7 @@ namespace pcl
         pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh_ptr_;
         double radius_;
         pcl::visualization::PCLPlotter::Ptr plot_;
-        QVTKWidget *qvtk_;
+        PCLQVTKWidget* qvtk_;
         QWidget *hist_page_;
     };
 
diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/click_trackball_interactor_style.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/click_trackball_interactor_style.h
index cc7b9016..ecc1a390 100644
--- a/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/click_trackball_interactor_style.h
+++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/click_trackball_interactor_style.h
@@ -39,6 +39,8 @@
 
 #include <pcl/apps/cloud_composer/point_selectors/interactor_style_switch.h>
 
+#include <vtkInteractorStyleTrackballActor.h>
+
 namespace pcl
 {
   namespace cloud_composer
diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/interactor_style_switch.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/interactor_style_switch.h
index d039aa42..1dd72c1d 100644
--- a/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/interactor_style_switch.h
+++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/interactor_style_switch.h
@@ -39,12 +39,19 @@
 
 #include <QMap>
 
-#include <pcl/visualization/vtk.h>
 #include <pcl/visualization/interactor_style.h>
 #include <pcl/visualization/common/actor_map.h>
 #include <pcl/visualization/common/ren_win_interact_map.h>
 #include <pcl/visualization/pcl_visualizer.h>
 
+#include <vtkSmartPointer.h>
+#include <vtkAreaPicker.h>
+#include <vtkPointPicker.h>
+#include <vtkRenderWindowInteractor.h>
+#include <vtkCommand.h>
+#include <vtkRendererCollection.h>
+#include <vtkInteractorStyle.h>
+
 class QVTKWidget;
 
 namespace pcl
diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/manipulation_event.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/manipulation_event.h
index 5e31ff5b..49ef9008 100644
--- a/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/manipulation_event.h
+++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/manipulation_event.h
@@ -37,7 +37,6 @@
 
 #pragma once
 
-#include <pcl/visualization/vtk.h>
 #include <pcl/apps/cloud_composer/items/cloud_item.h>
 
 namespace pcl
diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/rectangular_frustum_selector.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/rectangular_frustum_selector.h
index bc6f7dcc..a89982b9 100644
--- a/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/rectangular_frustum_selector.h
+++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/rectangular_frustum_selector.h
@@ -39,6 +39,10 @@
 
 #include <pcl/apps/cloud_composer/point_selectors/interactor_style_switch.h>
 
+#include <vtkSmartPointer.h>
+#include <vtkRendererCollection.h>
+#include <vtkInteractorStyleRubberBandPick.h>
+
 namespace pcl
 {
   namespace cloud_composer
diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/selected_trackball_interactor_style.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/selected_trackball_interactor_style.h
index 58fc208c..cd4352ac 100644
--- a/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/selected_trackball_interactor_style.h
+++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/selected_trackball_interactor_style.h
@@ -37,6 +37,10 @@
 
 #include <pcl/apps/cloud_composer/point_selectors/interactor_style_switch.h>
 
+#include <vtkInteractorStyleTrackballActor.h>
+#include <vtkSmartPointer.h>
+#include <vtkMatrix4x4.h>
+
 namespace pcl
 {
   namespace cloud_composer
diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/selection_event.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/selection_event.h
index 0e8dfa49..0e40fcb5 100644
--- a/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/selection_event.h
+++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/selection_event.h
@@ -37,9 +37,14 @@
 
 #pragma once
 
-#include <pcl/visualization/vtk.h>
 #include <pcl/apps/cloud_composer/items/cloud_item.h>
 
+#include <vtkSmartPointer.h>
+#include <vtkPolyData.h>
+#include <vtkActor.h>
+#include <vtkDataSetMapper.h>
+#include <vtkRenderer.h>
+
 namespace pcl
 {
   namespace cloud_composer
diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/properties_model.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/properties_model.h
index fcc402bd..2d365701 100644
--- a/apps/cloud_composer/include/pcl/apps/cloud_composer/properties_model.h
+++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/properties_model.h
@@ -39,8 +39,6 @@
 
 #include <QStandardItemModel>
 
-#include <pcl/common/boost.h> 
-
 namespace pcl
 {
   namespace cloud_composer
diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/impl/organized_segmentation.hpp b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/impl/organized_segmentation.hpp
index 1b32f201..708ea1f3 100644
--- a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/impl/organized_segmentation.hpp
+++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/impl/organized_segmentation.hpp
@@ -122,7 +122,7 @@ pcl::cloud_composer::OrganizedSegmentationTool::performTemplatedAction (const QL
     euclidean_segmentation.setInputCloud (input_cloud);
     euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);
 
-    pcl::IndicesPtr extracted_indices (new std::vector<int> ());
+    pcl::IndicesPtr extracted_indices (new pcl::Indices ());
     for (std::size_t i = 0; i < euclidean_label_indices.size (); i++)
     {
       if (euclidean_label_indices[i].indices.size () >= (std::size_t) min_cluster_size)
diff --git a/apps/cloud_composer/src/cloud_view.cpp b/apps/cloud_composer/src/cloud_view.cpp
index 3ac9bf7c..465c557a 100644
--- a/apps/cloud_composer/src/cloud_view.cpp
+++ b/apps/cloud_composer/src/cloud_view.cpp
@@ -5,20 +5,29 @@
 #include <pcl/apps/cloud_composer/point_selectors/selection_event.h>
 #include <pcl/apps/cloud_composer/point_selectors/manipulation_event.h>
 
+#include <vtkGenericOpenGLRenderWindow.h>
+
 #include <QDebug>
 
-#include <QVTKWidget.h>
 
 pcl::cloud_composer::CloudView::CloudView (QWidget* parent)
   : QWidget (parent)
 {
-  vis_.reset (new pcl::visualization::PCLVisualizer ("", false));
-  vis_->getInteractorStyle ()->setKeyboardModifier (pcl::visualization::INTERACTOR_KB_MOD_SHIFT);
+  qvtk_ = new PCLQVTKWidget(this);
   //Create the QVTKWidget
-  qvtk_ = new QVTKWidget (this);
-  qvtk_->SetRenderWindow (vis_->getRenderWindow ());
+#if VTK_MAJOR_VERSION > 8
+  auto renderer = vtkSmartPointer<vtkRenderer>::New();
+  auto renderWindow = vtkSmartPointer<vtkGenericOpenGLRenderWindow>::New();
+  renderWindow->AddRenderer(renderer);
+  vis_.reset(new pcl::visualization::PCLVisualizer(renderer, renderWindow, "", false));
+#else
+  vis_.reset(new pcl::visualization::PCLVisualizer("", false));
+#endif // VTK_MAJOR_VERSION > 8
+  setRenderWindowCompat(*qvtk_, *(vis_->getRenderWindow()));
+  vis_->setupInteractor(getInteractorCompat(*qvtk_), getRenderWindowCompat(*qvtk_), style_switch_);
+  vis_->getInteractorStyle()->setKeyboardModifier(pcl::visualization::INTERACTOR_KB_MOD_SHIFT);
+  
   initializeInteractorSwitch ();
-  vis_->setupInteractor (qvtk_->GetInteractor (), qvtk_->GetRenderWindow (), style_switch_);
   
   QGridLayout *mainLayout = new QGridLayout (this);
   mainLayout-> addWidget (qvtk_,0,0);
@@ -28,13 +37,22 @@ pcl::cloud_composer::CloudView::CloudView (ProjectModel* model, QWidget* parent)
   : QWidget (parent)
 {
   model_ = model;
-  vis_.reset (new pcl::visualization::PCLVisualizer ("", false));
- // vis_->getInteractorStyle ()->setKeyboardModifier (pcl::visualization::INTERACTOR_KB_MOD_SHIFT);
+  
+  qvtk_ = new PCLQVTKWidget(this);
   //Create the QVTKWidget
-  qvtk_ = new QVTKWidget (this);
-  qvtk_->SetRenderWindow (vis_->getRenderWindow ());
+#if VTK_MAJOR_VERSION > 8
+  auto renderer = vtkSmartPointer<vtkRenderer>::New();
+  auto renderWindow = vtkSmartPointer<vtkGenericOpenGLRenderWindow>::New();
+  renderWindow->AddRenderer(renderer);
+  vis_.reset(new pcl::visualization::PCLVisualizer(renderer, renderWindow, "", false));
+#else
+  vis_.reset(new pcl::visualization::PCLVisualizer("", false));
+#endif // VTK_MAJOR_VERSION > 8
+  setRenderWindowCompat(*qvtk_, *(vis_->getRenderWindow()));
+  vis_->setupInteractor(getInteractorCompat(*qvtk_), getRenderWindowCompat(*qvtk_), style_switch_);
+  //vis_->getInteractorStyle()->setKeyboardModifier(pcl::visualization::INTERACTOR_KB_MOD_SHIFT);
+
   initializeInteractorSwitch ();
-  vis_->setupInteractor (qvtk_->GetInteractor (), qvtk_->GetRenderWindow (), style_switch_);
   setModel(model);
   
   QGridLayout *mainLayout = new QGridLayout (this);
@@ -57,9 +75,9 @@ pcl::cloud_composer::CloudView::setModel (ProjectModel* new_model)
   connectSignalsAndSlots();
   //Refresh the view
   qvtk_->show();
-  qvtk_->update ();
+  refresh();
   
- // vis_->addOrientationMarkerWidgetAxes (qvtk_->GetInteractor ());
+ // vis_->addOrientationMarkerWidgetAxes (getInteractorCompat(qvtk_));
 }
 
 void
@@ -76,7 +94,11 @@ pcl::cloud_composer::CloudView::connectSignalsAndSlots()
 void
 pcl::cloud_composer::CloudView::refresh ()
 {
+#if VTK_MAJOR_VERSION > 8
+  qvtk_->renderWindow()->Render();
+#else
   qvtk_->update (); 
+#endif // VTK_MAJOR_VERSION > 8
 }
 
 void
@@ -88,7 +110,7 @@ pcl::cloud_composer::CloudView::itemChanged (QStandardItem* changed_item)
   {
     item->paintView (vis_);
   }
-  qvtk_->update ();
+  refresh();
 }
 
 
@@ -115,8 +137,7 @@ pcl::cloud_composer::CloudView::rowsInserted (const QModelIndex& parent, int sta
       rowsInserted(new_item->index(),0,new_item->rowCount ()-1);
   }
   
-  qvtk_->update ();
-
+  refresh();
 }
 
 void
@@ -143,20 +164,20 @@ pcl::cloud_composer::CloudView::rowsAboutToBeRemoved (const QModelIndex& parent,
     if (item_to_remove->rowCount () > 0) 
       rowsAboutToBeRemoved(item_to_remove->index(),0,item_to_remove->rowCount ()-1);
   }
-  qvtk_->update ();
+  refresh();
 }
 
 
 void 
 pcl::cloud_composer::CloudView::paintEvent (QPaintEvent*)
 {
-  qvtk_->update ();
+  refresh();
 }
 
 void 
 pcl::cloud_composer::CloudView::resizeEvent (QResizeEvent*)
 {
-  qvtk_->update ();
+  refresh();
 }
 
 void
@@ -186,7 +207,7 @@ pcl::cloud_composer::CloudView::selectedItemChanged (const QItemSelection & sele
       }
     }
   }
-  qvtk_->update ();
+  refresh();
 }
 
 void
@@ -204,14 +225,14 @@ pcl::cloud_composer::CloudView::setAxisVisibility (bool visible)
   if (visible)
   {
     qDebug () << "Adding coordinate system!";
-    vis_->addOrientationMarkerWidgetAxes ( qvtk_->GetInteractor() );
+    vis_->addOrientationMarkerWidgetAxes(getInteractorCompat(*qvtk_));
   }
   else
   {
     vis_->removeOrientationMarkerWidgetAxes ();
   }
 
-  qvtk_->update ();
+  refresh();
 }
 
 void
@@ -224,7 +245,7 @@ pcl::cloud_composer::CloudView::addOrientationMarkerWidgetAxes ()
     axes_widget_ = vtkSmartPointer<vtkOrientationMarkerWidget>::New ();
     axes_widget_->SetOutlineColor ( 0.9300, 0.5700, 0.1300 );
     axes_widget_->SetOrientationMarker( axes );
-    axes_widget_->SetInteractor( qvtk_->GetInteractor () );
+    axes_widget_->SetInteractor(getInteractorCompat(*qvtk_));
     axes_widget_->SetViewport( 0.0, 0.0, 0.4, 0.4 );
     axes_widget_->SetEnabled( 1 );
     axes_widget_->InteractiveOn();
@@ -244,8 +265,6 @@ pcl::cloud_composer::CloudView::removeOrientationMarkerWidgetAxes ()
   {
     axes_widget_->SetEnabled (false);
   }
-  
-  
 }
 
 ////////  Interactor Functions
@@ -255,7 +274,7 @@ pcl::cloud_composer::CloudView::initializeInteractorSwitch ()
 {
   style_switch_ = vtkSmartPointer<InteractorStyleSwitch>::New();
   style_switch_->initializeInteractorStyles (vis_, model_);
-  style_switch_->SetInteractor (qvtk_->GetInteractor ());
+  style_switch_->SetInteractor(getInteractorCompat(*qvtk_));
   style_switch_->setCurrentInteractorStyle (interactor_styles::PCL_VISUALIZER);
   
   //Connect the events!
diff --git a/apps/cloud_composer/src/items/fpfh_item.cpp b/apps/cloud_composer/src/items/fpfh_item.cpp
index 4ee81783..f48b5623 100644
--- a/apps/cloud_composer/src/items/fpfh_item.cpp
+++ b/apps/cloud_composer/src/items/fpfh_item.cpp
@@ -3,8 +3,6 @@
 
 #include <QGridLayout>
 
-#include <QVTKWidget.h>
-
 pcl::cloud_composer::FPFHItem::FPFHItem (QString name, const pcl::PointCloud<pcl::FPFHSignature33>::Ptr& fpfh_ptr, double radius)
   : CloudComposerItem (std::move(name))
   , fpfh_ptr_ (fpfh_ptr)
@@ -40,7 +38,7 @@ pcl::cloud_composer::FPFHItem::getInspectorTabs ()
   if (!plot_)
   {
     plot_.reset (new pcl::visualization::PCLPlotter);
-    qvtk_ = new QVTKWidget ();
+    qvtk_ = new PCLQVTKWidget();
     hist_page_ = new QWidget ();
     QGridLayout *mainLayout = new QGridLayout (hist_page_);
     mainLayout-> addWidget (qvtk_,0,0);
@@ -49,10 +47,15 @@ pcl::cloud_composer::FPFHItem::getInspectorTabs ()
   //Plot the histogram
   plot_->addFeatureHistogram (*fpfh_ptr_, fpfh_ptr_->width, data(ItemDataRole::ITEM_ID).toString().toStdString ());
   //Set the render window of the QVTK widget, update
-  plot_->setViewInteractor (vtkSmartPointer<vtkRenderWindowInteractor> (qvtk_->GetInteractor ()));
-  qvtk_->SetRenderWindow (plot_->getRenderWindow ());
+  plot_->setViewInteractor(getInteractorCompat(*qvtk_));
+  setRenderWindowCompat(*qvtk_, *(plot_->getRenderWindow()));
+#if VTK_MAJOR_VERSION > 8
+  qvtk_->renderWindow()->Render();
+#else
+  qvtk_->update();
+#endif // VTK_MAJOR_VERSION > 8
   qvtk_->show ();
-  qvtk_->update ();
+  
   
   QMap <QString, QWidget*> tabs;
   tabs.insert ("Histogram",hist_page_);
diff --git a/apps/cloud_composer/src/point_selectors/click_trackball_interactor_style.cpp b/apps/cloud_composer/src/point_selectors/click_trackball_interactor_style.cpp
index e316c55f..c21b2125 100644
--- a/apps/cloud_composer/src/point_selectors/click_trackball_interactor_style.cpp
+++ b/apps/cloud_composer/src/point_selectors/click_trackball_interactor_style.cpp
@@ -4,6 +4,10 @@
 
 #include <QDebug>
 
+
+#include <vtkObjectFactory.h> // For vtkStandardNewMacro
+#include <vtkTransform.h>
+
 namespace pcl
 {
   namespace cloud_composer
diff --git a/apps/cloud_composer/src/point_selectors/interactor_style_switch.cpp b/apps/cloud_composer/src/point_selectors/interactor_style_switch.cpp
index b1f3f597..72fbf7f6 100644
--- a/apps/cloud_composer/src/point_selectors/interactor_style_switch.cpp
+++ b/apps/cloud_composer/src/point_selectors/interactor_style_switch.cpp
@@ -6,6 +6,9 @@
 
 #include <QDebug>
 
+#include <vtkCallbackCommand.h>
+#include <vtkObjectFactory.h>
+
 namespace pcl
 {
   namespace cloud_composer
diff --git a/apps/cloud_composer/src/point_selectors/rectangular_frustum_selector.cpp b/apps/cloud_composer/src/point_selectors/rectangular_frustum_selector.cpp
index c237692c..0bd7114c 100644
--- a/apps/cloud_composer/src/point_selectors/rectangular_frustum_selector.cpp
+++ b/apps/cloud_composer/src/point_selectors/rectangular_frustum_selector.cpp
@@ -3,6 +3,14 @@
 
 #include <QDebug>
 
+#include <vtkSmartPointer.h>
+#include <vtkIdFilter.h>
+#include <vtkExtractGeometry.h>
+#include <vtkVertexGlyphFilter.h>
+#include <vtkPlanes.h>
+#include <vtkAreaPicker.h>
+#include <vtkObjectFactory.h>
+
 namespace pcl
 {
   namespace cloud_composer
diff --git a/apps/cloud_composer/src/point_selectors/selected_trackball_interactor_style.cpp b/apps/cloud_composer/src/point_selectors/selected_trackball_interactor_style.cpp
index dc145333..9ad0f579 100644
--- a/apps/cloud_composer/src/point_selectors/selected_trackball_interactor_style.cpp
+++ b/apps/cloud_composer/src/point_selectors/selected_trackball_interactor_style.cpp
@@ -4,6 +4,15 @@
 #include <QDebug>
 #include <QItemSelectionModel>
 
+#include <vtkSmartPointer.h>
+#include <vtkMatrix4x4.h>
+#include <vtkLODActor.h>
+#include <vtkInteractorStyleTrackballActor.h>
+#include <vtkRenderWindowInteractor.h>
+#include <vtkTransform.h>
+#include <vtkObjectFactory.h>
+
+
 namespace pcl
 {
   namespace cloud_composer
diff --git a/apps/cloud_composer/src/project_model.cpp b/apps/cloud_composer/src/project_model.cpp
index 0f1575bd..5be855c4 100644
--- a/apps/cloud_composer/src/project_model.cpp
+++ b/apps/cloud_composer/src/project_model.cpp
@@ -14,6 +14,11 @@
 #include <QMessageBox>
 #include <QThread>
 
+#include <vtkSmartPointer.h>
+#include <vtkImageData.h>
+#include <vtkImageReader2Factory.h>
+#include <vtkImageReader2.h>
+
 pcl::cloud_composer::ProjectModel::ProjectModel (QObject* parent)
   : QStandardItemModel (parent)
 {
diff --git a/apps/cloud_composer/src/transform_clouds.cpp b/apps/cloud_composer/src/transform_clouds.cpp
index 78da15ff..1953fd3e 100644
--- a/apps/cloud_composer/src/transform_clouds.cpp
+++ b/apps/cloud_composer/src/transform_clouds.cpp
@@ -1,7 +1,6 @@
 #include <pcl/apps/cloud_composer/transform_clouds.h>
 #include <pcl/apps/cloud_composer/items/cloud_item.h>
 
-#include <pcl/filters/extract_indices.h>
 #include <pcl/point_types.h>
 
 #include <pcl/apps/cloud_composer/impl/transform_clouds.hpp>
diff --git a/apps/cloud_composer/tools/euclidean_clustering.cpp b/apps/cloud_composer/tools/euclidean_clustering.cpp
index 4a52dd75..13f5d914 100644
--- a/apps/cloud_composer/tools/euclidean_clustering.cpp
+++ b/apps/cloud_composer/tools/euclidean_clustering.cpp
@@ -69,7 +69,7 @@ pcl::cloud_composer::EuclideanClusteringTool::performAction (ConstItemList input
       Eigen::Vector4f source_origin = input_item->data (ItemDataRole::ORIGIN).value<Eigen::Vector4f> ();
       Eigen::Quaternionf source_orientation =  input_item->data (ItemDataRole::ORIENTATION).value<Eigen::Quaternionf> ();
       //Vector to accumulate the extracted indices
-      pcl::IndicesPtr extracted_indices (new std::vector<int> ());
+      pcl::IndicesPtr extracted_indices (new pcl::Indices ());
       //Put found clusters into new cloud_items!
       qDebug () << "Found "<<cluster_indices.size ()<<" clusters!";
       int cluster_count = 0;
diff --git a/apps/in_hand_scanner/CMakeLists.txt b/apps/in_hand_scanner/CMakeLists.txt
index 4cb19e77..35e9884f 100644
--- a/apps/in_hand_scanner/CMakeLists.txt
+++ b/apps/in_hand_scanner/CMakeLists.txt
@@ -2,34 +2,9 @@ set(SUBSUBSYS_NAME in_hand_scanner)
 set(SUBSUBSYS_DESC "In-hand scanner for small objects")
 set(SUBSUBSYS_DEPS     common     features     io     kdtree apps)
 set(SUBSUBSYS_LIBS pcl_common pcl_features pcl_io pcl_kdtree)
+set(SUBSUBSYS_EXT_DEPS Qt5 OpenGL OpenGL_GLU openni)
 
 ################################################################################
-# QT5 Found?
-if(NOT Qt5_FOUND)
-  set(DEFAULT AUTO_OFF)
-  set(REASON "Qt5 was not found.")
-elseif(NOT ${DEFAULT} STREQUAL "AUTO_OFF")
-  set(DEFAULT TRUE)
-  set(REASON)
-endif()
-
-# OpenGL
-if(NOT OPENGL_FOUND AND NOT OPENGL_GLU_FOUND)
-  set(DEFAULT AUTO_OFF)
-  set(REASON "OpenGL & GLU are required for the in_hand_scanner app!")
-elseif(NOT ${DEFAULT} STREQUAL "AUTO_OFF")
-  set(DEFAULT TRUE)
-  set(REASON)
-endif()
-
-#OpenNI
-if(NOT WITH_OPENNI)
-  set(DEFAULT AUTO_OFF)
-  set(REASON "OpenNI was not found or was disabled by the user.")
-elseif(NOT ${DEFAULT} STREQUAL "AUTO_OFF")
-  set(DEFAULT TRUE)
-  set(REASON)
-endif()
 
 # Default to not building for now
 if(${DEFAULT} STREQUAL "TRUE")
@@ -37,7 +12,7 @@ if(${DEFAULT} STREQUAL "TRUE")
 endif()
 
 pcl_subsubsys_option(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}")
-pcl_subsubsys_depend(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" DEPS ${SUBSYS_DEPS} EXT_DEPS Qt5 OpenGL OpenGL_GLU openni)
+pcl_subsubsys_depend(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" DEPS ${SUBSYS_DEPS} EXT_DEPS ${SUBSUBSYS_EXT_DEPS})
 
 pcl_add_doc("${SUBSUBSYS_NAME}")
 
@@ -53,6 +28,10 @@ set(INCS
   "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/mesh_processing.h"
   "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/utils.h"
   "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/visibility_confidence.h"
+  "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/main_window.h"
+  "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/help_window.h"
+  "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/opengl_viewer.h"
+  "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/in_hand_scanner.h"
 )
 
 set(IMPL_INCS
@@ -72,20 +51,16 @@ set(SRCS
   src/visibility_confidence.cpp
 )
 
-# Qt
-set(MOC_IN_HAND_SCANNER_INC     "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/in_hand_scanner.h")
-set(MOC_OPENGL_VIEWER_INC       "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/opengl_viewer.h")
-set(MOC_OFFLINE_INTEGRATION_INC "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/offline_integration.h")
-
-set(MOC_MAIN_WINDOW_INC "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/main_window.h")
-set(MOC_HELP_WINDOW_INC "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/help_window.h")
-set(UI_MAIN_WINDOW      src/main_window.ui)
-set(UI_HELP_WINDOW      src/help_window.ui)
+set(UI
+  src/main_window.ui
+  src/help_window.ui)
 
 # Offline integration
 set(OI_INCS
   "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/integration.h"
   "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/visibility_confidence.h"
+  "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/opengl_viewer.h"
+  "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/offline_integration.h"
 )
 
 set(OI_SRCS
@@ -102,23 +77,20 @@ if(NOT build)
   return()
 endif()
 
-qt5_wrap_cpp(MOC_IN_HAND_SCANNER_SRC     "${MOC_IN_HAND_SCANNER_INC}")
-qt5_wrap_cpp(MOC_OPENGL_VIEWER_SRC       "${MOC_OPENGL_VIEWER_INC}" OPTIONS -DBOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION)
-qt5_wrap_cpp(MOC_OFFLINE_INTEGRATION_SRC "${MOC_OFFLINE_INTEGRATION_INC}" OPTIONS -DBOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION)
-
-qt5_wrap_cpp(MOC_MAIN_WINDOW_SRC "${MOC_MAIN_WINDOW_INC}")
-qt5_wrap_cpp(MOC_HELP_WINDOW_SRC "${MOC_HELP_WINDOW_INC}")
-qt5_wrap_ui(UI_MAIN_WINDOW_INC   "${UI_MAIN_WINDOW}")
-qt5_wrap_ui(UI_HELP_WINDOW_INC   "${UI_HELP_WINDOW}")
-
 include_directories("${CMAKE_CURRENT_BINARY_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}/include")
 
-# In-hand scanner
-list(APPEND INCS "${MOC_IN_HAND_SCANNER_INC}" "${MOC_OPENGL_VIEWER_INC}" "${MOC_MAIN_WINDOW_INC}" "${MOC_HELP_WINDOW_INC}" "${UI_MAIN_WINDOW_INC}" "${UI_HELP_WINDOW_INC}")
-list(APPEND SRCS "${MOC_IN_HAND_SCANNER_SRC}" "${MOC_OPENGL_VIEWER_SRC}" "${MOC_MAIN_WINDOW_SRC}" "${MOC_HELP_WINDOW_SRC}")
-
 set(EXE_NAME "pcl_${SUBSUBSYS_NAME}")
-PCL_ADD_EXECUTABLE(${EXE_NAME} COMPONENT ${SUBSUBSYS_NAME} SOURCES ${SRCS} ${INCS} ${IMPL_INCS} BUNDLE)
+
+PCL_ADD_EXECUTABLE(
+  ${EXE_NAME}
+  COMPONENT
+    ${SUBSUBSYS_NAME}
+  SOURCES
+    ${SRCS}
+    ${INCS}
+    ${IMPL_INCS}
+    ${UI}
+  BUNDLE)
 target_link_libraries("${EXE_NAME}" ${SUBSUBSYS_LIBS} ${OPENGL_LIBRARIES} Qt5::Concurrent Qt5::Widgets Qt5::OpenGL)
 
 pcl_add_includes("${SUBSUBSYS_NAME}" "${SUBSUBSYS_NAME}" ${INCS})
@@ -126,11 +98,15 @@ pcl_add_includes("${SUBSUBSYS_NAME}" "${SUBSUBSYS_NAME}/impl" ${IMPL_INCS})
 
 PCL_MAKE_PKGCONFIG(${EXE_NAME} COMPONENT ${SUBSUBSYS_NAME} DESC ${SUBSUBSYS_DESC})
 
-# Offline integration
-list(APPEND OI_INCS "${MOC_OPENGL_VIEWER_INC}" "${MOC_OFFLINE_INTEGRATION_INC}")
-list(APPEND OI_SRCS "${MOC_OPENGL_VIEWER_SRC}" "${MOC_OFFLINE_INTEGRATION_SRC}")
+PCL_ADD_EXECUTABLE(
+  pcl_offline_integration
+  COMPONENT
+    ${SUBSUBSYS_NAME}
+  SOURCES
+    ${OI_SRCS}
+    ${OI_INCS}
+  BUNDLE)
 
-PCL_ADD_EXECUTABLE(pcl_offline_integration COMPONENT ${SUBSUBSYS_NAME} SOURCES ${OI_SRCS} ${OI_INCS} BUNDLE)
 target_link_libraries(pcl_offline_integration ${SUBSUBSYS_LIBS} ${OPENGL_LIBRARIES} Qt5::Concurrent Qt5::Widgets Qt5::OpenGL)
 
 # Add to the compound apps target
diff --git a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/boost.h b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/boost.h
index 5575dccb..68491e6f 100644
--- a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/boost.h
+++ b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/boost.h
@@ -43,6 +43,6 @@
 #ifdef __GNUC__
 #  pragma GCC system_header
 #endif
-
+PCL_DEPRECATED_HEADER(1, 15, "Please include the needed boost headers directly.")
 #include <boost/math/special_functions/fpclassify.hpp>
 #include <boost/signals2/connection.hpp>
diff --git a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/eigen.h b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/eigen.h
index 7c1208e3..ccbefab4 100644
--- a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/eigen.h
+++ b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/eigen.h
@@ -43,7 +43,7 @@
 #ifdef __GNUC__
 #  pragma GCC system_header
 #endif
-
+PCL_DEPRECATED_HEADER(1, 15, "Please include the needed eigen headers directly.")
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 #include <Eigen/Cholesky>
diff --git a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/icp.h b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/icp.h
index 46991846..1448564f 100644
--- a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/icp.h
+++ b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/icp.h
@@ -41,8 +41,6 @@
 #pragma once
 
 #include <pcl/pcl_exports.h>
-#include <pcl/apps/in_hand_scanner/boost.h>
-#include <pcl/apps/in_hand_scanner/eigen.h>
 #include <pcl/apps/in_hand_scanner/common_types.h>
 #include <pcl/kdtree/kdtree.h>
 
diff --git a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/impl/common_types.hpp b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/impl/common_types.hpp
index 76f82bc3..a0e5a1eb 100644
--- a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/impl/common_types.hpp
+++ b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/impl/common_types.hpp
@@ -99,6 +99,8 @@ namespace pcl
         this->directions = other.directions;
       }
 
+      inline PointIHS& operator=(const PointIHS& other) = default;
+
       inline PointIHS (const pcl::PointXYZRGBNormal& other, const float weight)
       {
         this->x       = other.x;
diff --git a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/in_hand_scanner.h b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/in_hand_scanner.h
index b972e668..fa2c81db 100644
--- a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/in_hand_scanner.h
+++ b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/in_hand_scanner.h
@@ -43,10 +43,9 @@
 #include <pcl/memory.h>
 #include <pcl/pcl_exports.h>
 #include <pcl/pcl_macros.h>
-#include <pcl/apps/in_hand_scanner/boost.h>
 #include <pcl/apps/in_hand_scanner/common_types.h>
 #include <pcl/apps/in_hand_scanner/opengl_viewer.h>
-
+#include <boost/signals2/connection.hpp> // for connection
 #include <mutex>
 #include <string>
 #include <sstream>
diff --git a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/input_data_processing.h b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/input_data_processing.h
index 63b9cc15..46362e9f 100644
--- a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/input_data_processing.h
+++ b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/input_data_processing.h
@@ -41,7 +41,6 @@
 #pragma once
 
 #include <pcl/pcl_exports.h>
-#include <pcl/apps/in_hand_scanner/eigen.h>
 #include <pcl/apps/in_hand_scanner/common_types.h>
 #include <pcl/apps/in_hand_scanner/utils.h>
 #include <pcl/features/integral_image_normal.h>
diff --git a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/offline_integration.h b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/offline_integration.h
index cff11995..895fe8a3 100644
--- a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/offline_integration.h
+++ b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/offline_integration.h
@@ -46,8 +46,6 @@
 #include <pcl/common/time.h>
 #include <pcl/features/integral_image_normal.h>
 #include <pcl/apps/in_hand_scanner/common_types.h>
-#include <pcl/apps/in_hand_scanner/boost.h>
-#include <pcl/apps/in_hand_scanner/eigen.h>
 #include <pcl/apps/in_hand_scanner/opengl_viewer.h>
 
 #include <mutex>
diff --git a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/opengl_viewer.h b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/opengl_viewer.h
index a684650c..46f73b21 100644
--- a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/opengl_viewer.h
+++ b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/opengl_viewer.h
@@ -44,9 +44,7 @@
 #include <pcl/pcl_exports.h>
 #include <pcl/pcl_macros.h>
 #include <pcl/common/time.h>
-#include <pcl/apps/in_hand_scanner/boost.h>
 #include <pcl/apps/in_hand_scanner/common_types.h>
-#include <pcl/apps/in_hand_scanner/eigen.h>
 
 #include <QGLWidget>
 
diff --git a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/visibility_confidence.h b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/visibility_confidence.h
index 086baacb..ef4ed284 100644
--- a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/visibility_confidence.h
+++ b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/visibility_confidence.h
@@ -45,7 +45,6 @@
 #include <pcl/memory.h>
 #include <pcl/pcl_exports.h>
 #include <pcl/pcl_macros.h>
-#include <pcl/apps/in_hand_scanner/eigen.h>
 
 namespace pcl
 {
diff --git a/apps/in_hand_scanner/src/icp.cpp b/apps/in_hand_scanner/src/icp.cpp
index 0688595e..8f21a802 100644
--- a/apps/in_hand_scanner/src/icp.cpp
+++ b/apps/in_hand_scanner/src/icp.cpp
@@ -203,7 +203,7 @@ pcl::ihs::ICP::findTransformation (const MeshConstPtr&              mesh_model,
   kd_tree_->setInputCloud (cloud_model_selected);
   t_build = sw.getTime ();
 
-  std::vector <int>   index (1);
+  pcl::Indices   index (1);
   std::vector <float> squared_distance (1);
 
   // Clouds with one to one correspondences
diff --git a/apps/in_hand_scanner/src/input_data_processing.cpp b/apps/in_hand_scanner/src/input_data_processing.cpp
index 1afbb53d..22df6626 100644
--- a/apps/in_hand_scanner/src/input_data_processing.cpp
+++ b/apps/in_hand_scanner/src/input_data_processing.cpp
@@ -42,7 +42,6 @@
 
 #include <pcl/common/point_tests.h>
 #include <pcl/features/integral_image_normal.h>
-#include <pcl/apps/in_hand_scanner/boost.h>
 
 ////////////////////////////////////////////////////////////////////////////////
 
diff --git a/apps/in_hand_scanner/src/integration.cpp b/apps/in_hand_scanner/src/integration.cpp
index 565a8ecb..a779afe1 100644
--- a/apps/in_hand_scanner/src/integration.cpp
+++ b/apps/in_hand_scanner/src/integration.cpp
@@ -45,7 +45,6 @@
 #include <limits>
 
 #include <pcl/kdtree/kdtree_flann.h>
-#include <pcl/apps/in_hand_scanner/boost.h>
 #include <pcl/apps/in_hand_scanner/visibility_confidence.h>
 #include <pcl/apps/in_hand_scanner/utils.h>
 
@@ -219,7 +218,7 @@ pcl::ihs::Integration::merge (const CloudXYZRGBNormalConstPtr& cloud_data,
     xyz_model->push_back (PointXYZ (pt.x, pt.y, pt.z));
   }
   kd_tree_->setInputCloud (xyz_model);
-  std::vector <int>   index (1);
+  pcl::Indices   index (1);
   std::vector <float> squared_distance (1);
 
   mesh_model->reserveVertices (mesh_model->sizeVertices () + cloud_data->size ());
diff --git a/apps/in_hand_scanner/src/visibility_confidence.cpp b/apps/in_hand_scanner/src/visibility_confidence.cpp
index d0bda321..fb2ff56f 100644
--- a/apps/in_hand_scanner/src/visibility_confidence.cpp
+++ b/apps/in_hand_scanner/src/visibility_confidence.cpp
@@ -39,6 +39,7 @@
  */
 
 #include <pcl/apps/in_hand_scanner/visibility_confidence.h>
+#include <Eigen/Geometry> // for Isometry3f
 
 pcl::ihs::Dome::Dome ()
 {
diff --git a/apps/include/pcl/apps/dominant_plane_segmentation.h b/apps/include/pcl/apps/dominant_plane_segmentation.h
index 9fe10b50..b74e8ea5 100644
--- a/apps/include/pcl/apps/dominant_plane_segmentation.h
+++ b/apps/include/pcl/apps/dominant_plane_segmentation.h
@@ -35,13 +35,10 @@
 
 #pragma once
 
-#include <pcl/common/common.h>
 #include <pcl/features/normal_3d.h>
 #include <pcl/filters/passthrough.h>
 #include <pcl/filters/project_inliers.h>
 #include <pcl/filters/voxel_grid.h>
-#include <pcl/sample_consensus/method_types.h>
-#include <pcl/sample_consensus/model_types.h>
 #include <pcl/search/kdtree.h> // for KdTree
 #include <pcl/segmentation/extract_clusters.h>
 #include <pcl/segmentation/extract_polygonal_prism_data.h>
diff --git a/apps/include/pcl/apps/face_detection/face_detection_apps_utils.h b/apps/include/pcl/apps/face_detection/face_detection_apps_utils.h
index ee7ab84e..b986ea9a 100644
--- a/apps/include/pcl/apps/face_detection/face_detection_apps_utils.h
+++ b/apps/include/pcl/apps/face_detection/face_detection_apps_utils.h
@@ -99,13 +99,12 @@ displayHeads(std::vector<Eigen::VectorXf>& heads,
              pcl::visualization::PCLVisualizer& vis)
 {
   for (std::size_t i = 0; i < heads.size(); i++) {
-    std::stringstream name;
-    name << "sphere" << i;
+    std::string name = "sphere" + std::to_string(i);
     pcl::PointXYZ center_point;
     center_point.x = heads[i][0];
     center_point.y = heads[i][1];
     center_point.z = heads[i][2];
-    vis.addSphere(center_point, 0.02, 0, 255, 0, name.str());
+    vis.addSphere(center_point, 0.02, 0, 255, 0, name);
 
     pcl::ModelCoefficients cylinder_coeff;
     cylinder_coeff.values.resize(7); // We need 7 values
@@ -127,8 +126,8 @@ displayHeads(std::vector<Eigen::VectorXf>& heads,
     cylinder_coeff.values[5] = vec[2];
 
     cylinder_coeff.values[6] = 0.01f;
-    name << "cylinder";
-    vis.addCylinder(cylinder_coeff, name.str());
+    name += "cylinder";
+    vis.addCylinder(cylinder_coeff, name);
   }
 }
 
diff --git a/apps/include/pcl/apps/impl/dominant_plane_segmentation.hpp b/apps/include/pcl/apps/impl/dominant_plane_segmentation.hpp
index e2b72a7a..4a47568e 100644
--- a/apps/include/pcl/apps/impl/dominant_plane_segmentation.hpp
+++ b/apps/include/pcl/apps/impl/dominant_plane_segmentation.hpp
@@ -33,11 +33,8 @@
  *
  */
 
-#include <pcl/common/time.h>
-#include <pcl/features/integral_image_normal.h>
 #include <pcl/filters/extract_indices.h> // for ExtractIndices
-#include <pcl/visualization/pcl_visualizer.h>
-#include <pcl/memory.h> // for pcl::make_shared
+#include <pcl/memory.h>                  // for pcl::make_shared
 
 template <typename PointType>
 void
@@ -84,7 +81,7 @@ pcl::apps::DominantPlaneSegmentation<PointType>::compute_table_plane()
   pass_.filter(*cloud_filtered_);
 
   if (int(cloud_filtered_->size()) < k_) {
-    PCL_WARN("[DominantPlaneSegmentation] Filtering returned %lu points! Aborting.",
+    PCL_WARN("[DominantPlaneSegmentation] Filtering returned %lu points! Aborting.\n",
              cloud_filtered_->size());
     return;
   }
@@ -105,7 +102,7 @@ pcl::apps::DominantPlaneSegmentation<PointType>::compute_table_plane()
   seg_.segment(*table_inliers_, *table_coefficients_);
 
   if (table_inliers_->indices.empty()) {
-    PCL_WARN("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.");
+    PCL_WARN("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.\n");
     return;
   }
 
@@ -236,7 +233,7 @@ pcl::apps::DominantPlaneSegmentation<PointType>::compute_fast(
   seg_.segment(*table_inliers_, *table_coefficients_);
 
   if (table_inliers_->indices.empty()) {
-    PCL_WARN("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.");
+    PCL_WARN("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.\n");
     return;
   }
 
@@ -303,7 +300,7 @@ pcl::apps::DominantPlaneSegmentation<PointType>::compute_fast(
     binary_cloud->points.resize(input_->size());
     binary_cloud->is_dense = input_->is_dense;
 
-    for (const int& idx : cloud_object_indices.indices) {
+    for (const auto& idx : cloud_object_indices.indices) {
       (*binary_cloud)[idx].getVector4fMap() = (*input_)[idx].getVector4fMap();
       (*binary_cloud)[idx].intensity = 1.0;
     }
@@ -749,7 +746,7 @@ pcl::apps::DominantPlaneSegmentation<PointType>::compute_full(
   pass_.filter(*cloud_filtered_);
 
   if (int(cloud_filtered_->size()) < k_) {
-    PCL_WARN("[DominantPlaneSegmentation] Filtering returned %lu points! Aborting.",
+    PCL_WARN("[DominantPlaneSegmentation] Filtering returned %lu points! Aborting.\n",
              cloud_filtered_->size());
     return;
   }
@@ -780,7 +777,7 @@ pcl::apps::DominantPlaneSegmentation<PointType>::compute_full(
   seg_.segment(*table_inliers_, *table_coefficients_);
 
   if (table_inliers_->indices.empty()) {
-    PCL_WARN("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.");
+    PCL_WARN("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.\n");
     return;
   }
 
diff --git a/apps/include/pcl/apps/manual_registration.h b/apps/include/pcl/apps/manual_registration.h
index aef724e2..d9fffbdb 100644
--- a/apps/include/pcl/apps/manual_registration.h
+++ b/apps/include/pcl/apps/manual_registration.h
@@ -34,14 +34,8 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <pcl/common/angles.h>
 #include <pcl/common/common.h>
 #include <pcl/common/time.h>
-#include <pcl/common/transforms.h>
-#include <pcl/console/parse.h>
-#include <pcl/console/print.h>
-#include <pcl/io/pcd_grabber.h>
-#include <pcl/io/pcd_io.h>
 #include <pcl/registration/transformation_estimation_svd.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/visualization/point_cloud_handlers.h>
@@ -51,7 +45,6 @@
 #include <QMainWindow>
 #include <QMutex>
 #include <QTimer>
-#include <ui_manual_registration.h>
 
 using PointT = pcl::PointXYZRGBA;
 
@@ -106,6 +99,9 @@ public:
   DstPointPickCallback(const pcl::visualization::PointPickingEvent& event, void*);
 
 protected:
+  void
+  refreshView();
+
   pcl::visualization::PCLVisualizer::Ptr vis_src_;
   pcl::visualization::PCLVisualizer::Ptr vis_dst_;
 
diff --git a/apps/include/pcl/apps/nn_classification.h b/apps/include/pcl/apps/nn_classification.h
index fcca0ec6..19bbd0ec 100644
--- a/apps/include/pcl/apps/nn_classification.h
+++ b/apps/include/pcl/apps/nn_classification.h
@@ -42,6 +42,8 @@
 #include <pcl/io/pcd_io.h>
 #include <pcl/kdtree/kdtree_flann.h>
 
+#include <cfloat> // for FLT_MAX
+
 namespace pcl {
 
 /**
@@ -191,7 +193,7 @@ public:
   ResultPtr
   classify(const PointT& p_q, double radius, float gaussian_param, int max_nn = INT_MAX)
   {
-    std::vector<int> k_indices;
+    pcl::Indices k_indices;
     std::vector<float> k_sqr_distances;
     getSimilarExemplars(p_q, radius, k_indices, k_sqr_distances, max_nn);
     return getGaussianBestScores(gaussian_param, k_indices, k_sqr_distances);
@@ -210,7 +212,7 @@ public:
   int
   getKNearestExemplars(const PointT& p_q,
                        int k,
-                       std::vector<int>& k_indices,
+                       pcl::Indices& k_indices,
                        std::vector<float>& k_sqr_distances)
   {
     k_indices.resize(k);
@@ -230,7 +232,7 @@ public:
   int
   getSimilarExemplars(const PointT& p_q,
                       double radius,
-                      std::vector<int>& k_indices,
+                      pcl::Indices& k_indices,
                       std::vector<float>& k_sqr_distances,
                       int max_nn = INT_MAX)
   {
@@ -244,17 +246,16 @@ public:
    * \return a square distance to each training class
    */
   std::shared_ptr<std::vector<float>>
-  getSmallestSquaredDistances(std::vector<int>& k_indices,
+  getSmallestSquaredDistances(pcl::Indices& k_indices,
                               std::vector<float>& k_sqr_distances)
   {
     // Reserve space for distances
     auto sqr_distances = std::make_shared<std::vector<float>>(classes_.size(), FLT_MAX);
 
     // Select square distance to each class
-    for (std::vector<int>::const_iterator i = k_indices.begin(); i != k_indices.end();
-         ++i)
-      if ((*sqr_distances)[labels_idx_[*i]] > k_sqr_distances[i - k_indices.begin()])
-        (*sqr_distances)[labels_idx_[*i]] = k_sqr_distances[i - k_indices.begin()];
+    for (auto i = k_indices.cbegin(); i != k_indices.cend(); ++i)
+      if ((*sqr_distances)[labels_idx_[*i]] > k_sqr_distances[i - k_indices.cbegin()])
+        (*sqr_distances)[labels_idx_[*i]] = k_sqr_distances[i - k_indices.cbegin()];
     return sqr_distances;
   }
 
@@ -267,7 +268,7 @@ public:
    * \return pair of label and score for each training class from the neighborhood
    */
   ResultPtr
-  getLinearBestScores(std::vector<int>& k_indices, std::vector<float>& k_sqr_distances)
+  getLinearBestScores(pcl::Indices& k_indices, std::vector<float>& k_sqr_distances)
   {
     // Get smallest squared distances and transform them to a score for each class
     auto sqr_distances = getSmallestSquaredDistances(k_indices, k_sqr_distances);
@@ -305,7 +306,7 @@ public:
    */
   ResultPtr
   getGaussianBestScores(float gaussian_param,
-                        std::vector<int>& k_indices,
+                        pcl::Indices& k_indices,
                         std::vector<float>& k_sqr_distances)
   {
     // Get smallest squared distances and transform them to a score for each class
diff --git a/apps/include/pcl/apps/openni_passthrough.h b/apps/include/pcl/apps/openni_passthrough.h
index d264865b..941a49a7 100644
--- a/apps/include/pcl/apps/openni_passthrough.h
+++ b/apps/include/pcl/apps/openni_passthrough.h
@@ -86,6 +86,9 @@ public:
   cloud_cb(const CloudConstPtr& cloud);
 
 protected:
+  void
+  refreshView();
+
   pcl::visualization::PCLVisualizer::Ptr vis_;
   pcl::OpenNIGrabber& grabber_;
   std::string device_id_;
diff --git a/apps/include/pcl/apps/organized_segmentation_demo.h b/apps/include/pcl/apps/organized_segmentation_demo.h
index f8b660d9..b83809cc 100644
--- a/apps/include/pcl/apps/organized_segmentation_demo.h
+++ b/apps/include/pcl/apps/organized_segmentation_demo.h
@@ -38,13 +38,8 @@
 
 #include <pcl/apps/organized_segmentation_demo_qt.h>
 #include <pcl/common/time.h>
-#include <pcl/common/transforms.h>
 #include <pcl/features/integral_image_normal.h>
-#include <pcl/filters/voxel_grid.h>
-#include <pcl/io/oni_grabber.h>
-#include <pcl/io/openni_grabber.h>
-#include <pcl/io/pcd_grabber.h>
-#include <pcl/io/pcd_io.h>
+#include <pcl/io/grabber.h> // for Grabber
 #include <pcl/segmentation/edge_aware_plane_comparator.h>
 #include <pcl/segmentation/euclidean_cluster_comparator.h>
 #include <pcl/segmentation/euclidean_plane_coefficient_comparator.h>
@@ -189,4 +184,8 @@ public Q_SLOTS:
 private Q_SLOTS:
   void
   timeoutSlot();
+
+private:
+  void
+  refreshView();
 };
diff --git a/apps/include/pcl/apps/pcd_video_player.h b/apps/include/pcl/apps/pcd_video_player.h
index 46a63ef3..2f5e4a19 100644
--- a/apps/include/pcl/apps/pcd_video_player.h
+++ b/apps/include/pcl/apps/pcd_video_player.h
@@ -34,14 +34,8 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <pcl/common/angles.h>
 #include <pcl/common/common.h>
 #include <pcl/common/time.h>
-#include <pcl/common/transforms.h>
-#include <pcl/console/parse.h>
-#include <pcl/console/print.h>
-#include <pcl/io/pcd_grabber.h>
-#include <pcl/io/pcd_io.h>
 #include <pcl/registration/transformation_estimation_svd.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/visualization/point_cloud_handlers.h>
@@ -53,7 +47,6 @@
 #include <QMainWindow>
 #include <QMutex>
 #include <QTimer>
-#include <ui_pcd_video_player.h>
 
 #include <ctime>
 #include <iostream>
@@ -93,6 +86,9 @@ public:
   ~PCDVideoPlayer() {}
 
 protected:
+  void
+  refreshView();
+
   pcl::visualization::PCLVisualizer::Ptr vis_;
   pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_;
 
diff --git a/apps/include/pcl/apps/render_views_tesselated_sphere.h b/apps/include/pcl/apps/render_views_tesselated_sphere.h
index c61f39f2..cc59e3b0 100644
--- a/apps/include/pcl/apps/render_views_tesselated_sphere.h
+++ b/apps/include/pcl/apps/render_views_tesselated_sphere.h
@@ -7,7 +7,8 @@
 
 #pragma once
 
-#include <pcl/common/common.h>
+#include <pcl/point_cloud.h> // for PointCloud
+#include <pcl/point_types.h> // for PointXYZ
 
 #include <vtkPolyData.h>
 #include <vtkSmartPointer.h>
diff --git a/apps/modeler/CMakeLists.txt b/apps/modeler/CMakeLists.txt
index 7b1d14b5..c6b6c978 100644
--- a/apps/modeler/CMakeLists.txt
+++ b/apps/modeler/CMakeLists.txt
@@ -1,30 +1,11 @@
 set(SUBSUBSYS_NAME modeler)
 set(SUBSUBSYS_DESC "PCLModeler: PCL based reconstruction platform")
 set(SUBSUBSYS_DEPS common geometry io filters sample_consensus segmentation visualization kdtree features surface octree registration keypoints tracking search apps)
+set(SUBSUBSYS_EXT_DEPS vtk Qt5)
 set(REASON "")
 
-# Find VTK
-if(NOT VTK_FOUND)
-  set(DEFAULT AUTO_OFF)
-  set(REASON "VTK was not found.")
-else()
-  set(DEFAULT TRUE)
-  set(REASON)
-  set(VTK_USE_FILE "${VTK_USE_FILE}" CACHE INTERNAL "VTK_USE_FILE")
-  include("${VTK_USE_FILE}")
-endif()
-
-# QT5 Found?
-if(NOT Qt5_FOUND)
-  set(DEFAULT AUTO_OFF)
-  set(REASON "Qt5 was not found.")
-elseif(NOT ${DEFAULT} STREQUAL "AUTO_OFF")
-  set(DEFAULT TRUE)
-  set(REASON)
-endif()
-
 # QVTK?
-if(NOT QVTK_FOUND)
+if(NOT HAVE_QVTK)
   set(DEFAULT AUTO_OFF)
   set(REASON "VTK was not built with Qt support.")
 elseif(NOT ${DEFAULT} STREQUAL "AUTO_OFF")
@@ -37,8 +18,8 @@ if(${DEFAULT} STREQUAL "TRUE")
   set(DEFAULT FALSE)
 endif()
 
-PCL_SUBSUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}")
-PCL_SUBSUBSYS_DEPEND(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" DEPS ${SUBSYS_DEPS} EXT_DEPS vtk)
+PCL_SUBSUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" "${SUBSUBSYS_DESC}" ${DEFAULT} "${REASON}")
+PCL_SUBSUBSYS_DEPEND(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" DEPS ${SUBSUBSYS_DEPS} EXT_DEPS ${SUBSUBSYS_EXT_DEPS})
 
 PCL_ADD_DOC("${SUBSUBSYS_NAME}")
 
@@ -53,21 +34,17 @@ set(uis
   main_window.ui
 )
 
-set(moc_incs
-  "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/main_window.h"
-  "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/scene_tree.h"
-  "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/parameter_dialog.h"
-  "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/thread_controller.h"
-  "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/abstract_worker.h"
-  "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/cloud_mesh_item_updater.h"
-)
-
 set(resources
   resources/resources.qrc
 )
 
 set(incs
-  ${moc_incs}
+   "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/main_window.h"
+  "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/scene_tree.h"
+  "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/parameter_dialog.h"
+  "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/thread_controller.h"
+  "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/abstract_worker.h"
+  "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/cloud_mesh_item_updater.h"
 
   "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/dock_widget.h"
   "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/abstract_item.h"
@@ -126,24 +103,25 @@ set(impl_incs
   "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/impl/scene_tree.hpp"
 )
 
-# Qt stuff
-QT5_WRAP_UI(ui_srcs ${uis})
-QT5_WRAP_CPP(moc_srcs ${moc_incs} OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED)
-QT5_ADD_RESOURCES(resource_srcs ${resources})
-
-# Organize files
-source_group("Resources" FILES ${uis} ${resources} ${EXE_ICON})
-source_group("Generated" FILES ${ui_srcs} ${moc_srcs} ${resource_srcs} ${RCS_SOURCES})
-set_source_files_properties(${srcs} PROPERTIES OBJECT_DEPENDS "${ui_srcs}")
+list(APPEND CMAKE_AUTOUIC_SEARCH_PATHS "${CMAKE_CURRENT_SOURCE_DIR}")
 
 # Generate executable
 set(EXE_NAME "pcl_${SUBSUBSYS_NAME}")
-PCL_ADD_EXECUTABLE(${EXE_NAME} COMPONENT ${SUBSUBSYS_NAME} SOURCES ${ui_srcs} ${moc_srcs} ${resource_srcs} ${srcs} ${incs} ${impl_incs})
-target_link_libraries("${EXE_NAME}" pcl_common pcl_io pcl_kdtree pcl_filters pcl_visualization pcl_segmentation pcl_surface pcl_features pcl_sample_consensus pcl_search Qt5::Widgets)
+PCL_ADD_EXECUTABLE(
+  ${EXE_NAME}
+  COMPONENT
+    ${SUBSUBSYS_NAME}
+  SOURCES
+    ${uis}
+    ${resources}
+    ${srcs}
+    ${incs}
+    ${impl_incs})
 
-# Put the ui in the windows project file
-if(("${CMAKE_BUILD_TOOL}" MATCHES "msdev") OR("${CMAKE_BUILD_TOOL}" MATCHES "devenv"))
-  list(APPEND srcs ${uis})
+target_link_libraries("${EXE_NAME}" pcl_common pcl_io pcl_kdtree pcl_filters pcl_visualization pcl_segmentation pcl_surface pcl_features pcl_sample_consensus pcl_search Qt5::Widgets)
+#TODO: Update when CMAKE 3.10 is available
+if(NOT (${VTK_VERSION} VERSION_LESS 9.0))
+  target_link_libraries("${EXE_NAME}" VTK::GUISupportQt)
 endif()
 
 # Install include files
diff --git a/apps/modeler/include/pcl/apps/modeler/render_window.h b/apps/modeler/include/pcl/apps/modeler/render_window.h
index b5f07a70..0413e85b 100755
--- a/apps/modeler/include/pcl/apps/modeler/render_window.h
+++ b/apps/modeler/include/pcl/apps/modeler/render_window.h
@@ -35,8 +35,7 @@
  */
 
 #pragma once
-
-#include <QVTKWidget.h>
+#include <pcl/visualization/qvtk_compatibility.h>
 
 #include <vtkSmartPointer.h>
 
@@ -47,7 +46,7 @@ namespace modeler {
 
 class RenderWindowItem;
 
-class RenderWindow : public QVTKWidget {
+class RenderWindow : public PCLQVTKWidget {
 public:
   RenderWindow(RenderWindowItem* render_window_item,
                QWidget* parent = nullptr,
diff --git a/apps/modeler/src/cloud_mesh.cpp b/apps/modeler/src/cloud_mesh.cpp
index f2ba9b82..c7936b3b 100755
--- a/apps/modeler/src/cloud_mesh.cpp
+++ b/apps/modeler/src/cloud_mesh.cpp
@@ -115,10 +115,10 @@ pcl::modeler::CloudMesh::save(const std::vector<const CloudMesh*>& cloud_meshes,
   CloudMesh cloud_mesh;
   for (const auto& mesh : cloud_meshes) {
     if (filename.rfind(".obj") == (filename.length() - 4)) {
-      std::size_t delta = cloud_mesh.cloud_->size();
+      index_t delta = cloud_mesh.cloud_->size();
       for (auto polygon : mesh->polygons_) {
-        for (unsigned int& vertice : polygon.vertices)
-          vertice += static_cast<unsigned int>(delta);
+        for (index_t& vertice : polygon.vertices)
+          vertice += delta;
         cloud_mesh.polygons_.push_back(polygon);
       }
     }
@@ -174,7 +174,7 @@ pcl::modeler::CloudMesh::updateVtkPoints()
   }
   // Need to check for NaNs, Infs, ec
   else {
-    pcl::IndicesPtr indices(new std::vector<int>());
+    pcl::IndicesPtr indices(new pcl::Indices());
     pcl::removeNaNFromPointCloud(*cloud_, *indices);
 
     data->SetNumberOfValues(3 * indices->size());
@@ -198,17 +198,17 @@ pcl::modeler::CloudMesh::updateVtkPolygons()
   if (cloud_->is_dense) {
     for (const auto& polygon : polygons_) {
       vtk_polygons_->InsertNextCell(polygon.vertices.size());
-      for (const unsigned int& vertex : polygon.vertices)
+      for (const auto& vertex : polygon.vertices)
         vtk_polygons_->InsertCellPoint(vertex);
     }
   }
   else {
-    pcl::IndicesPtr indices(new std::vector<int>());
+    pcl::IndicesPtr indices(new pcl::Indices());
     pcl::removeNaNFromPointCloud(*cloud_, *indices);
 
     for (const auto& polygon : polygons_) {
       vtk_polygons_->InsertNextCell(polygon.vertices.size());
-      for (const unsigned int& vertex : polygon.vertices)
+      for (const auto& vertex : polygon.vertices)
         vtk_polygons_->InsertCellPoint((*indices)[vertex]);
     }
   }
diff --git a/apps/modeler/src/cloud_mesh_item.cpp b/apps/modeler/src/cloud_mesh_item.cpp
index 30905030..e40ddbf5 100755
--- a/apps/modeler/src/cloud_mesh_item.cpp
+++ b/apps/modeler/src/cloud_mesh_item.cpp
@@ -156,12 +156,13 @@ void
 pcl::modeler::CloudMeshItem::createChannels()
 {
   RenderWindowItem* render_window_item = dynamic_cast<RenderWindowItem*>(parent());
-  addChild(new PointsActorItem(
-      this, cloud_mesh_, render_window_item->getRenderWindow()->GetRenderWindow()));
-  addChild(new NormalsActorItem(
-      this, cloud_mesh_, render_window_item->getRenderWindow()->GetRenderWindow()));
-  addChild(new SurfaceActorItem(
-      this, cloud_mesh_, render_window_item->getRenderWindow()->GetRenderWindow()));
+  vtkRenderWindow* win =
+      getRenderWindowCompat(*(render_window_item->getRenderWindow()));
+
+  addChild(new PointsActorItem(this, cloud_mesh_, win));
+  addChild(new NormalsActorItem(this, cloud_mesh_, win));
+  addChild(new SurfaceActorItem(this, cloud_mesh_, win));
+
   for (int i = 0, i_end = childCount(); i < i_end; ++i) {
     ChannelActorItem* child_item = dynamic_cast<ChannelActorItem*>(child(i));
     child_item->init();
@@ -242,7 +243,7 @@ pcl::modeler::CloudMeshItem::updateRenderWindow()
   for (int i = 0, i_end = childCount(); i < i_end; ++i) {
     ChannelActorItem* child_item = dynamic_cast<ChannelActorItem*>(child(i));
     child_item->switchRenderWindow(
-        render_window_item->getRenderWindow()->GetRenderWindow());
+        getRenderWindowCompat(*render_window_item->getRenderWindow()));
   }
 
   render_window_item->getRenderWindow()->updateAxes();
diff --git a/apps/modeler/src/normal_estimation_worker.cpp b/apps/modeler/src/normal_estimation_worker.cpp
index 56fce622..468d7362 100755
--- a/apps/modeler/src/normal_estimation_worker.cpp
+++ b/apps/modeler/src/normal_estimation_worker.cpp
@@ -112,7 +112,7 @@ pcl::modeler::NormalEstimationWorker::processImpl(CloudMeshItem* cloud_mesh_item
   pcl::NormalEstimation<pcl::PointSurfel, pcl::PointNormal> normal_estimator;
   normal_estimator.setInputCloud(cloud);
 
-  pcl::IndicesPtr indices(new std::vector<int>());
+  pcl::IndicesPtr indices(new pcl::Indices());
   pcl::removeNaNFromPointCloud(*cloud, *indices);
   normal_estimator.setIndices(indices);
 
diff --git a/apps/modeler/src/normals_actor_item.cpp b/apps/modeler/src/normals_actor_item.cpp
index b9141fda..6530c560 100644
--- a/apps/modeler/src/normals_actor_item.cpp
+++ b/apps/modeler/src/normals_actor_item.cpp
@@ -95,7 +95,7 @@ pcl::modeler::NormalsActorItem::createNormalLines()
     }
   }
   else {
-    pcl::IndicesPtr indices(new std::vector<int>());
+    pcl::IndicesPtr indices(new pcl::Indices());
     pcl::removeNaNFromPointCloud(*cloud, *indices);
 
     vtkIdType nr_normals = static_cast<vtkIdType>((indices->size() - 1) / level_ + 1);
diff --git a/apps/modeler/src/render_window.cpp b/apps/modeler/src/render_window.cpp
index 73c5fd06..9be04f77 100755
--- a/apps/modeler/src/render_window.cpp
+++ b/apps/modeler/src/render_window.cpp
@@ -52,7 +52,7 @@
 pcl::modeler::RenderWindow::RenderWindow(RenderWindowItem* render_window_item,
                                          QWidget* parent,
                                          Qt::WindowFlags flags)
-: QVTKWidget(parent, flags)
+: PCLQVTKWidget(parent, flags)
 , axes_(vtkSmartPointer<vtkCubeAxesActor>::New())
 , render_window_item_(render_window_item)
 {
@@ -76,7 +76,7 @@ pcl::modeler::RenderWindow::~RenderWindow()
 void
 pcl::modeler::RenderWindow::initRenderer()
 {
-  vtkSmartPointer<vtkRenderWindow> win = GetRenderWindow();
+  vtkSmartPointer<vtkRenderWindow> win = getRenderWindowCompat(*this);
   vtkSmartPointer<vtkRenderer> renderer = vtkSmartPointer<vtkRenderer>::New();
   win->AddRenderer(renderer);
 
@@ -105,7 +105,7 @@ pcl::modeler::RenderWindow::focusInEvent(QFocusEvent* event)
   dynamic_cast<SceneTree*>(render_window_item_->treeWidget())
       ->selectRenderWindowItem(render_window_item_);
 
-  QVTKWidget::focusInEvent(event);
+  PCLQVTKWidget::focusInEvent(event);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
@@ -130,7 +130,7 @@ pcl::modeler::RenderWindow::setTitle(const QString& title)
 void
 pcl::modeler::RenderWindow::render()
 {
-  GetRenderWindow()->Render();
+  getRenderWindowCompat(*this)->Render();
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
@@ -138,9 +138,11 @@ void
 pcl::modeler::RenderWindow::resetCamera()
 {
   double bounds[6];
-  GetRenderWindow()->GetRenderers()->GetFirstRenderer()->ComputeVisiblePropBounds(
-      bounds);
-  GetRenderWindow()->GetRenderers()->GetFirstRenderer()->ResetCamera(bounds);
+  getRenderWindowCompat(*this)
+      ->GetRenderers()
+      ->GetFirstRenderer()
+      ->ComputeVisiblePropBounds(bounds);
+  getRenderWindowCompat(*this)->GetRenderers()->GetFirstRenderer()->ResetCamera(bounds);
   render();
 }
 
@@ -148,14 +150,16 @@ pcl::modeler::RenderWindow::resetCamera()
 void
 pcl::modeler::RenderWindow::getBackground(double& r, double& g, double& b)
 {
-  GetRenderWindow()->GetRenderers()->GetFirstRenderer()->GetBackground(r, g, b);
+  getRenderWindowCompat(*this)->GetRenderers()->GetFirstRenderer()->GetBackground(
+      r, g, b);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
 pcl::modeler::RenderWindow::setBackground(double r, double g, double b)
 {
-  GetRenderWindow()->GetRenderers()->GetFirstRenderer()->SetBackground(r, g, b);
+  getRenderWindowCompat(*this)->GetRenderers()->GetFirstRenderer()->SetBackground(
+      r, g, b);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
@@ -165,7 +169,7 @@ pcl::modeler::RenderWindow::updateAxes()
   vtkBoundingBox bb;
 
   vtkActorCollection* actors =
-      GetRenderWindow()->GetRenderers()->GetFirstRenderer()->GetActors();
+      getRenderWindowCompat(*this)->GetRenderers()->GetFirstRenderer()->GetActors();
 
   actors->InitTraversal();
   for (int i = 0, i_end = actors->GetNumberOfItems(); i < i_end; ++i) {
@@ -181,8 +185,10 @@ pcl::modeler::RenderWindow::updateAxes()
   double bounds[6];
   bb.GetBounds(bounds);
   axes_->SetBounds(bounds);
-  axes_->SetCamera(
-      GetRenderWindow()->GetRenderers()->GetFirstRenderer()->GetActiveCamera());
+  axes_->SetCamera(getRenderWindowCompat(*this)
+                       ->GetRenderers()
+                       ->GetFirstRenderer()
+                       ->GetActiveCamera());
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
@@ -190,7 +196,8 @@ void
 pcl::modeler::RenderWindow::setShowAxes(bool flag)
 {
   if (flag)
-    GetRenderWindow()->GetRenderers()->GetFirstRenderer()->AddActor(axes_);
+    getRenderWindowCompat(*this)->GetRenderers()->GetFirstRenderer()->AddActor(axes_);
   else
-    GetRenderWindow()->GetRenderers()->GetFirstRenderer()->RemoveActor(axes_);
+    getRenderWindowCompat(*this)->GetRenderers()->GetFirstRenderer()->RemoveActor(
+        axes_);
 }
diff --git a/apps/point_cloud_editor/CMakeLists.txt b/apps/point_cloud_editor/CMakeLists.txt
index cb749b65..b09bd01f 100644
--- a/apps/point_cloud_editor/CMakeLists.txt
+++ b/apps/point_cloud_editor/CMakeLists.txt
@@ -1,25 +1,7 @@
 set(SUBSUBSYS_NAME point_cloud_editor)
 set(SUBSUBSYS_DESC "Point Cloud Editor - Simple editor for 3D point clouds")
 set(SUBSUBSYS_DEPS common filters io apps)
-
-# QT5 Found?
-if(NOT Qt5_FOUND)
-  set(DEFAULT AUTO_OFF)
-  set(REASON "Qt5 was not found.")
-elseif(NOT ${DEFAULT} STREQUAL "AUTO_OFF")
-  set(DEFAULT TRUE)
-  set(REASON)
-endif()
-
-# Find OpenGL
-if(NOT OPENGL_FOUND)
-  set(DEFAULT AUTO_OFF)
-  set(REASON "OpenGL was not found.")
-elseif(NOT ${DEFAULT} STREQUAL "AUTO_OFF")
-  set(DEFAULT TRUE)
-  set(REASON)
-endif()
-
+set(SUBSUBSYS_EXT_DEPS vtk Qt5 OpenGL)
 
 # Default to not building for now
 if(${DEFAULT} STREQUAL "TRUE")
@@ -27,25 +9,22 @@ if(${DEFAULT} STREQUAL "TRUE")
 endif()
 
 PCL_SUBSUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}")
-PCL_SUBSUBSYS_DEPEND(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" ${SUBSYS_DEPS})
+PCL_SUBSUBSYS_DEPEND(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" ${SUBSUBSYS_DEPS} EXT_DEPS ${SUBSUBSYS_EXT_DEPS})
 PCL_ADD_DOC(${SUBSUBSYS_NAME})
 
 if(NOT build)
   return()
 endif()
 
-set(MOC_INCS
-  "include/pcl/apps/${SUBSUBSYS_NAME}/cloudEditorWidget.h"
-  "include/pcl/apps/${SUBSUBSYS_NAME}/mainWindow.h"
-  "include/pcl/apps/${SUBSUBSYS_NAME}/denoiseParameterForm.h"
-  "include/pcl/apps/${SUBSUBSYS_NAME}/statisticsDialog.h"
-)
-
 set(RSRC
   resources/pceditor_resources.qrc
 )
 
-set(INCS ${MOC_INCS}
+set(INCS
+  "include/pcl/apps/${SUBSUBSYS_NAME}/cloudEditorWidget.h"
+  "include/pcl/apps/${SUBSUBSYS_NAME}/mainWindow.h"
+  "include/pcl/apps/${SUBSUBSYS_NAME}/denoiseParameterForm.h"
+  "include/pcl/apps/${SUBSUBSYS_NAME}/statisticsDialog.h"
   "include/pcl/apps/${SUBSUBSYS_NAME}/cloud.h"
   "include/pcl/apps/${SUBSUBSYS_NAME}/cloudTransformTool.h"
   "include/pcl/apps/${SUBSUBSYS_NAME}/command.h"
@@ -92,16 +71,20 @@ set(SRCS
   src/denoiseParameterForm.cpp
 )
 
-qt5_wrap_cpp(MOC_SRCS ${MOC_INCS} OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED)
-qt5_add_resources(RESOURCES_SRCS ${RSRC})
-
 include_directories(
   "${CMAKE_CURRENT_BINARY_DIR}"
   "${CMAKE_CURRENT_SOURCE_DIR}/include"
 )
 
 set(EXE_NAME "pcl_${SUBSUBSYS_NAME}")
-PCL_ADD_EXECUTABLE(${EXE_NAME} COMPONENT ${SUBSUBSYS_NAME} SOURCES ${SRCS} ${RESOURCES_SRCS} ${MOC_SRCS} ${INCS})
+PCL_ADD_EXECUTABLE(
+  ${EXE_NAME}
+  COMPONENT
+    ${SUBSUBSYS_NAME}
+  SOURCES
+    ${SRCS}
+    ${RSRC}
+    ${INCS})
 
 target_link_libraries("${EXE_NAME}" Qt5::Widgets Qt5::OpenGL ${OPENGL_LIBRARIES} ${BOOST_LIBRARIES} pcl_common pcl_io pcl_filters)
 
diff --git a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/cloud.h b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/cloud.h
index fc66674e..0557d073 100644
--- a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/cloud.h
+++ b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/cloud.h
@@ -73,7 +73,7 @@
 /// the origin.
 // XXX - handle shifting upon setting of a Cloud3D
 // XXX - add functions for retrieving an unshifted Cloud3D
-// XXX - add functions for retrieveing unshifted points by index
+// XXX - add functions for retrieving unshifted points by index
 // XXX - mark access functions below as returning shifted values
 class Cloud : public Statistics
 {
@@ -256,7 +256,7 @@ class Cloud : public Statistics
     void
     drawWithHighlightColor () const;
 
-    /// @brief Sets the axis along which the displyed points should have the
+    /// @brief Sets the axis along which the displayed points should have the
     /// color ramp applied.
     /// @param a The axis id describing which direction the ramp should be
     /// applied.
@@ -384,17 +384,17 @@ class Cloud : public Statistics
     static const float DEFAULT_POINT_DISPLAY_SIZE_;
     /// Default Highlight Point Size
     static const float DEFAULT_POINT_HIGHLIGHT_SIZE_;
-    /// Default Point Color - Red componenet
+    /// Default Point Color - Red component
     static const float DEFAULT_POINT_DISPLAY_COLOR_RED_;
-    /// Default Point Color - Green componenet
+    /// Default Point Color - Green component
     static const float DEFAULT_POINT_DISPLAY_COLOR_GREEN_;
-    /// Default Point Color - Blue componenet
+    /// Default Point Color - Blue component
     static const float DEFAULT_POINT_DISPLAY_COLOR_BLUE_;
-    /// Default Point Highlight Color - Red componenet
+    /// Default Point Highlight Color - Red component
     static const float DEFAULT_POINT_HIGHLIGHT_COLOR_RED_;
-    /// Default Point Highlight Color - Green componenet
+    /// Default Point Highlight Color - Green component
     static const float DEFAULT_POINT_HIGHLIGHT_COLOR_GREEN_;
-    /// Default Point Highlight Color - Blue componenet
+    /// Default Point Highlight Color - Blue component
     static const float DEFAULT_POINT_HIGHLIGHT_COLOR_BLUE_;
 
   private:
diff --git a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/common.h b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/common.h
index 0f3b4613..a90606aa 100644
--- a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/common.h
+++ b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/common.h
@@ -39,7 +39,7 @@
 
 #pragma once
 
-#include <sstream>
+#include <string>
 
 /// @brief Sets an array representing a 4x4 matrix to the identity
 /// @param matrix A pointer to memory representing at least MATRIX_SIZE
@@ -65,19 +65,6 @@ multMatrix(const float* left, const float* right, float* result);
 bool
 invertMatrix(const float* matrix, float* inverse);
 
-/// @brief Helper function for converting objects to strings (using operator<<)
-/// @param input The object to be converted
-/// @param result A reference to the string where the resulting string will be
-/// stored.
-template<typename T>
-void
-toString(T input, std::string &result)
-{
-  std::stringstream ss;
-  ss << input;
-  result = ss.str();
-}
-
 /// @brief Converts the passed string to lowercase in place
 /// @param s The string to be made lower.
 void
diff --git a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/selection.h b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/selection.h
index 3e744e30..7855e171 100644
--- a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/selection.h
+++ b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/selection.h
@@ -60,6 +60,11 @@ class Selection : public Statistics
         registerStats();
     }
 
+    /// @brief Copy constructor.
+    /// @param selection a const reference to a selection object whose
+    /// properties will be copied.
+    Selection (const Selection& selection) = default;
+
     /// @brief Equal operator
     /// @param selection a const reference to a selection object whose
     /// properties will be copied.
diff --git a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/transformCommand.h b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/transformCommand.h
index 19b302d9..5d799838 100644
--- a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/transformCommand.h
+++ b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/transformCommand.h
@@ -71,7 +71,7 @@ class TransformCommand : public Command
     operator= (const TransformCommand&) = delete;
 
   protected:
-    // Transforms the coorindates of the selected points according to the transform
+    // Transforms the coordinates of the selected points according to the transform
     // matrix.
     void
     execute () override;
@@ -90,7 +90,7 @@ class TransformCommand : public Command
     /// pointers to constructor params
     ConstSelectionPtr selection_ptr_;
 
-    /// a pointer poiting to the cloud
+    /// a pointer pointing to the cloud
     CloudPtr cloud_ptr_;
 
     float translate_x_, translate_y_, translate_z_;
diff --git a/apps/point_cloud_editor/src/cloud.cpp b/apps/point_cloud_editor/src/cloud.cpp
index 186f8397..61e82e58 100644
--- a/apps/point_cloud_editor/src/cloud.cpp
+++ b/apps/point_cloud_editor/src/cloud.cpp
@@ -445,10 +445,9 @@ Cloud::restore (const CopyBuffer& copy_buffer, const Selection& selection)
 std::string
 Cloud::getStat () const
 {
-  std::string title = "Total number of points: ";
-  std::string num_str;
-  ::toString(cloud_.size(), num_str);
-  return (title + num_str);
+  const std::string title = "Total number of points: ";
+  const std::string num_str = std::to_string(cloud_.size());
+  return title + num_str;
 }
 
 void
diff --git a/apps/point_cloud_editor/src/cloudEditorWidget.cpp b/apps/point_cloud_editor/src/cloudEditorWidget.cpp
index 9a297d6b..c3fcfa61 100644
--- a/apps/point_cloud_editor/src/cloudEditorWidget.cpp
+++ b/apps/point_cloud_editor/src/cloudEditorWidget.cpp
@@ -37,7 +37,6 @@
 /// @details the implementation of class CloudEditorWidget.
 /// @author  Yue Li and Matthew Hielsberg
 
-#include <cctype>
 #include <QFileDialog>
 #include <QMessageBox>
 #include <QMouseEvent>
@@ -535,7 +534,7 @@ CloudEditorWidget::loadFilePCD(const std::string &filename)
   if (pcl::io::loadPCDFile<Point3D>(filename, tmp) == -1)
     throw;
   pcl_cloud_ptr = PclCloudPtr(new Cloud3D(tmp));
-  std::vector<int> index;
+  pcl::Indices index;
   pcl::removeNaNFromPointCloud(*pcl_cloud_ptr, *pcl_cloud_ptr, index);
   Statistics::clear();
   cloud_ptr_ = CloudPtr(new Cloud(*pcl_cloud_ptr, true));
diff --git a/apps/point_cloud_editor/src/cloudTransformTool.cpp b/apps/point_cloud_editor/src/cloudTransformTool.cpp
index be754ee4..b0a493f6 100644
--- a/apps/point_cloud_editor/src/cloudTransformTool.cpp
+++ b/apps/point_cloud_editor/src/cloudTransformTool.cpp
@@ -37,8 +37,6 @@
 /// @details the implementation of class CloudTransformTool
 /// @author Yue Li and Matthew Hielsberg
 
-#include <algorithm>
-#include <cmath>
 #include <pcl/apps/point_cloud_editor/common.h>
 #include <pcl/apps/point_cloud_editor/cloudTransformTool.h>
 #include <pcl/apps/point_cloud_editor/cloud.h>
diff --git a/apps/point_cloud_editor/src/copyBuffer.cpp b/apps/point_cloud_editor/src/copyBuffer.cpp
index 2e0f921d..420ecbf4 100644
--- a/apps/point_cloud_editor/src/copyBuffer.cpp
+++ b/apps/point_cloud_editor/src/copyBuffer.cpp
@@ -74,9 +74,8 @@ std::string
 CopyBuffer::getStat () const
 {
   if (buffer_.size() == 0)
-    return ("");
-  std::string title = "The number of points copied to the clipboard: ";
-  std::string num_str;
-  ::toString(buffer_.size(), num_str);
-  return (title + num_str);
+    return "";
+  const std::string title = "The number of points copied to the clipboard: ";
+  const std::string num_str = std::to_string(buffer_.size());
+  return title + num_str;
 }
diff --git a/apps/point_cloud_editor/src/denoiseCommand.cpp b/apps/point_cloud_editor/src/denoiseCommand.cpp
index 93a0f6d8..35b7c571 100644
--- a/apps/point_cloud_editor/src/denoiseCommand.cpp
+++ b/apps/point_cloud_editor/src/denoiseCommand.cpp
@@ -37,7 +37,6 @@
 /// @details the implementation of the class DenoiseCommand
 /// @author Yue Li and Matthew Hielsberg
 
-#include <pcl/PointIndices.h>
 #include <pcl/point_types.h>
 #include <pcl/filters/statistical_outlier_removal.h>
 #include <pcl/apps/point_cloud_editor/denoiseCommand.h>
@@ -61,7 +60,7 @@ DenoiseCommand::execute ()
   filter.filter(temp_cloud);
   // back up the removed indices.
   pcl::IndicesConstPtr indices_ptr = filter.getRemovedIndices();
-  for(const int &it : *indices_ptr)
+  for(const auto &it : *indices_ptr)
     removed_indices_.addIndex(static_cast<unsigned int>(it));
   // back up the removed points.
   removed_points_.set(cloud_ptr_, removed_indices_);
diff --git a/apps/point_cloud_editor/src/mainWindow.cpp b/apps/point_cloud_editor/src/mainWindow.cpp
index 3277311f..1835a1cc 100644
--- a/apps/point_cloud_editor/src/mainWindow.cpp
+++ b/apps/point_cloud_editor/src/mainWindow.cpp
@@ -37,7 +37,6 @@
 /// @details the implementation of the MainWindow class
 /// @author Yue Li and Matthew Hielsberg
 
-#include <algorithm>
 #include <pcl/apps/point_cloud_editor/mainWindow.h>
 #include <pcl/apps/point_cloud_editor/cloudEditorWidget.h>
 #include <pcl/apps/point_cloud_editor/localTypes.h>
diff --git a/apps/point_cloud_editor/src/selection.cpp b/apps/point_cloud_editor/src/selection.cpp
index b91ecb63..070452a5 100644
--- a/apps/point_cloud_editor/src/selection.cpp
+++ b/apps/point_cloud_editor/src/selection.cpp
@@ -118,9 +118,8 @@ std::string
 Selection::getStat () const
 {
   if (selected_indices_.empty ())
-    return ("");
-  std::string title = "Total number of selected points: ";
-  std::string num_str;
-  ::toString(selected_indices_.size(), num_str);
-  return (title + num_str);
+    return "";
+  const std::string title = "Total number of selected points: ";
+  const std::string num_str = std::to_string(selected_indices_.size());
+  return title + num_str;
 }
diff --git a/apps/point_cloud_editor/src/selectionTransformTool.cpp b/apps/point_cloud_editor/src/selectionTransformTool.cpp
index aa23621f..e1e09e43 100644
--- a/apps/point_cloud_editor/src/selectionTransformTool.cpp
+++ b/apps/point_cloud_editor/src/selectionTransformTool.cpp
@@ -37,7 +37,6 @@
 /// @details the implementation of class CloudTransformTool
 /// @author Yue Li and Matthew Hielsberg
 
-#include <cmath>
 #include <pcl/apps/point_cloud_editor/selectionTransformTool.h>
 #include <pcl/apps/point_cloud_editor/cloud.h>
 #include <pcl/apps/point_cloud_editor/selection.h>
diff --git a/apps/point_cloud_editor/src/trackball.cpp b/apps/point_cloud_editor/src/trackball.cpp
index 64463059..f47732a0 100644
--- a/apps/point_cloud_editor/src/trackball.cpp
+++ b/apps/point_cloud_editor/src/trackball.cpp
@@ -38,7 +38,6 @@
 /// class has been based on
 /// @author Matthew Hielsberg
 
-#include <algorithm>
 #include <limits>
 #include <pcl/apps/point_cloud_editor/common.h>
 #include <pcl/apps/point_cloud_editor/trackball.h>
diff --git a/apps/src/face_detection/filesystem_face_detection.cpp b/apps/src/face_detection/filesystem_face_detection.cpp
index ec2deece..fcb5028b 100644
--- a/apps/src/face_detection/filesystem_face_detection.cpp
+++ b/apps/src/face_detection/filesystem_face_detection.cpp
@@ -46,8 +46,8 @@ run(pcl::RFFaceDetectorTrainer& fdrf,
 
   float rgb_m;
   bool exists_m;
-  pcl::for_each_type<FieldListM>(pcl::CopyIfFieldExists<PointInT, float>(
-      (*scene_vis)[0], "rgb", exists_m, rgb_m));
+  pcl::for_each_type<FieldListM>(
+      pcl::CopyIfFieldExists<PointInT, float>((*scene_vis)[0], "rgb", exists_m, rgb_m));
 
   std::cout << "Color exists:" << static_cast<int>(exists_m) << std::endl;
   if (exists_m) {
diff --git a/apps/src/face_detection/openni_face_detection.cpp b/apps/src/face_detection/openni_face_detection.cpp
index 39130057..3af6d659 100644
--- a/apps/src/face_detection/openni_face_detection.cpp
+++ b/apps/src/face_detection/openni_face_detection.cpp
@@ -8,7 +8,6 @@
 #include <pcl/apps/face_detection/openni_frame_source.h>
 #include <pcl/common/time.h>
 #include <pcl/console/parse.h>
-#include <pcl/features/integral_image_normal.h>
 #include <pcl/recognition/face_detection/rf_face_detector_trainer.h>
 // clang-format off
 #include <pcl/apps/face_detection/face_detection_apps_utils.h>
diff --git a/apps/src/face_detection/openni_frame_source.cpp b/apps/src/face_detection/openni_frame_source.cpp
index 07991aeb..10c3df3f 100644
--- a/apps/src/face_detection/openni_frame_source.cpp
+++ b/apps/src/face_detection/openni_frame_source.cpp
@@ -1,5 +1,4 @@
 #include <pcl/apps/face_detection/openni_frame_source.h>
-#include <pcl/io/pcd_io.h>
 #include <pcl/memory.h>
 
 namespace OpenNIFrameSource {
diff --git a/apps/src/feature_matching.cpp b/apps/src/feature_matching.cpp
index df76392c..488af48c 100644
--- a/apps/src/feature_matching.cpp
+++ b/apps/src/feature_matching.cpp
@@ -4,20 +4,19 @@
 #include <pcl/features/pfh.h>
 #include <pcl/features/pfhrgb.h>
 #include <pcl/features/shot_omp.h>
+#include <pcl/filters/extract_indices.h> // for ExtractIndices
 #include <pcl/io/pcd_io.h>
 #include <pcl/keypoints/harris_3d.h>
 #include <pcl/keypoints/sift_keypoint.h>
 #include <pcl/registration/correspondence_rejection_sample_consensus.h>
 #include <pcl/registration/icp.h>
 #include <pcl/registration/transformation_estimation_svd.h>
-#include <pcl/registration/transforms.h>
 #include <pcl/sample_consensus/method_types.h>
 #include <pcl/sample_consensus/model_types.h>
 #include <pcl/search/kdtree.h>
 #include <pcl/segmentation/extract_clusters.h>
 #include <pcl/segmentation/sac_segmentation.h>
 #include <pcl/surface/gp3.h>
-#include <pcl/surface/grid_projection.h>
 #include <pcl/surface/marching_cubes_hoppe.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/ModelCoefficients.h>
@@ -222,7 +221,7 @@ ICCVTutorial<FeatureType>::segmentation(
   extract.setNegative(true);
 
   extract.filter(*segmented);
-  std::vector<int> indices;
+  pcl::Indices indices;
   pcl::removeNaNFromPointCloud(*segmented, *segmented, indices);
   std::cout << "OK" << std::endl;
 
@@ -247,7 +246,7 @@ ICCVTutorial<FeatureType>::segmentation(
     if (cluster_indices.size() > 1)
       std::cout << " Using largest one...";
     std::cout << std::endl;
-    typename pcl::IndicesPtr indices(new std::vector<int>);
+    typename pcl::IndicesPtr indices(new pcl::Indices);
     *indices = cluster_indices[0].indices;
     extract.setInputCloud(segmented);
     extract.setIndices(indices);
@@ -326,7 +325,7 @@ ICCVTutorial<FeatureType>::findCorrespondences(
   // Find the index of the best match for each keypoint, and store it in
   // "correspondences_out"
   const int k = 1;
-  std::vector<int> k_indices(k);
+  pcl::Indices k_indices(k);
   std::vector<float> k_squared_distances(k);
   for (int i = 0; i < static_cast<int>(source->size()); ++i) {
     descriptor_kdtree.nearestKSearch(*source, i, k, k_indices, k_squared_distances);
diff --git a/apps/src/grabcut_2d.cpp b/apps/src/grabcut_2d.cpp
index ff4912e8..d383f55a 100644
--- a/apps/src/grabcut_2d.cpp
+++ b/apps/src/grabcut_2d.cpp
@@ -236,11 +236,8 @@ GrabCutHelper::display(int display_type)
     break;
 
   case 1:
-    glDrawPixels(gmm_image_->width,
-                 gmm_image_->height,
-                 GL_RGB,
-                 GL_FLOAT,
-                 &((*gmm_image_)[0]));
+    glDrawPixels(
+        gmm_image_->width, gmm_image_->height, GL_RGB, GL_FLOAT, &((*gmm_image_)[0]));
     break;
 
   case 2:
diff --git a/apps/src/manual_registration/manual_registration.cpp b/apps/src/manual_registration/manual_registration.cpp
index 9299661f..8e4c4699 100644
--- a/apps/src/manual_registration/manual_registration.cpp
+++ b/apps/src/manual_registration/manual_registration.cpp
@@ -39,13 +39,16 @@
  */
 
 #include <pcl/apps/manual_registration.h>
+#include <pcl/io/pcd_io.h> // for loadPCDFile
 
 #include <QApplication>
 #include <QEvent>
 #include <QMutexLocker>
 #include <QObject>
+#include <ui_manual_registration.h>
 
 #include <vtkCamera.h>
+#include <vtkGenericOpenGLRenderWindow.h>
 #include <vtkRenderWindow.h>
 #include <vtkRendererCollection.h>
 
@@ -66,28 +69,46 @@ ManualRegistration::ManualRegistration()
   this->setWindowTitle("PCL Manual Registration");
 
   // Set up the source window
+#if VTK_MAJOR_VERSION > 8
+  auto renderer_src = vtkSmartPointer<vtkRenderer>::New();
+  auto renderWindow_src = vtkSmartPointer<vtkGenericOpenGLRenderWindow>::New();
+  renderWindow_src->AddRenderer(renderer_src);
+  vis_src_.reset(
+      new pcl::visualization::PCLVisualizer(renderer_src, renderWindow_src, "", false));
+#else
   vis_src_.reset(new pcl::visualization::PCLVisualizer("", false));
-  ui_->qvtk_widget_src->SetRenderWindow(vis_src_->getRenderWindow());
-  vis_src_->setupInteractor(ui_->qvtk_widget_src->GetInteractor(),
-                            ui_->qvtk_widget_src->GetRenderWindow());
+#endif // VTK_MAJOR_VERSION > 8
+  setRenderWindowCompat(*(ui_->qvtk_widget_src), *(vis_src_->getRenderWindow()));
+  vis_src_->setupInteractor(getInteractorCompat(*(ui_->qvtk_widget_src)),
+                            getRenderWindowCompat(*(ui_->qvtk_widget_src)));
+
   vis_src_->getInteractorStyle()->setKeyboardModifier(
       pcl::visualization::INTERACTOR_KB_MOD_SHIFT);
-  ui_->qvtk_widget_src->update();
 
   vis_src_->registerPointPickingCallback(&ManualRegistration::SourcePointPickCallback,
                                          *this);
 
   // Set up the destination window
+#if VTK_MAJOR_VERSION > 8
+  auto renderer_dst = vtkSmartPointer<vtkRenderer>::New();
+  auto renderWindow_dst = vtkSmartPointer<vtkGenericOpenGLRenderWindow>::New();
+  renderWindow_dst->AddRenderer(renderer_dst);
+  vis_dst_.reset(
+      new pcl::visualization::PCLVisualizer(renderer_dst, renderWindow_dst, "", false));
+#else
   vis_dst_.reset(new pcl::visualization::PCLVisualizer("", false));
-  ui_->qvtk_widget_dst->SetRenderWindow(vis_dst_->getRenderWindow());
-  vis_dst_->setupInteractor(ui_->qvtk_widget_dst->GetInteractor(),
-                            ui_->qvtk_widget_dst->GetRenderWindow());
+#endif // VTK_MAJOR_VERSION > 8
+  setRenderWindowCompat(*(ui_->qvtk_widget_dst), *(vis_dst_->getRenderWindow()));
+  vis_dst_->setupInteractor(getInteractorCompat(*(ui_->qvtk_widget_dst)),
+                            getRenderWindowCompat(*(ui_->qvtk_widget_dst)));
+
   vis_dst_->getInteractorStyle()->setKeyboardModifier(
       pcl::visualization::INTERACTOR_KB_MOD_SHIFT);
-  ui_->qvtk_widget_dst->update();
 
   vis_dst_->registerPointPickingCallback(&ManualRegistration::DstPointPickCallback,
                                          *this);
+  // Render view
+  refreshView();
 
   // Connect all buttons
   connect(ui_->confirmSrcPointButton,
@@ -155,9 +176,8 @@ void
 ManualRegistration::confirmSrcPointPressed()
 {
   if (src_point_selected_) {
-    src_pc_.points.push_back(src_point_);
-    PCL_INFO("Selected %zu source points\n",
-             static_cast<std::size_t>(src_pc_.size()));
+    src_pc_.push_back(src_point_);
+    PCL_INFO("Selected %zu source points\n", static_cast<std::size_t>(src_pc_.size()));
     src_point_selected_ = false;
     src_pc_.width = src_pc_.size();
   }
@@ -170,7 +190,7 @@ void
 ManualRegistration::confirmDstPointPressed()
 {
   if (dst_point_selected_) {
-    dst_pc_.points.push_back(dst_point_);
+    dst_pc_.push_back(dst_point_);
     PCL_INFO("Selected %zu destination points\n",
              static_cast<std::size_t>(dst_pc_.size()));
     dst_point_selected_ = false;
@@ -198,8 +218,8 @@ ManualRegistration::clearPressed()
 {
   dst_point_selected_ = false;
   src_point_selected_ = false;
-  src_pc_.points.clear();
-  dst_pc_.points.clear();
+  src_pc_.clear();
+  dst_pc_.clear();
   src_pc_.height = 1;
   src_pc_.width = 0;
   dst_pc_.height = 1;
@@ -236,8 +256,8 @@ ManualRegistration::orthoChanged(int state)
         ->GetActiveCamera()
         ->SetParallelProjection(1);
   }
-  ui_->qvtk_widget_src->update();
-  ui_->qvtk_widget_dst->update();
+
+  refreshView();
 }
 
 // TODO
@@ -274,8 +294,17 @@ ManualRegistration::timeoutSlot()
     }
     cloud_dst_modified_ = false;
   }
-  ui_->qvtk_widget_src->update();
+  refreshView();
+}
+
+void
+ManualRegistration::refreshView()
+{
+#if VTK_MAJOR_VERSION > 8
+  ui_->qvtk_widget_dst->renderWindow()->Render();
+#else
   ui_->qvtk_widget_dst->update();
+#endif // VTK_MAJOR_VERSION > 8
 }
 
 void
diff --git a/apps/src/manual_registration/manual_registration.ui b/apps/src/manual_registration/manual_registration.ui
index 3728fd1f..1d4032f3 100644
--- a/apps/src/manual_registration/manual_registration.ui
+++ b/apps/src/manual_registration/manual_registration.ui
@@ -37,7 +37,7 @@
    </property>
    <layout class="QGridLayout" name="gridLayout">
     <item row="0" column="3">
-     <widget class="QVTKWidget" name="qvtk_widget_src">
+     <widget class="PCLQVTKWidget" name="qvtk_widget_src">
       <property name="sizePolicy">
        <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
         <horstretch>255</horstretch>
@@ -133,7 +133,7 @@
      </layout>
     </item>
     <item row="0" column="7">
-     <widget class="QVTKWidget" name="qvtk_widget_dst">
+     <widget class="PCLQVTKWidget" name="qvtk_widget_dst">
       <property name="sizePolicy">
        <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
         <horstretch>255</horstretch>
@@ -161,9 +161,9 @@
  </widget>
  <customwidgets>
   <customwidget>
-   <class>QVTKWidget</class>
-   <extends>QWidget</extends>
-   <header>QVTKWidget.h</header>
+   <class>PCLQVTKWidget</class>
+   <extends>QOpenGLWidget</extends>
+   <header location="global">pcl/visualization/qvtk_compatibility.h</header>
   </customwidget>
  </customwidgets>
  <resources/>
diff --git a/apps/src/multiscale_feature_persistence_example.cpp b/apps/src/multiscale_feature_persistence_example.cpp
index 372c4cf4..5ae97db8 100644
--- a/apps/src/multiscale_feature_persistence_example.cpp
+++ b/apps/src/multiscale_feature_persistence_example.cpp
@@ -108,7 +108,7 @@ main(int argc, char** argv)
   feature_persistence.setDistanceMetric(pcl::CS);
 
   PointCloud<FPFHSignature33>::Ptr output_features(new PointCloud<FPFHSignature33>());
-  auto output_indices = pcl::make_shared<std::vector<int>>();
+  auto output_indices = pcl::make_shared<pcl::Indices>();
   feature_persistence.determinePersistentFeatures(*output_features, output_indices);
 
   PCL_INFO("persistent features cloud size: %zu\n",
diff --git a/apps/src/ni_agast.cpp b/apps/src/ni_agast.cpp
index 423a229e..6793e490 100644
--- a/apps/src/ni_agast.cpp
+++ b/apps/src/ni_agast.cpp
@@ -43,7 +43,6 @@
 #include <pcl/console/parse.h>
 #include <pcl/console/print.h>
 #include <pcl/io/openni_grabber.h>
-#include <pcl/io/pcd_io.h>
 #include <pcl/keypoints/agast_2d.h>
 #include <pcl/visualization/image_viewer.h>
 #include <pcl/visualization/pcl_visualizer.h>
@@ -203,9 +202,7 @@ public:
   std::string
   getStrBool(bool state)
   {
-    std::stringstream ss;
-    ss << state;
-    return ss.str();
+    return state ? "1" : "0";
   }
 
   /////////////////////////////////////////////////////////////////////////
@@ -224,9 +221,8 @@ public:
 
     std::size_t j = 0;
     for (std::size_t i = 0; i < keypoints->size(); ++i) {
-      const PointT& pt =
-          (*cloud)(static_cast<long unsigned int>((*keypoints)[i].u),
-                   static_cast<long unsigned int>((*keypoints)[i].v));
+      const PointT& pt = (*cloud)(static_cast<long unsigned int>((*keypoints)[i].u),
+                                  static_cast<long unsigned int>((*keypoints)[i].v));
       if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z))
         continue;
 
diff --git a/apps/src/ni_brisk.cpp b/apps/src/ni_brisk.cpp
index cf5e0aa9..274cdf5a 100644
--- a/apps/src/ni_brisk.cpp
+++ b/apps/src/ni_brisk.cpp
@@ -40,14 +40,9 @@
 #define SHOW_FPS 1
 
 #include <pcl/apps/timer.h>
-#include <pcl/common/angles.h>
 #include <pcl/common/common.h>
 #include <pcl/common/time.h>
-#include <pcl/console/parse.h>
-#include <pcl/console/print.h>
-#include <pcl/filters/extract_indices.h>
 #include <pcl/io/openni_grabber.h>
-#include <pcl/io/pcd_io.h>
 #include <pcl/keypoints/brisk_2d.h>
 #include <pcl/visualization/image_viewer.h>
 #include <pcl/visualization/pcl_visualizer.h>
@@ -105,9 +100,7 @@ public:
   std::string
   getStrBool(bool state)
   {
-    std::stringstream ss;
-    ss << state;
-    return ss.str();
+    return state ? "1" : "0";
   }
 
   /////////////////////////////////////////////////////////////////////////
@@ -181,8 +174,7 @@ public:
 
     std::size_t j = 0;
     for (std::size_t i = 0; i < keypoints->size(); ++i) {
-      PointT pt =
-          bilinearInterpolation(cloud, (*keypoints)[i].x, (*keypoints)[i].y);
+      PointT pt = bilinearInterpolation(cloud, (*keypoints)[i].x, (*keypoints)[i].y);
 
       keypoints3d[j].x = pt.x;
       keypoints3d[j].y = pt.y;
diff --git a/apps/src/ni_linemod.cpp b/apps/src/ni_linemod.cpp
index f76fac30..52452ec0 100644
--- a/apps/src/ni_linemod.cpp
+++ b/apps/src/ni_linemod.cpp
@@ -41,13 +41,10 @@
 #include <pcl/common/angles.h>
 #include <pcl/common/common.h>
 #include <pcl/common/time.h>
-#include <pcl/console/parse.h>
 #include <pcl/console/print.h>
 #include <pcl/features/integral_image_normal.h>
-#include <pcl/filters/extract_indices.h>
 #include <pcl/geometry/polygon_operations.h>
 #include <pcl/io/openni_grabber.h>
-#include <pcl/io/pcd_io.h>
 #include <pcl/sample_consensus/sac_model_plane.h>
 #include <pcl/search/organized.h>
 #include <pcl/segmentation/edge_aware_plane_comparator.h>
@@ -211,7 +208,7 @@ public:
    * \param[out] object the segmented resultant object
    */
   void
-  segmentObject(int picked_idx,
+  segmentObject(pcl::index_t picked_idx,
                 const CloudConstPtr& cloud,
                 const PointIndices::Ptr& plane_indices,
                 const PointIndices::Ptr& plane_boundary_indices,
@@ -233,7 +230,7 @@ public:
       for (int p_it = 0; p_it < static_cast<int>(indices_fullset_.size()); ++p_it)
         indices_fullset_[p_it] = p_it;
     }
-    std::vector<int> indices_subset = plane_indices->indices;
+    pcl::Indices indices_subset = plane_indices->indices;
     std::sort(indices_subset.begin(), indices_subset.end());
     set_difference(indices_fullset_.begin(),
                    indices_fullset_.end(),
@@ -262,7 +259,7 @@ public:
     l.label = 0;
     PointCloud<Label>::Ptr scene(new PointCloud<Label>(cloud->width, cloud->height, l));
     // Mask the objects that we want to split into clusters
-    for (const int& index : points_above_plane->indices)
+    for (const auto& index : points_above_plane->indices)
       (*scene)[index].label = 1;
     euclidean_cluster_comparator->setLabels(scene);
 
@@ -296,7 +293,7 @@ public:
   /////////////////////////////////////////////////////////////////////////
   void
   segment(const PointT& picked_point,
-          int picked_idx,
+          pcl::index_t picked_idx,
           PlanarRegion<PointT>& region,
           PointIndices&,
           CloudPtr& object)
@@ -388,7 +385,7 @@ public:
     if (idx == -1)
       return;
 
-    std::vector<int> indices(1);
+    pcl::Indices indices(1);
     std::vector<float> distances(1);
 
     // Use mutices to make sure we get the right cloud
@@ -399,9 +396,8 @@ public:
     event.getPoint(picked_pt.x, picked_pt.y, picked_pt.z);
 
     // Add a sphere to it in the PCLVisualizer window
-    std::stringstream ss;
-    ss << "sphere_" << idx;
-    cloud_viewer_.addSphere(picked_pt, 0.01, 1.0, 0.0, 0.0, ss.str());
+    const std::string sphere_name = "sphere_" + std::to_string(idx);
+    cloud_viewer_.addSphere(picked_pt, 0.01, 1.0, 0.0, 0.0, sphere_name);
 
     // Check to see if we have access to the actual cloud data. Use the previously built
     // search object.
@@ -476,8 +472,7 @@ public:
     // Compute the min/max of the object
     PointT min_pt, max_pt;
     getMinMax3D(*object, min_pt, max_pt);
-    std::stringstream ss2;
-    ss2 << "cube_" << idx;
+    const std::string cube_name = "cube_" + std::to_string(idx);
     // Visualize the bounding box in 3D...
     cloud_viewer_.addCube(min_pt.x,
                           max_pt.x,
@@ -488,9 +483,9 @@ public:
                           0.0,
                           1.0,
                           0.0,
-                          ss2.str());
+                          cube_name);
     cloud_viewer_.setShapeRenderingProperties(
-        visualization::PCL_VISUALIZER_LINE_WIDTH, 10, ss2.str());
+        visualization::PCL_VISUALIZER_LINE_WIDTH, 10, cube_name);
 
     // ...and 2D
     image_viewer_.addRectangle(search_.getInputCloud(), *object);
@@ -574,7 +569,7 @@ private:
   bool first_frame_;
 
   // Segmentation
-  std::vector<int> indices_fullset_;
+  pcl::Indices indices_fullset_;
   PointIndices::Ptr plane_indices_;
   CloudPtr plane_;
   IntegralImageNormalEstimation<PointT, Normal> ne_;
diff --git a/apps/src/ni_susan.cpp b/apps/src/ni_susan.cpp
index f9680a13..d67eb256 100644
--- a/apps/src/ni_susan.cpp
+++ b/apps/src/ni_susan.cpp
@@ -40,10 +40,8 @@
 #define SHOW_FPS 1
 
 #include <pcl/apps/timer.h>
-#include <pcl/common/angles.h>
 #include <pcl/common/common.h>
 #include <pcl/io/openni_grabber.h>
-#include <pcl/io/pcd_io.h>
 #include <pcl/keypoints/susan.h>
 #include <pcl/visualization/image_viewer.h>
 #include <pcl/visualization/pcl_visualizer.h>
@@ -100,9 +98,7 @@ public:
   std::string
   getStrBool(bool state)
   {
-    std::stringstream ss;
-    ss << state;
-    return ss.str();
+    return state ? "1" : "0";
   }
 
   /////////////////////////////////////////////////////////////////////////
diff --git a/apps/src/ni_trajkovic.cpp b/apps/src/ni_trajkovic.cpp
index 88f57e10..b418e72c 100644
--- a/apps/src/ni_trajkovic.cpp
+++ b/apps/src/ni_trajkovic.cpp
@@ -40,7 +40,6 @@
 #define SHOW_FPS 1
 
 #include <pcl/apps/timer.h>
-#include <pcl/common/common.h>
 #include <pcl/console/parse.h>
 #include <pcl/console/print.h>
 #include <pcl/io/openni_grabber.h>
@@ -170,7 +169,7 @@ public:
           image_viewer_.removeLayer(getStrBool(keypts));
           std::vector<int> uv;
           uv.reserve(keypoints_indices_->indices.size() * 2);
-          for (const int& index : keypoints_indices_->indices) {
+          for (const auto& index : keypoints_indices_->indices) {
             int u(index % cloud->width);
             int v(index / cloud->width);
             image_viewer_.markPoint(u,
diff --git a/apps/src/nn_classification_example.cpp b/apps/src/nn_classification_example.cpp
index 7580c1e6..79800b66 100644
--- a/apps/src/nn_classification_example.cpp
+++ b/apps/src/nn_classification_example.cpp
@@ -37,7 +37,6 @@
 
 #include <pcl/apps/vfh_nn_classifier.h>
 #include <pcl/io/pcd_io.h>
-#include <pcl/point_types.h>
 
 int
 main(int, char* argv[])
diff --git a/apps/src/openni_3d_concave_hull.cpp b/apps/src/openni_3d_concave_hull.cpp
index 7371852b..d42bf142 100644
--- a/apps/src/openni_3d_concave_hull.cpp
+++ b/apps/src/openni_3d_concave_hull.cpp
@@ -37,7 +37,6 @@
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/io/openni_camera/openni_driver.h>
 #include <pcl/io/openni_grabber.h>
-#include <pcl/io/pcd_io.h>
 #include <pcl/surface/concave_hull.h>
 #include <pcl/visualization/cloud_viewer.h>
 #include <pcl/point_cloud.h>
diff --git a/apps/src/openni_3d_convex_hull.cpp b/apps/src/openni_3d_convex_hull.cpp
index 75b5b7ec..45957b2a 100644
--- a/apps/src/openni_3d_convex_hull.cpp
+++ b/apps/src/openni_3d_convex_hull.cpp
@@ -37,7 +37,6 @@
 #include <pcl/filters/passthrough.h>
 #include <pcl/io/openni_camera/openni_driver.h>
 #include <pcl/io/openni_grabber.h>
-#include <pcl/io/pcd_io.h>
 #include <pcl/surface/convex_hull.h>
 #include <pcl/visualization/cloud_viewer.h>
 #include <pcl/point_cloud.h>
diff --git a/apps/src/openni_boundary_estimation.cpp b/apps/src/openni_boundary_estimation.cpp
index 52e5c231..cc93ef47 100644
--- a/apps/src/openni_boundary_estimation.cpp
+++ b/apps/src/openni_boundary_estimation.cpp
@@ -36,13 +36,11 @@
  */
 
 #include <pcl/common/time.h>
-#include <pcl/console/parse.h>
 #include <pcl/features/boundary.h>
 #include <pcl/features/integral_image_normal.h>
 #include <pcl/filters/approximate_voxel_grid.h>
 #include <pcl/io/openni_camera/openni_driver.h>
 #include <pcl/io/openni_grabber.h>
-#include <pcl/io/pcd_io.h>
 #include <pcl/visualization/cloud_viewer.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
diff --git a/apps/src/openni_change_viewer.cpp b/apps/src/openni_change_viewer.cpp
index 68616234..b07a4e86 100644
--- a/apps/src/openni_change_viewer.cpp
+++ b/apps/src/openni_change_viewer.cpp
@@ -70,7 +70,7 @@ public:
     octree->addPointsFromInputCloud();
 
     std::cerr << octree->getLeafCount() << " -- ";
-    std::vector<int> newPointIdxVector;
+    pcl::Indices newPointIdxVector;
 
     // get a vector of new points, which did not exist in previous buffer
     octree->getPointIndicesFromNewVoxels(newPointIdxVector, noise_filter_);
@@ -84,7 +84,7 @@ public:
       filtered_cloud.reset(new pcl::PointCloud<pcl::PointXYZRGBA>(*cloud));
       filtered_cloud->points.reserve(newPointIdxVector.size());
 
-      for (const int& idx : newPointIdxVector)
+      for (const auto& idx : newPointIdxVector)
         (*filtered_cloud)[idx].rgba = 255 << 16;
 
       if (!viewer.wasStopped())
@@ -96,7 +96,7 @@ public:
 
       filtered_cloud->points.reserve(newPointIdxVector.size());
 
-      for (const int& idx : newPointIdxVector)
+      for (const auto& idx : newPointIdxVector)
         filtered_cloud->points.push_back((*cloud)[idx]);
 
       if (!viewer.wasStopped())
diff --git a/apps/src/openni_fast_mesh.cpp b/apps/src/openni_fast_mesh.cpp
index 47ff83bf..05efbe6c 100644
--- a/apps/src/openni_fast_mesh.cpp
+++ b/apps/src/openni_fast_mesh.cpp
@@ -36,7 +36,6 @@
 #include <pcl/common/time.h>
 #include <pcl/io/openni_camera/openni_driver.h>
 #include <pcl/io/openni_grabber.h>
-#include <pcl/io/pcd_io.h>
 #include <pcl/surface/organized_fast_mesh.h>
 #include <pcl/visualization/pcl_visualizer.h> // for PCLVisualizer
 #include <pcl/point_cloud.h>
diff --git a/apps/src/openni_feature_persistence.cpp b/apps/src/openni_feature_persistence.cpp
index 039c3a09..17ac9195 100644
--- a/apps/src/openni_feature_persistence.cpp
+++ b/apps/src/openni_feature_persistence.cpp
@@ -44,7 +44,6 @@
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/io/openni_camera/openni_driver.h>
 #include <pcl/io/openni_grabber.h>
-#include <pcl/io/pcd_io.h>
 #include <pcl/visualization/cloud_viewer.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -132,7 +131,7 @@ public:
     cloud_subsampled_.reset(new typename pcl::PointCloud<PointType>());
     normals_.reset(new pcl::PointCloud<pcl::Normal>());
     features_.reset(new pcl::PointCloud<pcl::FPFHSignature33>());
-    feature_indices_.reset(new std::vector<int>());
+    feature_indices_.reset(new pcl::Indices());
     feature_locations_.reset(new typename pcl::PointCloud<PointType>());
 
     // Subsample input cloud
@@ -153,7 +152,8 @@ public:
     extract_indices_filter_.setIndices(feature_indices_);
     extract_indices_filter_.filter(*feature_locations_);
 
-    PCL_INFO("Persistent feature locations %zu\n", static_cast<std::size_t>(feature_locations_->size()));
+    PCL_INFO("Persistent feature locations %zu\n",
+             static_cast<std::size_t>(feature_locations_->size()));
 
     cloud_ = cloud;
 
diff --git a/apps/src/openni_grab_frame.cpp b/apps/src/openni_grab_frame.cpp
index 61489b25..f61ff619 100644
--- a/apps/src/openni_grab_frame.cpp
+++ b/apps/src/openni_grab_frame.cpp
@@ -157,23 +157,21 @@ public:
   saveCloud()
   {
     FPS_CALC("I/O");
-    std::stringstream ss;
-    ss << dir_name_ << "/" << file_name_ << "_"
-       << boost::posix_time::to_iso_string(
-              boost::posix_time::microsec_clock::local_time())
-       << ".pcd";
+    const std::string time = boost::posix_time::to_iso_string(
+        boost::posix_time::microsec_clock::local_time());
+    const std::string filepath = dir_name_ + '/' + file_name_ + '_' + time + ".pcd";
 
     if (format_ & 1) {
-      writer_.writeBinary<PointType>(ss.str(), *cloud_);
+      writer_.writeBinary<PointType>(filepath, *cloud_);
       // std::cerr << "Data saved in BINARY format to " << ss.str () << std::endl;
     }
 
     if (format_ & 2) {
-      writer_.writeBinaryCompressed<PointType>(ss.str(), *cloud_);
+      writer_.writeBinaryCompressed<PointType>(filepath, *cloud_);
     }
 
     if (format_ & 4) {
-      writer_.writeBinaryCompressed<PointType>(ss.str(), *cloud_);
+      writer_.writeBinaryCompressed<PointType>(filepath, *cloud_);
     }
   }
 
diff --git a/apps/src/openni_grab_images.cpp b/apps/src/openni_grab_images.cpp
index 94412ab5..7dacf080 100644
--- a/apps/src/openni_grab_images.cpp
+++ b/apps/src/openni_grab_images.cpp
@@ -149,19 +149,17 @@ public:
           0, image->getWidth() - 1, 0, image->getHeight() - 1, 0, 0);
       importer_->SetDataExtentToWholeExtent();
 
-      std::stringstream ss;
-      ss << "frame_" + time + "_rgb.tiff";
+      const std::string rgb_frame_filename = "frame_" + time + "_rgb.tiff";
       importer_->SetImportVoidPointer(const_cast<void*>(data), 1);
       importer_->Update();
       flipper_->SetInputConnection(importer_->GetOutputPort());
       flipper_->Update();
-      writer_->SetFileName(ss.str().c_str());
+      writer_->SetFileName(rgb_frame_filename.c_str());
       writer_->SetInputConnection(flipper_->GetOutputPort());
       writer_->Write();
     }
     if (depth_image) {
-      std::stringstream ss;
-      ss << "frame_" + time + "_depth.tiff";
+      const std::string depth_frame_filename = "frame_" + time + "_depth.tiff";
 
       depth_importer_->SetWholeExtent(
           0, depth_image->getWidth() - 1, 0, depth_image->getHeight() - 1, 0, 0);
@@ -173,7 +171,7 @@ public:
       depth_importer_->Update();
       flipper_->SetInputConnection(depth_importer_->GetOutputPort());
       flipper_->Update();
-      writer_->SetFileName(ss.str().c_str());
+      writer_->SetFileName(depth_frame_filename.c_str());
       writer_->SetInputConnection(flipper_->GetOutputPort());
       writer_->Write();
     }
@@ -239,16 +237,15 @@ public:
               0, image->getWidth() - 1, 0, image->getHeight() - 1, 0, 0);
           importer_->SetDataExtentToWholeExtent();
 
-          std::stringstream ss;
-          ss << "frame_" + time + "_rgb.tiff";
+          const std::string rgb_frame_filename = "frame_" + time + "_rgb.tiff";
           importer_->SetImportVoidPointer(const_cast<void*>(data), 1);
           importer_->Update();
           flipper_->SetInputConnection(importer_->GetOutputPort());
           flipper_->Update();
-          writer_->SetFileName(ss.str().c_str());
+          writer_->SetFileName(rgb_frame_filename.c_str());
           writer_->SetInputConnection(flipper_->GetOutputPort());
           writer_->Write();
-          std::cout << "writing rgb frame: " << ss.str() << std::endl;
+          std::cout << "writing rgb frame: " << rgb_frame_filename << std::endl;
         }
       }
 
@@ -267,8 +264,7 @@ public:
         depth_image_viewer_.addRGBImage(
             depth_data, depth_image->getWidth(), depth_image->getHeight());
         if (continuous_ || trigger_) {
-          std::stringstream ss;
-          ss << "frame_" + time + "_depth.tiff";
+          const std::string depth_frame_filename = "frame_" + time + "_depth.tiff";
 
           depth_importer_->SetWholeExtent(
               0, depth_image->getWidth() - 1, 0, depth_image->getHeight() - 1, 0, 0);
@@ -280,10 +276,10 @@ public:
           depth_importer_->Update();
           flipper_->SetInputConnection(depth_importer_->GetOutputPort());
           flipper_->Update();
-          writer_->SetFileName(ss.str().c_str());
+          writer_->SetFileName(depth_frame_filename.c_str());
           writer_->SetInputConnection(flipper_->GetOutputPort());
           writer_->Write();
-          std::cout << "writing depth frame: " << ss.str() << std::endl;
+          std::cout << "writing depth frame: " << depth_frame_filename << std::endl;
         }
       }
       trigger_ = false;
diff --git a/apps/src/openni_ii_normal_estimation.cpp b/apps/src/openni_ii_normal_estimation.cpp
index 6d59fa55..1644b26b 100644
--- a/apps/src/openni_ii_normal_estimation.cpp
+++ b/apps/src/openni_ii_normal_estimation.cpp
@@ -34,11 +34,9 @@
  */
 
 #include <pcl/common/time.h>
-#include <pcl/console/parse.h>
 #include <pcl/features/integral_image_normal.h>
 #include <pcl/io/openni_camera/openni_driver.h>
 #include <pcl/io/openni_grabber.h>
-#include <pcl/io/pcd_io.h>
 #include <pcl/visualization/cloud_viewer.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
diff --git a/apps/src/openni_klt.cpp b/apps/src/openni_klt.cpp
index ff17a983..b553ef59 100644
--- a/apps/src/openni_klt.cpp
+++ b/apps/src/openni_klt.cpp
@@ -41,9 +41,10 @@
 #include <pcl/io/pcd_io.h>
 #include <pcl/keypoints/harris_2d.h>
 #include <pcl/tracking/pyramidal_klt.h>
-#include <pcl/visualization/boost.h>
 #include <pcl/visualization/image_viewer.h>
 
+#include <boost/date_time/posix_time/posix_time.hpp> // for to_iso_string, local_time
+
 #include <mutex>
 
 #define SHOW_FPS 1
@@ -251,14 +252,14 @@ public:
       if (tracker_->getInitialized() && cloud_) {
         if (points_mutex_.try_lock()) {
           keypoints_ = tracker_->getTrackedPoints();
-          points_status_ = tracker_->getPointsToTrackStatus();
+          points_status_ = tracker_->getStatusOfPointsToTrack();
           points_mutex_.unlock();
         }
 
         std::vector<float> markers;
         markers.reserve(keypoints_->size() * 2);
         for (std::size_t i = 0; i < keypoints_->size(); ++i) {
-          if (points_status_->indices[i] < 0)
+          if ((*points_status_)[i] < 0)
             continue;
           const pcl::PointUV& uv = (*keypoints_)[i];
           markers.push_back(uv.u);
@@ -295,7 +296,7 @@ public:
   typename pcl::tracking::PyramidalKLTTracker<PointType>::Ptr tracker_;
   pcl::PointCloud<pcl::PointUV>::ConstPtr keypoints_;
   pcl::PointIndicesConstPtr points_;
-  pcl::PointIndicesConstPtr points_status_;
+  pcl::shared_ptr<const std::vector<int>> points_status_;
   int counter_;
 };
 
diff --git a/apps/src/openni_mls_smoothing.cpp b/apps/src/openni_mls_smoothing.cpp
index 34f51865..c451e11b 100644
--- a/apps/src/openni_mls_smoothing.cpp
+++ b/apps/src/openni_mls_smoothing.cpp
@@ -37,11 +37,12 @@
 #include <pcl/console/parse.h>
 #include <pcl/io/openni_camera/openni_driver.h>
 #include <pcl/io/openni_grabber.h>
+#include <pcl/search/kdtree.h> // for KdTree
 #include <pcl/surface/mls.h>
-#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/visualization/keyboard_event.h> // for KeyboardEvent
+#include <pcl/visualization/pcl_visualizer.h> // for PCLVisualizer
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
-#include <pcl/search/kdtree.h> // for KdTree
 
 #include <mutex>
 
diff --git a/apps/src/openni_mobile_server.cpp b/apps/src/openni_mobile_server.cpp
index 3b59005d..b663d91c 100644
--- a/apps/src/openni_mobile_server.cpp
+++ b/apps/src/openni_mobile_server.cpp
@@ -36,7 +36,6 @@
 #include <pcl/common/time.h>
 #include <pcl/console/parse.h>
 #include <pcl/filters/voxel_grid.h>
-#include <pcl/io/openni_camera/openni_driver.h>
 #include <pcl/io/openni_grabber.h>
 #include <pcl/visualization/cloud_viewer.h>
 #include <pcl/point_cloud.h>
diff --git a/apps/src/openni_octree_compression.cpp b/apps/src/openni_octree_compression.cpp
index 3ded13cf..7203f1b2 100644
--- a/apps/src/openni_octree_compression.cpp
+++ b/apps/src/openni_octree_compression.cpp
@@ -382,8 +382,9 @@ main(int argc, char** argv)
   if (!bServerFileMode) {
     if (bEnDecode) {
       // ENCODING
-      ofstream compressedPCFile;
-      compressedPCFile.open(fileName.c_str(), std::ios::out | std::ios::trunc | std::ios::binary);
+      std::ofstream compressedPCFile;
+      compressedPCFile.open(fileName.c_str(),
+                            std::ios::out | std::ios::trunc | std::ios::binary);
 
       if (!bShowInputCloud) {
         EventHelper v(compressedPCFile, octreeCoder, field_name, min_v, max_v);
diff --git a/apps/src/openni_organized_compression.cpp b/apps/src/openni_organized_compression.cpp
index 569bd3bc..ae855521 100644
--- a/apps/src/openni_organized_compression.cpp
+++ b/apps/src/openni_organized_compression.cpp
@@ -396,7 +396,8 @@ main(int argc, char** argv)
     if (bEnDecode) {
       // ENCODING
       std::ofstream compressedPCFile;
-      compressedPCFile.open(fileName.c_str(), std::ios::out | std::ios::trunc | std::ios::binary);
+      compressedPCFile.open(fileName.c_str(),
+                            std::ios::out | std::ios::trunc | std::ios::binary);
 
       if (!bShowInputCloud) {
         EventHelper v(compressedPCFile,
diff --git a/apps/src/openni_organized_edge_detection.cpp b/apps/src/openni_organized_edge_detection.cpp
index b06f8c2e..2f4d07cb 100644
--- a/apps/src/openni_organized_edge_detection.cpp
+++ b/apps/src/openni_organized_edge_detection.cpp
@@ -36,7 +36,6 @@
 #include <pcl/common/time.h>
 #include <pcl/features/integral_image_normal.h>
 #include <pcl/features/organized_edge_detection.h>
-#include <pcl/io/io.h>
 #include <pcl/io/openni_grabber.h>
 #include <pcl/visualization/pcl_visualizer.h>
 
diff --git a/apps/src/openni_organized_multi_plane_segmentation.cpp b/apps/src/openni_organized_multi_plane_segmentation.cpp
index a2a2ad03..91030eab 100644
--- a/apps/src/openni_organized_multi_plane_segmentation.cpp
+++ b/apps/src/openni_organized_multi_plane_segmentation.cpp
@@ -35,16 +35,12 @@
 
 #include <pcl/common/time.h>
 #include <pcl/features/integral_image_normal.h>
-#include <pcl/features/normal_3d.h>
-#include <pcl/filters/extract_indices.h>
-#include <pcl/io/io.h>
 #include <pcl/io/openni_grabber.h>
 #include <pcl/segmentation/organized_connected_component_segmentation.h>
 #include <pcl/segmentation/organized_multi_plane_segmentation.h>
 #include <pcl/segmentation/planar_region.h>
 #include <pcl/visualization/cloud_viewer.h>
 #include <pcl/visualization/pcl_visualizer.h>
-#include <pcl/ModelCoefficients.h>
 
 #include <mutex>
 
diff --git a/apps/src/openni_passthrough.cpp b/apps/src/openni_passthrough.cpp
index 3fbf00cc..b516e68e 100644
--- a/apps/src/openni_passthrough.cpp
+++ b/apps/src/openni_passthrough.cpp
@@ -36,14 +36,16 @@
  */
 
 #include <pcl/apps/openni_passthrough.h>
-#include <pcl/console/parse.h>
 
 #include <QApplication>
 #include <QEvent>
 #include <QMutexLocker>
 #include <QObject>
+#include <ui_openni_passthrough.h>
 
+#include <vtkGenericOpenGLRenderWindow.h>
 #include <vtkRenderWindow.h>
+#include <vtkRendererCollection.h>
 
 #include <thread>
 
@@ -61,13 +63,23 @@ OpenNIPassthrough::OpenNIPassthrough(pcl::OpenNIGrabber& grabber)
   ui_->setupUi(this);
 
   this->setWindowTitle("PCL OpenNI PassThrough Viewer");
+  // Create the QVTKWidget
+#if VTK_MAJOR_VERSION > 8
+  auto renderer = vtkSmartPointer<vtkRenderer>::New();
+  auto renderWindow = vtkSmartPointer<vtkGenericOpenGLRenderWindow>::New();
+  renderWindow->AddRenderer(renderer);
+  vis_.reset(new pcl::visualization::PCLVisualizer(renderer, renderWindow, "", false));
+#else
   vis_.reset(new pcl::visualization::PCLVisualizer("", false));
-  ui_->qvtk_widget->SetRenderWindow(vis_->getRenderWindow());
-  vis_->setupInteractor(ui_->qvtk_widget->GetInteractor(),
-                        ui_->qvtk_widget->GetRenderWindow());
+#endif // VTK_MAJOR_VERSION > 8
+  setRenderWindowCompat(*(ui_->qvtk_widget), *(vis_->getRenderWindow()));
+  vis_->setupInteractor(getInteractorCompat(*(ui_->qvtk_widget)),
+                        getRenderWindowCompat(*(ui_->qvtk_widget)));
+
   vis_->getInteractorStyle()->setKeyboardModifier(
       pcl::visualization::INTERACTOR_KB_MOD_SHIFT);
-  ui_->qvtk_widget->update();
+
+  refreshView();
 
   // Start the OpenNI data acquision
   std::function<void(const CloudConstPtr&)> f = [this](const CloudConstPtr& cloud) {
@@ -125,6 +137,16 @@ OpenNIPassthrough::timeoutSlot()
   ui_->qvtk_widget->update();
 }
 
+void
+OpenNIPassthrough::refreshView()
+{
+#if VTK_MAJOR_VERSION > 8
+  ui_->qvtk_widget->renderWindow()->Render();
+#else
+  ui_->qvtk_widget->update();
+#endif // VTK_MAJOR_VERSION > 8
+}
+
 int
 main(int argc, char** argv)
 {
diff --git a/apps/src/openni_passthrough.ui b/apps/src/openni_passthrough.ui
index 47035fd2..39d29a2b 100644
--- a/apps/src/openni_passthrough.ui
+++ b/apps/src/openni_passthrough.ui
@@ -37,7 +37,7 @@
    </property>
    <layout class="QGridLayout" name="gridLayout">
     <item row="0" column="0">
-     <widget class="QVTKWidget" name="qvtk_widget">
+     <widget class="PCLQVTKWidget" name="qvtk_widget">
       <property name="sizePolicy">
        <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
         <horstretch>255</horstretch>
@@ -94,9 +94,9 @@
  </widget>
  <customwidgets>
   <customwidget>
-   <class>QVTKWidget</class>
-   <extends>QWidget</extends>
-   <header>QVTKWidget.h</header>
+   <class>PCLQVTKWidget</class>
+   <extends>QOpenGLWidget</extends>
+   <header location="global">pcl/visualization/qvtk_compatibility.h</header>
   </customwidget>
  </customwidgets>
  <resources>
diff --git a/apps/src/openni_shift_to_depth_conversion.cpp b/apps/src/openni_shift_to_depth_conversion.cpp
index a53c21ce..292a06ef 100644
--- a/apps/src/openni_shift_to_depth_conversion.cpp
+++ b/apps/src/openni_shift_to_depth_conversion.cpp
@@ -35,7 +35,6 @@
  */
 
 #include <pcl/common/time.h>
-#include <pcl/console/parse.h>
 #include <pcl/io/openni_grabber.h>
 #include <pcl/visualization/cloud_viewer.h>
 #include <pcl/point_cloud.h>
@@ -43,11 +42,7 @@
 
 #include <boost/asio.hpp>
 
-#include <cstdio>
-#include <cstdlib>
 #include <iostream>
-#include <sstream>
-#include <string>
 #include <thread>
 #include <vector>
 
@@ -156,8 +151,8 @@ protected:
     std::size_t cloud_size = width_arg * height_arg;
 
     // Reset point cloud
-    cloud_arg.points.clear();
-    cloud_arg.points.reserve(cloud_size);
+    cloud_arg.clear();
+    cloud_arg.reserve(cloud_size);
 
     // Define point cloud parameters
     cloud_arg.width = static_cast<std::uint32_t>(width_arg);
@@ -198,7 +193,7 @@ protected:
         }
 
         // Add point to cloud
-        cloud_arg.points.push_back(newPoint);
+        cloud_arg.push_back(newPoint);
         // Increment point iterator
         ++i;
       }
diff --git a/apps/src/openni_tracking.cpp b/apps/src/openni_tracking.cpp
index 2ca8ba7a..ccc6377a 100644
--- a/apps/src/openni_tracking.cpp
+++ b/apps/src/openni_tracking.cpp
@@ -39,8 +39,6 @@
 #include <pcl/common/time.h>
 #include <pcl/common/transforms.h>
 #include <pcl/console/parse.h>
-#include <pcl/features/integral_image_normal.h>
-#include <pcl/features/normal_3d.h>
 #include <pcl/features/normal_3d_omp.h>
 #include <pcl/filters/approximate_voxel_grid.h>
 #include <pcl/filters/extract_indices.h>
@@ -48,10 +46,8 @@
 #include <pcl/filters/project_inliers.h>
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/io/openni_grabber.h>
-#include <pcl/io/pcd_io.h>
 #include <pcl/sample_consensus/method_types.h>
 #include <pcl/sample_consensus/model_types.h>
-#include <pcl/search/pcl_search.h>
 #include <pcl/segmentation/extract_clusters.h>
 #include <pcl/segmentation/extract_polygonal_prism_data.h>
 #include <pcl/segmentation/sac_segmentation.h>
@@ -439,13 +435,13 @@ public:
     result.width = cloud->width;
     result.height = cloud->height;
     result.is_dense = cloud->is_dense;
-    for (std::size_t i = 0; i < cloud->size(); i++) {
+    for (const auto& pt : *cloud) {
       RefPointType point;
-      point.x = (*cloud)[i].x;
-      point.y = (*cloud)[i].y;
-      point.z = (*cloud)[i].z;
-      point.rgba = (*cloud)[i].rgba;
-      result.points.push_back(point);
+      point.x = pt.x;
+      point.y = pt.y;
+      point.z = pt.z;
+      point.rgba = pt.rgba;
+      result.push_back(point);
     }
   }
 
@@ -472,11 +468,11 @@ public:
   void
   removeZeroPoints(const CloudConstPtr& cloud, Cloud& result)
   {
-    for (const auto& point: *cloud) {
+    for (const auto& point : *cloud) {
       if (!(std::abs(point.x) < 0.01 && std::abs(point.y) < 0.01 &&
             std::abs(point.z) < 0.01) &&
           !std::isnan(point.x) && !std::isnan(point.y) && !std::isnan(point.z))
-        result.points.push_back(point);
+        result.push_back(point);
     }
 
     result.width = result.size();
@@ -491,9 +487,9 @@ public:
                         Cloud& result)
   {
     pcl::PointIndices segmented_indices = cluster_indices[segment_index];
-    for (const int& index : segmented_indices.indices) {
+    for (const auto& index : segmented_indices.indices) {
       PointType point = (*cloud)[index];
-      result.points.push_back(point);
+      result.push_back(point);
     }
     result.width = result.size();
     result.height = 1;
diff --git a/apps/src/openni_voxel_grid.cpp b/apps/src/openni_voxel_grid.cpp
index e1c3f947..b51395fc 100644
--- a/apps/src/openni_voxel_grid.cpp
+++ b/apps/src/openni_voxel_grid.cpp
@@ -35,7 +35,6 @@
 
 #include <pcl/common/time.h>
 #include <pcl/console/parse.h>
-#include <pcl/filters/approximate_voxel_grid.h>
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/io/openni_camera/openni_driver.h>
 #include <pcl/io/openni_grabber.h>
diff --git a/apps/src/organized_segmentation_demo.cpp b/apps/src/organized_segmentation_demo.cpp
index b7c01dc3..e4ce5619 100644
--- a/apps/src/organized_segmentation_demo.cpp
+++ b/apps/src/organized_segmentation_demo.cpp
@@ -1,7 +1,7 @@
 #include <pcl/apps/organized_segmentation_demo.h>
 #include <pcl/common/angles.h>
+#include <pcl/io/openni_grabber.h> // for OpenNIGrabber
 #include <pcl/segmentation/extract_polygonal_prism_data.h>
-#include <pcl/surface/convex_hull.h>
 #include <pcl/memory.h> // for pcl::dynamic_pointer_cast
 
 #include <boost/signals2/connection.hpp> // for boost::signals2::connection
@@ -11,7 +11,9 @@
 #include <QMutexLocker>
 #include <QObject>
 
+#include <vtkGenericOpenGLRenderWindow.h>
 #include <vtkRenderWindow.h>
+#include <vtkRendererCollection.h>
 
 // #include <boost/filesystem.hpp>  // for boost::filesystem::directory_iterator
 #include <boost/signals2/connection.hpp> // for boost::signals2::connection
@@ -204,13 +206,23 @@ OrganizedSegmentationDemo::OrganizedSegmentationDemo(pcl::Grabber& grabber)
   ui_->setupUi(this);
 
   this->setWindowTitle("PCL Organized Connected Component Segmentation Demo");
+
+#if VTK_MAJOR_VERSION > 8
+  auto renderer = vtkSmartPointer<vtkRenderer>::New();
+  auto renderWindow = vtkSmartPointer<vtkGenericOpenGLRenderWindow>::New();
+  renderWindow->AddRenderer(renderer);
+  vis_.reset(new pcl::visualization::PCLVisualizer(renderer, renderWindow, "", false));
+#else
   vis_.reset(new pcl::visualization::PCLVisualizer("", false));
-  ui_->qvtk_widget->SetRenderWindow(vis_->getRenderWindow());
-  vis_->setupInteractor(ui_->qvtk_widget->GetInteractor(),
-                        ui_->qvtk_widget->GetRenderWindow());
+#endif // VTK_MAJOR_VERSION > 8
+  setRenderWindowCompat(*(ui_->qvtk_widget), *(vis_->getRenderWindow()));
+  vis_->setupInteractor(getInteractorCompat(*(ui_->qvtk_widget)),
+                        getRenderWindowCompat(*(ui_->qvtk_widget)));
+
+  refreshView();
+
   vis_->getInteractorStyle()->setKeyboardModifier(
       pcl::visualization::INTERACTOR_KB_MOD_SHIFT);
-  ui_->qvtk_widget->update();
 
   std::function<void(const CloudConstPtr&)> f = [this](const CloudConstPtr& cloud) {
     cloud_cb(cloud);
@@ -308,6 +320,16 @@ OrganizedSegmentationDemo::OrganizedSegmentationDemo(pcl::Grabber& grabber)
   grabber_.start();
 }
 
+void
+OrganizedSegmentationDemo::refreshView()
+{
+#if VTK_MAJOR_VERSION > 8
+  ui_->qvtk_widget->renderWindow()->Render();
+#else
+  ui_->qvtk_widget->update();
+#endif // VTK_MAJOR_VERSION > 8
+}
+
 void
 OrganizedSegmentationDemo::cloud_cb(const CloudConstPtr& cloud)
 {
@@ -440,8 +462,7 @@ OrganizedSegmentationDemo::timeoutSlot()
       data_modified_ = false;
     }
   }
-
-  ui_->qvtk_widget->update();
+  refreshView();
 }
 
 void
diff --git a/apps/src/organized_segmentation_demo.ui b/apps/src/organized_segmentation_demo.ui
index 53174633..20951b52 100644
--- a/apps/src/organized_segmentation_demo.ui
+++ b/apps/src/organized_segmentation_demo.ui
@@ -37,7 +37,7 @@
    </property>
    <layout class="QGridLayout" name="gridLayout">
     <item row="0" column="3">
-     <widget class="QVTKWidget" name="qvtk_widget">
+     <widget class="PCLQVTKWidget" name="qvtk_widget">
       <property name="sizePolicy">
        <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
         <horstretch>255</horstretch>
@@ -214,9 +214,9 @@
  </widget>
  <customwidgets>
   <customwidget>
-   <class>QVTKWidget</class>
-   <extends>QWidget</extends>
-   <header>QVTKWidget.h</header>
+   <class>PCLQVTKWidget</class>
+   <extends>QOpenGLWidget</extends>
+   <header location="global">pcl/visualization/qvtk_compatibility.h</header>
   </customwidget>
  </customwidgets>
  <resources>
diff --git a/apps/src/pcd_organized_multi_plane_segmentation.cpp b/apps/src/pcd_organized_multi_plane_segmentation.cpp
index d10fdf38..82d45070 100644
--- a/apps/src/pcd_organized_multi_plane_segmentation.cpp
+++ b/apps/src/pcd_organized_multi_plane_segmentation.cpp
@@ -36,7 +36,6 @@
 #include <pcl/console/parse.h>
 #include <pcl/features/integral_image_normal.h>
 #include <pcl/geometry/polygon_operations.h>
-#include <pcl/io/io.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/segmentation/organized_connected_component_segmentation.h>
 #include <pcl/segmentation/organized_multi_plane_segmentation.h>
@@ -199,13 +198,12 @@ public:
                       "approx_plane_%02zu_%03zu",
                       static_cast<std::size_t>(i),
                       static_cast<std::size_t>(idx));
-        viewer.addLine(
-            (*approx_contour)[idx],
-            (*approx_contour)[(idx + 1) % approx_contour->size()],
-            0.5 * red[i],
-            0.5 * grn[i],
-            0.5 * blu[i],
-            name);
+        viewer.addLine((*approx_contour)[idx],
+                       (*approx_contour)[(idx + 1) % approx_contour->size()],
+                       0.5 * red[i],
+                       0.5 * grn[i],
+                       0.5 * blu[i],
+                       name);
       }
     }
   }
diff --git a/apps/src/pcd_select_object_plane.cpp b/apps/src/pcd_select_object_plane.cpp
index 3e8f4280..b7914d72 100644
--- a/apps/src/pcd_select_object_plane.cpp
+++ b/apps/src/pcd_select_object_plane.cpp
@@ -47,6 +47,7 @@
 #include <pcl/filters/project_inliers.h>
 #include <pcl/geometry/polygon_operations.h>
 #include <pcl/io/pcd_io.h>
+#include <pcl/sample_consensus/sac_model_plane.h> // for pointToPlaneDistance
 #include <pcl/segmentation/edge_aware_plane_comparator.h>
 #include <pcl/segmentation/euclidean_cluster_comparator.h>
 #include <pcl/segmentation/extract_clusters.h>
@@ -148,7 +149,7 @@ public:
    * \param[out] object the segmented resultant object
    */
   void
-  segmentObject(int picked_idx,
+  segmentObject(pcl::index_t picked_idx,
                 const typename PointCloud<PointT>::ConstPtr& cloud,
                 const PointIndices::Ptr& plane_indices,
                 PointCloud<PointT>& object)
@@ -178,9 +179,8 @@ public:
     exppd.setInputCloud(cloud);
     exppd.setIndices(indices_but_the_plane);
     exppd.setInputPlanarHull(plane_hull);
-    exppd.setViewPoint((*cloud)[picked_idx].x,
-                       (*cloud)[picked_idx].y,
-                       (*cloud)[picked_idx].z);
+    exppd.setViewPoint(
+        (*cloud)[picked_idx].x, (*cloud)[picked_idx].y, (*cloud)[picked_idx].z);
     exppd.setHeightLimits(0.001, 0.5); // up to half a meter
     exppd.segment(*points_above_plane);
 
@@ -199,7 +199,7 @@ public:
       PointCloud<Label>::Ptr scene(
           new PointCloud<Label>(cloud->width, cloud->height, l));
       // Mask the objects that we want to split into clusters
-      for (const int& index : points_above_plane->indices)
+      for (const auto& index : points_above_plane->indices)
         (*scene)[index].label = 1;
       euclidean_cluster_comparator->setLabels(scene);
 
@@ -257,7 +257,7 @@ public:
   /////////////////////////////////////////////////////////////////////////
   void
   segment(const PointT& picked_point,
-          int picked_idx,
+          pcl::index_t picked_idx,
           PlanarRegion<PointT>& region,
           typename PointCloud<PointT>::Ptr& object)
   {
@@ -440,7 +440,7 @@ public:
     if (idx == -1)
       return;
 
-    std::vector<int> indices(1);
+    pcl::Indices indices(1);
     std::vector<float> distances(1);
 
     // Get the point that was picked
@@ -455,9 +455,8 @@ public:
                picked_pt.z);
 
     // Add a sphere to it in the PCLVisualizer window
-    std::stringstream ss;
-    ss << "sphere_" << idx;
-    cloud_viewer_->addSphere(picked_pt, 0.01, 1.0, 0.0, 0.0, ss.str());
+    const std::string sphere_name = "sphere_" + std::to_string(idx);
+    cloud_viewer_->addSphere(picked_pt, 0.01, 1.0, 0.0, 0.0, sphere_name);
 
     // Because VTK/OpenGL stores data without NaN, we lose the 1-1 correspondence, so we
     // must search for the real point
diff --git a/apps/src/pcd_video_player/pcd_video_player.cpp b/apps/src/pcd_video_player/pcd_video_player.cpp
index 15db9d7d..0e90e874 100644
--- a/apps/src/pcd_video_player/pcd_video_player.cpp
+++ b/apps/src/pcd_video_player/pcd_video_player.cpp
@@ -47,8 +47,10 @@
 #include <QMutexLocker>
 #include <QObject>
 #include <QRadioButton>
+#include <ui_pcd_video_player.h>
 
 #include <vtkCamera.h>
+#include <vtkGenericOpenGLRenderWindow.h>
 #include <vtkRenderWindow.h>
 #include <vtkRendererCollection.h>
 
@@ -80,14 +82,23 @@ PCDVideoPlayer::PCDVideoPlayer()
   // Setup the cloud pointer
   cloud_.reset(new pcl::PointCloud<pcl::PointXYZRGBA>);
 
-  // Set up the qvtk window
+  // Create the QVTKWidget
+#if VTK_MAJOR_VERSION > 8
+  auto renderer = vtkSmartPointer<vtkRenderer>::New();
+  auto renderWindow = vtkSmartPointer<vtkGenericOpenGLRenderWindow>::New();
+  renderWindow->AddRenderer(renderer);
+  vis_.reset(new pcl::visualization::PCLVisualizer(renderer, renderWindow, "", false));
+#else
   vis_.reset(new pcl::visualization::PCLVisualizer("", false));
-  ui_->qvtkWidget->SetRenderWindow(vis_->getRenderWindow());
-  vis_->setupInteractor(ui_->qvtkWidget->GetInteractor(),
-                        ui_->qvtkWidget->GetRenderWindow());
+#endif // VTK_MAJOR_VERSION > 8
+  setRenderWindowCompat(*(ui_->qvtk_widget), *(vis_->getRenderWindow()));
+  vis_->setupInteractor(getInteractorCompat(*(ui_->qvtk_widget)),
+                        getRenderWindowCompat(*(ui_->qvtk_widget)));
+
   vis_->getInteractorStyle()->setKeyboardModifier(
       pcl::visualization::INTERACTOR_KB_MOD_SHIFT);
-  ui_->qvtkWidget->update();
+
+  refreshView();
 
   // Connect all buttons
   connect(ui_->playButton, SIGNAL(clicked()), this, SLOT(playButtonPressed()));
@@ -258,7 +269,8 @@ PCDVideoPlayer::timeoutSlot()
     }
     cloud_modified_ = false;
   }
-  ui_->qvtkWidget->update();
+
+  refreshView();
 }
 
 void
@@ -269,6 +281,16 @@ PCDVideoPlayer::indexSliderValueChanged(int value)
   cloud_modified_ = true;
 }
 
+void
+PCDVideoPlayer::refreshView()
+{
+#if VTK_MAJOR_VERSION > 8
+  ui_->qvtk_widget->renderWindow()->Render();
+#else
+  ui_->qvtk_widget->update();
+#endif // VTK_MAJOR_VERSION > 8
+}
+
 void
 print_usage()
 {
diff --git a/apps/src/pcd_video_player/pcd_video_player.ui b/apps/src/pcd_video_player/pcd_video_player.ui
index c80ed5d4..8d891bf9 100644
--- a/apps/src/pcd_video_player/pcd_video_player.ui
+++ b/apps/src/pcd_video_player/pcd_video_player.ui
@@ -14,122 +14,108 @@
    <string>MainWindow</string>
   </property>
   <widget class="QWidget" name="centralwidget">
-   <widget class="QWidget" name="verticalLayoutWidget">
-    <property name="geometry">
-     <rect>
-      <x>339</x>
-      <y>10</y>
-      <width>451</width>
-      <height>531</height>
-     </rect>
-    </property>
-    <layout class="QVBoxLayout" name="verticalLayout">
-     <item>
-      <widget class="QVTKWidget" name="qvtkWidget"/>
-     </item>
-     <item>
-      <widget class="QSlider" name="indexSlider">
-       <property name="orientation">
-        <enum>Qt::Horizontal</enum>
-       </property>
-      </widget>
-     </item>
-    </layout>
-   </widget>
-   <widget class="QWidget" name="verticalLayoutWidget_2">
-    <property name="geometry">
-     <rect>
-      <x>9</x>
-      <y>9</y>
-      <width>321</width>
-      <height>531</height>
-     </rect>
-    </property>
-    <layout class="QVBoxLayout" name="verticalLayout_2">
-     <item>
-      <widget class="QPushButton" name="selectFilesButton">
-       <property name="text">
-        <string>Select PCD Files</string>
-       </property>
-      </widget>
-     </item>
-     <item>
-      <widget class="QPushButton" name="selectFolderButton">
-       <property name="text">
-        <string>Select Folder</string>
-       </property>
-      </widget>
-     </item>
-     <item>
-      <widget class="Line" name="line_2">
-       <property name="orientation">
-        <enum>Qt::Horizontal</enum>
-       </property>
-      </widget>
-     </item>
-     <item>
-      <spacer name="verticalSpacer">
-       <property name="orientation">
-        <enum>Qt::Vertical</enum>
-       </property>
-       <property name="sizeHint" stdset="0">
-        <size>
-         <width>20</width>
-         <height>40</height>
-        </size>
-       </property>
-      </spacer>
-     </item>
-     <item>
-      <widget class="Line" name="line_3">
-       <property name="orientation">
-        <enum>Qt::Horizontal</enum>
-       </property>
-      </widget>
-     </item>
-     <item>
-      <layout class="QHBoxLayout" name="horizontalLayout">
-       <item>
-        <widget class="QToolButton" name="backButton">
-         <property name="text">
-          <string>...</string>
-         </property>
-         <property name="arrowType">
-          <enum>Qt::LeftArrow</enum>
-         </property>
-        </widget>
-       </item>
-       <item>
-        <widget class="QToolButton" name="stopButton">
-         <property name="text">
-          <string>Stop</string>
-         </property>
-        </widget>
-       </item>
-       <item>
-        <widget class="QToolButton" name="playButton">
-         <property name="text">
-          <string>Play</string>
-         </property>
-         <property name="arrowType">
-          <enum>Qt::NoArrow</enum>
-         </property>
-        </widget>
-       </item>
-       <item>
-        <widget class="QToolButton" name="nextButton">
-         <property name="text">
-          <string>...</string>
-         </property>
-         <property name="arrowType">
-          <enum>Qt::RightArrow</enum>
-         </property>
-        </widget>
-       </item>
-      </layout>
-     </item>
-    </layout>
-   </widget>
+   <layout class="QHBoxLayout" name="horizontalLayout_2">
+    <item>
+     <layout class="QVBoxLayout" name="verticalLayout_2">
+      <item>
+       <widget class="QPushButton" name="selectFilesButton">
+        <property name="text">
+         <string>Select PCD Files</string>
+        </property>
+       </widget>
+      </item>
+      <item>
+       <widget class="QPushButton" name="selectFolderButton">
+        <property name="text">
+         <string>Select Folder</string>
+        </property>
+       </widget>
+      </item>
+      <item>
+       <widget class="Line" name="line_2">
+        <property name="orientation">
+         <enum>Qt::Horizontal</enum>
+        </property>
+       </widget>
+      </item>
+      <item>
+       <spacer name="verticalSpacer">
+        <property name="orientation">
+         <enum>Qt::Vertical</enum>
+        </property>
+        <property name="sizeHint" stdset="0">
+         <size>
+          <width>20</width>
+          <height>40</height>
+         </size>
+        </property>
+       </spacer>
+      </item>
+      <item>
+       <widget class="Line" name="line_3">
+        <property name="orientation">
+         <enum>Qt::Horizontal</enum>
+        </property>
+       </widget>
+      </item>
+      <item>
+       <layout class="QHBoxLayout" name="horizontalLayout">
+        <item>
+         <widget class="QToolButton" name="backButton">
+          <property name="text">
+           <string>...</string>
+          </property>
+          <property name="arrowType">
+           <enum>Qt::LeftArrow</enum>
+          </property>
+         </widget>
+        </item>
+        <item>
+         <widget class="QToolButton" name="stopButton">
+          <property name="text">
+           <string>Stop</string>
+          </property>
+         </widget>
+        </item>
+        <item>
+         <widget class="QToolButton" name="playButton">
+          <property name="text">
+           <string>Play</string>
+          </property>
+          <property name="arrowType">
+           <enum>Qt::NoArrow</enum>
+          </property>
+         </widget>
+        </item>
+        <item>
+         <widget class="QToolButton" name="nextButton">
+          <property name="text">
+           <string>...</string>
+          </property>
+          <property name="arrowType">
+           <enum>Qt::RightArrow</enum>
+          </property>
+         </widget>
+        </item>
+       </layout>
+      </item>
+     </layout>
+    </item>
+    <item>
+     <layout class="QVBoxLayout" name="verticalLayout">
+      <item>
+       <widget class="PCLQVTKWidget" name="qvtk_widget"/>
+      </item>
+      <item>
+       <widget class="QSlider" name="indexSlider">
+        <property name="orientation">
+         <enum>Qt::Horizontal</enum>
+        </property>
+       </widget>
+      </item>
+     </layout>
+    </item>
+   </layout>
   </widget>
   <widget class="QMenuBar" name="menubar">
    <property name="geometry">
@@ -137,7 +123,7 @@
      <x>0</x>
      <y>0</y>
      <width>800</width>
-     <height>25</height>
+     <height>21</height>
     </rect>
    </property>
   </widget>
@@ -145,9 +131,9 @@
  </widget>
  <customwidgets>
   <customwidget>
-   <class>QVTKWidget</class>
-   <extends>QWidget</extends>
-   <header>QVTKWidget.h</header>
+   <class>PCLQVTKWidget</class>
+   <extends>QOpenGLWidget</extends>
+   <header location="global">pcl/visualization/qvtk_compatibility.h</header>
   </customwidget>
  </customwidgets>
  <resources/>
diff --git a/apps/src/ppf_object_recognition.cpp b/apps/src/ppf_object_recognition.cpp
index aec0e5ea..e02d9d62 100644
--- a/apps/src/ppf_object_recognition.cpp
+++ b/apps/src/ppf_object_recognition.cpp
@@ -86,8 +86,7 @@ main(int argc, char** argv)
   while (cloud_scene->size() > 0.3 * nr_points) {
     seg.setInputCloud(cloud_scene);
     seg.segment(*inliers, *coefficients);
-    PCL_INFO("Plane inliers: %zu\n",
-             static_cast<std::size_t>(inliers->indices.size()));
+    PCL_INFO("Plane inliers: %zu\n", static_cast<std::size_t>(inliers->indices.size()));
     if (inliers->indices.size() < 50000)
       break;
 
@@ -149,12 +148,11 @@ main(int argc, char** argv)
     pcl::transformPointCloud(
         *cloud_models[model_i], *cloud_output, final_transformation);
 
-    std::stringstream ss;
-    ss << "model_" << model_i;
+    const std::string mode_name = "model_" + std::to_string(model_i);
     visualization::PointCloudColorHandlerRandom<PointXYZ> random_color(
         cloud_output->makeShared());
-    viewer.addPointCloud(cloud_output, random_color, ss.str());
-    PCL_INFO("Showing model %s\n", ss.str().c_str());
+    viewer.addPointCloud(cloud_output, random_color, mode_name);
+    PCL_INFO("Showing model %s\n", mode_name.c_str());
   }
 
   PCL_INFO("All models have been registered!\n");
diff --git a/apps/src/render_views_tesselated_sphere.cpp b/apps/src/render_views_tesselated_sphere.cpp
index 3c6b7798..fd1a7a33 100644
--- a/apps/src/render_views_tesselated_sphere.cpp
+++ b/apps/src/render_views_tesselated_sphere.cpp
@@ -6,6 +6,7 @@
  */
 
 #include <pcl/apps/render_views_tesselated_sphere.h>
+#include <pcl/visualization/vtk/pcl_vtk_compatibility.h>
 #include <pcl/point_types.h>
 
 #include <vtkActor.h>
@@ -13,6 +14,7 @@
 #include <vtkCellArray.h>
 #include <vtkCellData.h>
 #include <vtkHardwareSelector.h>
+#include <vtkIdTypeArray.h>
 #include <vtkLoopSubdivisionFilter.h>
 #include <vtkPlatonicSolidSource.h>
 #include <vtkPointPicker.h>
@@ -34,7 +36,8 @@ pcl::apps::RenderViewsTesselatedSphere::generateViews()
 {
   // center object
   double CoM[3];
-  vtkIdType npts_com = 0, *ptIds_com = nullptr;
+  vtkIdType npts_com = 0;
+  vtkCellPtsPtr ptIds_com = nullptr;
   vtkSmartPointer<vtkCellArray> cells_com = polydata_->GetPolys();
 
   double center[3], p1_com[3], p2_com[3], p3_com[3], totalArea_com = 0;
@@ -95,7 +98,8 @@ pcl::apps::RenderViewsTesselatedSphere::generateViews()
   // * Compute area of the mesh
   //////////////////////////////
   vtkSmartPointer<vtkCellArray> cells = mapper->GetInput()->GetPolys();
-  vtkIdType npts = 0, *ptIds = nullptr;
+  vtkIdType npts = 0;
+  vtkCellPtsPtr ptIds = nullptr;
 
   double p1[3], p2[3], p3[3], totalArea = 0;
   for (cells->InitTraversal(); cells->GetNextCell(npts, ptIds);) {
@@ -363,7 +367,8 @@ pcl::apps::RenderViewsTesselatedSphere::generateViews()
       polydata->BuildCells();
 
       vtkSmartPointer<vtkCellArray> cells = polydata->GetPolys();
-      vtkIdType npts = 0, *ptIds = nullptr;
+      vtkIdType npts = 0;
+      vtkCellPtsPtr ptIds = nullptr;
 
       double p1[3], p2[3], p3[3], area, totalArea = 0;
       for (cells->InitTraversal(); cells->GetNextCell(npts, ptIds);) {
diff --git a/apps/src/stereo_ground_segmentation.cpp b/apps/src/stereo_ground_segmentation.cpp
index d0948ee8..baabce97 100755
--- a/apps/src/stereo_ground_segmentation.cpp
+++ b/apps/src/stereo_ground_segmentation.cpp
@@ -36,14 +36,8 @@
 #include <pcl/common/centroid.h> // for computeMeanAndCovarianceMatrix
 #include <pcl/common/distances.h>
 #include <pcl/common/intersections.h>
-#include <pcl/common/time.h>
 #include <pcl/features/integral_image_normal.h>
-#include <pcl/io/io.h>
-#include <pcl/io/openni_grabber.h>
-#include <pcl/io/pcd_grabber.h>
 #include <pcl/io/pcd_io.h>
-#include <pcl/io/ply_io.h>
-#include <pcl/io/png_io.h>
 #include <pcl/sample_consensus/sac_model_plane.h>
 #include <pcl/segmentation/euclidean_cluster_comparator.h>
 #include <pcl/segmentation/ground_plane_comparator.h>
@@ -56,6 +50,8 @@
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/ModelCoefficients.h>
 
+#include <boost/filesystem.hpp> // for directory_iterator
+
 #include <mutex>
 
 using PointT = pcl::PointXYZRGB;
@@ -445,8 +441,7 @@ public:
     // note the NAN points in the image as well
     for (std::size_t i = 0; i < cloud->size(); i++) {
       if (!pcl::isFinite((*cloud)[i])) {
-        (*ground_image)[i].b =
-            static_cast<std::uint8_t>(((*cloud)[i].b + 255) / 2);
+        (*ground_image)[i].b = static_cast<std::uint8_t>(((*cloud)[i].b + 255) / 2);
         (*label_image)[i].r = 0;
         (*label_image)[i].g = 0;
         (*label_image)[i].b = 255;
diff --git a/apps/src/test_search.cpp b/apps/src/test_search.cpp
index da14b536..e561ce51 100644
--- a/apps/src/test_search.cpp
+++ b/apps/src/test_search.cpp
@@ -55,9 +55,9 @@ main(int argc, char** argv)
   query.y = 0.5;
   query.z = 0.5;
 
-  std::vector<int> kd_indices;
+  pcl::Indices kd_indices;
   std::vector<float> kd_distances;
-  std::vector<int> bf_indices;
+  pcl::Indices bf_indices;
   std::vector<float> bf_distances;
 
   double start, stop;
diff --git a/benchmarks/CMakeLists.txt b/benchmarks/CMakeLists.txt
new file mode 100644
index 00000000..6fccc23f
--- /dev/null
+++ b/benchmarks/CMakeLists.txt
@@ -0,0 +1,25 @@
+set(SUBSYS_NAME benchmarks)
+set(SUBSYS_DESC "Point cloud library benchmarks")
+set(SUBSYS_DEPS common filters features search kdtree io)
+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})
+if(NOT build)
+  return()
+endif()
+
+find_package(benchmark REQUIRED)
+add_custom_target(run_benchmarks)
+
+PCL_ADD_BENCHMARK(features_normal_3d FILES features/normal_3d.cpp
+                  LINK_WITH pcl_io pcl_search pcl_features
+                  ARGUMENTS "${PCL_SOURCE_DIR}/test/table_scene_mug_stereo_textured.pcd"
+                            "${PCL_SOURCE_DIR}/test/milk_cartoon_all_small_clorox.pcd")
+
+PCL_ADD_BENCHMARK(filters_voxel_grid FILES filters/voxel_grid.cpp
+                  LINK_WITH pcl_io pcl_filters
+                  ARGUMENTS "${PCL_SOURCE_DIR}/test/table_scene_mug_stereo_textured.pcd"
+                            "${PCL_SOURCE_DIR}/test/milk_cartoon_all_small_clorox.pcd")
+
diff --git a/benchmarks/features/normal_3d.cpp b/benchmarks/features/normal_3d.cpp
new file mode 100644
index 00000000..958a0cd4
--- /dev/null
+++ b/benchmarks/features/normal_3d.cpp
@@ -0,0 +1,69 @@
+#include <pcl/features/normal_3d.h>     // for NormalEstimation
+#include <pcl/features/normal_3d_omp.h> // for NormalEstimationOMP
+#include <pcl/io/pcd_io.h>              // for PCDReader
+
+#include <benchmark/benchmark.h>
+
+static void
+BM_NormalEstimation(benchmark::State& state, const std::string& file)
+{
+  // Perform setup here
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PCDReader reader;
+  reader.read(file, *cloud);
+  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
+  ne.setInputCloud(cloud);
+  ne.setKSearch(state.range(0));
+  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
+  for (auto _ : state) {
+    // This code gets timed
+    ne.compute(*cloud_normals);
+  }
+}
+
+#ifdef _OPENMP
+static void
+BM_NormalEstimationOMP(benchmark::State& state, const std::string& file)
+{
+  // Perform setup here
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PCDReader reader;
+  reader.read(file, *cloud);
+  pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
+  ne.setInputCloud(cloud);
+  ne.setKSearch(100);
+  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
+  for (auto _ : state) {
+    // This code gets timed
+    ne.compute(*cloud_normals);
+  }
+}
+#endif
+
+int
+main(int argc, char** argv)
+{
+  if (argc < 3) {
+    std::cerr
+        << "No test files given. Please download `table_scene_mug_stereo_textured.pcd` "
+           "and `milk_cartoon_all_small_clorox.pcd`, and pass their paths to the test."
+        << std::endl;
+    return (-1);
+  }
+  benchmark::RegisterBenchmark("BM_NormalEstimation_mug", &BM_NormalEstimation, argv[1])
+      ->Arg(50)
+      ->Arg(100)
+      ->Unit(benchmark::kMillisecond);
+  benchmark::RegisterBenchmark(
+      "BM_NormalEstimation_milk", &BM_NormalEstimation, argv[2])
+      ->Arg(50)
+      ->Arg(100)
+      ->Unit(benchmark::kMillisecond);
+#ifdef _OPENMP
+  benchmark::RegisterBenchmark(
+      "BM_NormalEstimationOMP", &BM_NormalEstimationOMP, argv[1])
+      ->Unit(benchmark::kMillisecond);
+#endif
+  benchmark::Initialize(&argc, argv);
+  benchmark::RunSpecifiedBenchmarks();
+}
diff --git a/benchmarks/filters/voxel_grid.cpp b/benchmarks/filters/voxel_grid.cpp
new file mode 100644
index 00000000..066cacc6
--- /dev/null
+++ b/benchmarks/filters/voxel_grid.cpp
@@ -0,0 +1,72 @@
+#include <pcl/filters/approximate_voxel_grid.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/io/pcd_io.h> // for PCDReader
+
+#include <benchmark/benchmark.h>
+
+static void
+BM_VoxelGrid(benchmark::State& state, const std::string& file)
+{
+  // Perform setup here
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PCDReader reader;
+  reader.read(file, *cloud);
+
+  pcl::VoxelGrid<pcl::PointXYZ> vg;
+  vg.setLeafSize(0.01, 0.01, 0.01);
+  vg.setInputCloud(cloud);
+
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_voxelized(
+      new pcl::PointCloud<pcl::PointXYZ>);
+  for (auto _ : state) {
+    // This code gets timed
+    vg.filter(*cloud_voxelized);
+  }
+}
+
+static void
+BM_ApproxVoxelGrid(benchmark::State& state, const std::string& file)
+{
+  // Perform setup here
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PCDReader reader;
+  reader.read(file, *cloud);
+
+  pcl::ApproximateVoxelGrid<pcl::PointXYZ> avg;
+  avg.setLeafSize(0.01, 0.01, 0.01);
+  avg.setInputCloud(cloud);
+
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_voxelized(
+      new pcl::PointCloud<pcl::PointXYZ>);
+  for (auto _ : state) {
+    // This code gets timed
+    avg.filter(*cloud_voxelized);
+  }
+}
+
+int
+main(int argc, char** argv)
+{
+  if (argc < 3) {
+    std::cerr
+        << "No test files given. Please download `table_scene_mug_stereo_textured.pcd` "
+           "and `milk_cartoon_all_small_clorox.pcd`, and pass their paths to the test."
+        << std::endl;
+    return (-1);
+  }
+
+  benchmark::RegisterBenchmark("BM_VoxelGrid_milk", &BM_VoxelGrid, argv[2])
+      ->Unit(benchmark::kMillisecond);
+  benchmark::RegisterBenchmark(
+      "BM_ApproximateVoxelGrid_milk", &BM_ApproxVoxelGrid, argv[2])
+      ->Unit(benchmark::kMillisecond);
+
+  benchmark::RegisterBenchmark("BM_VoxelGrid_mug", &BM_VoxelGrid, argv[1])
+      ->Unit(benchmark::kMillisecond);
+  benchmark::RegisterBenchmark(
+      "BM_ApproximateVoxelGrid_mug", &BM_ApproxVoxelGrid, argv[1])
+      ->Unit(benchmark::kMillisecond);
+
+  benchmark::Initialize(&argc, argv);
+  benchmark::RunSpecifiedBenchmarks();
+}
diff --git a/cmake/Modules/FindFLANN.cmake b/cmake/Modules/FindFLANN.cmake
index fd15d1bb..a87f9c61 100644
--- a/cmake/Modules/FindFLANN.cmake
+++ b/cmake/Modules/FindFLANN.cmake
@@ -42,16 +42,38 @@ endif()
 
 # First try to locate FLANN using modern config
 find_package(flann NO_MODULE ${FLANN_FIND_VERSION} QUIET)
+
 if(flann_FOUND)
   unset(flann_FOUND)
   set(FLANN_FOUND ON)
+
   # Create interface library that effectively becomes an alias for the appropriate (static/dynamic) imported FLANN target
   add_library(FLANN::FLANN INTERFACE IMPORTED)
-  if(FLANN_USE_STATIC)
+
+  if(TARGET flann::flann_cpp_s AND TARGET flann::flann_cpp)
+    if(PCL_FLANN_REQUIRED_TYPE MATCHES "DONTCARE")
+      if(PCL_SHARED_LIBS)
+        set_property(TARGET FLANN::FLANN APPEND PROPERTY INTERFACE_LINK_LIBRARIES flann::flann_cpp)
+        set(FLANN_LIBRARY_TYPE SHARED)
+      else()
+        set_property(TARGET FLANN::FLANN APPEND PROPERTY INTERFACE_LINK_LIBRARIES flann::flann_cpp_s)
+        set(FLANN_LIBRARY_TYPE STATIC)
+      endif()
+    elseif(PCL_FLANN_REQUIRED_TYPE MATCHES "SHARED")
+      set_property(TARGET FLANN::FLANN APPEND PROPERTY INTERFACE_LINK_LIBRARIES flann::flann_cpp)
+      set(FLANN_LIBRARY_TYPE SHARED)
+    else()
+      set_property(TARGET FLANN::FLANN APPEND PROPERTY INTERFACE_LINK_LIBRARIES flann::flann_cpp_s)
+      set(FLANN_LIBRARY_TYPE STATIC)
+    endif()
+  elseif(TARGET flann::flann_cpp_s)
     set_property(TARGET FLANN::FLANN APPEND PROPERTY INTERFACE_LINK_LIBRARIES flann::flann_cpp_s)
+    set(FLANN_LIBRARY_TYPE STATIC)
   else()
     set_property(TARGET FLANN::FLANN APPEND PROPERTY INTERFACE_LINK_LIBRARIES flann::flann_cpp)
+    set(FLANN_LIBRARY_TYPE SHARED)
   endif()
+
   # Determine FLANN installation root based on the path to the processed Config file
   get_filename_component(_config_dir "${flann_CONFIG}" DIRECTORY)
   get_filename_component(FLANN_ROOT "${_config_dir}/../../.." ABSOLUTE)
@@ -82,19 +104,23 @@ find_path(FLANN_INCLUDE_DIR
     include
 )
 
-if(FLANN_USE_STATIC)
-  set(FLANN_RELEASE_NAME flann_cpp_s)
-  set(FLANN_DEBUG_NAME flann_cpp_s-gd)
-  set(FLANN_LIBRARY_TYPE STATIC)
-else()
-  set(FLANN_RELEASE_NAME flann_cpp)
-  set(FLANN_DEBUG_NAME flann_cpp-gd)
-  set(FLANN_LIBRARY_TYPE SHARED)
-endif()
+find_library(FLANN_LIBRARY_SHARED
+  NAMES
+    flann_cpp
+  HINTS
+    ${PC_FLANN_LIBRARY_DIRS}
+    ${FLANN_ROOT}
+    $ENV{FLANN_ROOT}
+  PATHS
+    $ENV{PROGRAMFILES}/Flann
+    $ENV{PROGRAMW6432}/Flann
+  PATH_SUFFIXES
+    lib
+)
 
-find_library(FLANN_LIBRARY
+find_library(FLANN_LIBRARY_DEBUG_SHARED
   NAMES
-    ${FLANN_RELEASE_NAME}
+    flann_cpp-gd flann_cppd
   HINTS
     ${PC_FLANN_LIBRARY_DIRS}
     ${FLANN_ROOT}
@@ -106,9 +132,9 @@ find_library(FLANN_LIBRARY
     lib
 )
 
-find_library(FLANN_LIBRARY_DEBUG
+find_library(FLANN_LIBRARY_STATIC
   NAMES
-    ${FLANN_DEBUG_NAME}
+    flann_cpp_s
   HINTS
     ${PC_FLANN_LIBRARY_DIRS}
     ${FLANN_ROOT}
@@ -120,6 +146,44 @@ find_library(FLANN_LIBRARY_DEBUG
     lib
 )
 
+find_library(FLANN_LIBRARY_DEBUG_STATIC
+  NAMES
+    flann_cpp_s-gd flann_cpp_sd
+  HINTS
+    ${PC_FLANN_LIBRARY_DIRS}
+    ${FLANN_ROOT}
+    $ENV{FLANN_ROOT}
+  PATHS
+    $ENV{PROGRAMFILES}/Flann
+    $ENV{PROGRAMW6432}/Flann
+  PATH_SUFFIXES
+    lib
+)
+
+if(FLANN_LIBRARY_SHARED AND FLANN_LIBRARY_STATIC)
+  if(PCL_FLANN_REQUIRED_TYPE MATCHES "DONTCARE")
+    if(PCL_SHARED_LIBS)
+      set(FLANN_LIBRARY_TYPE SHARED)
+      set(FLANN_LIBRARY ${FLANN_LIBRARY_SHARED})
+    else()
+      set(FLANN_LIBRARY_TYPE STATIC)
+      set(FLANN_LIBRARY ${FLANN_LIBRARY_STATIC})
+    endif()
+  elseif(PCL_FLANN_REQUIRED_TYPE MATCHES "SHARED")
+    set(FLANN_LIBRARY_TYPE SHARED)
+    set(FLANN_LIBRARY ${FLANN_LIBRARY_SHARED})
+  else()
+    set(FLANN_LIBRARY_TYPE STATIC)
+    set(FLANN_LIBRARY ${FLANN_LIBRARY_STATIC})
+  endif()
+elseif(FLANN_LIBRARY_STATIC)
+  set(FLANN_LIBRARY_TYPE STATIC)
+  set(FLANN_LIBRARY ${FLANN_LIBRARY_STATIC})
+elseif(FLANN_LIBRARY_SHARED)
+  set(FLANN_LIBRARY_TYPE SHARED)
+  set(FLANN_LIBRARY ${FLANN_LIBRARY_SHARED})
+endif()
+
 include(FindPackageHandleStandardArgs)
 find_package_handle_standard_args(
   FLANN DEFAULT_MSG
@@ -132,7 +196,7 @@ if(FLANN_FOUND)
   set_target_properties(FLANN::FLANN PROPERTIES INTERFACE_COMPILE_DEFINITIONS "${PC_FLANN_CFLAGS_OTHER}")
   set_property(TARGET FLANN::FLANN APPEND PROPERTY IMPORTED_CONFIGURATIONS "RELEASE")
   set_target_properties(FLANN::FLANN PROPERTIES IMPORTED_LINK_INTERFACE_LANGUAGES_RELEASE "CXX")
-  if(WIN32 AND NOT FLANN_USE_STATIC)
+  if(WIN32 AND (NOT FLANN_LIBRARY_TYPE MATCHES "STATIC"))
     set_target_properties(FLANN::FLANN PROPERTIES IMPORTED_IMPLIB_RELEASE "${FLANN_LIBRARY}")
   else()
     set_target_properties(FLANN::FLANN PROPERTIES IMPORTED_LOCATION_RELEASE "${FLANN_LIBRARY}")
@@ -140,7 +204,7 @@ if(FLANN_FOUND)
   if(FLANN_LIBRARY_DEBUG)
     set_property(TARGET FLANN::FLANN APPEND PROPERTY IMPORTED_CONFIGURATIONS "DEBUG")
     set_target_properties(FLANN::FLANN PROPERTIES IMPORTED_LINK_INTERFACE_LANGUAGES_DEBUG "CXX")
-    if(WIN32 AND NOT FLANN_USE_STATIC)
+    if(WIN32 AND (NOT FLANN_LIBRARY_TYPE MATCHES "STATIC"))
       set_target_properties(FLANN::FLANN PROPERTIES IMPORTED_IMPLIB_DEBUG "${FLANN_LIBRARY_DEBUG}")
     else()
       set_target_properties(FLANN::FLANN PROPERTIES IMPORTED_LOCATION_DEBUG "${FLANN_LIBRARY_DEBUG}")
diff --git a/cmake/Modules/FindGLEW.cmake b/cmake/Modules/FindGLEW.cmake
new file mode 100644
index 00000000..b0e1defc
--- /dev/null
+++ b/cmake/Modules/FindGLEW.cmake
@@ -0,0 +1,69 @@
+# Distributed under the OSI-approved BSD 3-Clause License.  See accompanying
+# file Copyright.txt or https://cmake.org/licensing for details.
+
+#.rst:
+# FindGLEW
+# --------
+#
+# Find the OpenGL Extension Wrangler Library (GLEW)
+#
+# IMPORTED Targets
+# ^^^^^^^^^^^^^^^^
+#
+# This module defines the :prop_tgt:`IMPORTED` target ``GLEW::GLEW``,
+# if GLEW has been found.
+#
+# Result Variables
+# ^^^^^^^^^^^^^^^^
+#
+# This module defines the following variables:
+#
+# ::
+#
+#   GLEW_INCLUDE_DIRS - include directories for GLEW
+#   GLEW_LIBRARIES - libraries to link against GLEW
+#   GLEW_FOUND - true if GLEW has been found and can be used
+
+find_path(GLEW_INCLUDE_DIR GL/glew.h)
+
+if(NOT GLEW_LIBRARY)
+  find_library(GLEW_LIBRARY_RELEASE NAMES GLEW glew32 glew glew32s PATH_SUFFIXES lib64 libx32)
+  find_library(GLEW_LIBRARY_DEBUG NAMES GLEWd glew32d glewd PATH_SUFFIXES lib64)
+
+  include(SelectLibraryConfigurations)
+  select_library_configurations(GLEW)
+endif ()
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(GLEW
+                                  REQUIRED_VARS GLEW_INCLUDE_DIR GLEW_LIBRARY)
+
+if(GLEW_FOUND)
+  set(GLEW_INCLUDE_DIRS ${GLEW_INCLUDE_DIR})
+
+  if(NOT GLEW_LIBRARIES)
+    set(GLEW_LIBRARIES ${GLEW_LIBRARY})
+  endif()
+
+  if (NOT TARGET GLEW::GLEW)
+    add_library(GLEW::GLEW UNKNOWN IMPORTED)
+    set_target_properties(GLEW::GLEW PROPERTIES
+      INTERFACE_INCLUDE_DIRECTORIES "${GLEW_INCLUDE_DIRS}")
+
+    if(GLEW_LIBRARY_RELEASE)
+      set_property(TARGET GLEW::GLEW APPEND PROPERTY IMPORTED_CONFIGURATIONS RELEASE)
+      set_target_properties(GLEW::GLEW PROPERTIES IMPORTED_LOCATION_RELEASE "${GLEW_LIBRARY_RELEASE}")
+    endif()
+
+    if(GLEW_LIBRARY_DEBUG)
+      set_property(TARGET GLEW::GLEW APPEND PROPERTY IMPORTED_CONFIGURATIONS DEBUG)
+      set_target_properties(GLEW::GLEW PROPERTIES IMPORTED_LOCATION_DEBUG "${GLEW_LIBRARY_DEBUG}")
+    endif()
+
+    if(NOT GLEW_LIBRARY_RELEASE AND NOT GLEW_LIBRARY_DEBUG)
+      set_property(TARGET GLEW::GLEW APPEND PROPERTY IMPORTED_LOCATION "${GLEW_LIBRARY}")
+    endif()
+  endif()
+endif()
+
+mark_as_advanced(GLEW_INCLUDE_DIR)
\ No newline at end of file
diff --git a/cmake/Modules/FindOpenNI.cmake b/cmake/Modules/FindOpenNI.cmake
index 09a60811..249e8962 100644
--- a/cmake/Modules/FindOpenNI.cmake
+++ b/cmake/Modules/FindOpenNI.cmake
@@ -11,30 +11,6 @@
 #  OPENNI_DEFINITIONS          Compiler flags for OpenNI
 
 find_package(PkgConfig QUIET)
-
-# Find LibUSB
-if(NOT WIN32)
-  pkg_check_modules(PC_USB_10 libusb-1.0)
-  find_path(USB_10_INCLUDE_DIR libusb-1.0/libusb.h
-            HINTS ${PC_USB_10_INCLUDEDIR} ${PC_USB_10_INCLUDE_DIRS} "${USB_10_ROOT}" "$ENV{USB_10_ROOT}"
-            PATH_SUFFIXES libusb-1.0)
-
-  find_library(USB_10_LIBRARY
-               NAMES usb-1.0
-               HINTS ${PC_USB_10_LIBDIR} ${PC_USB_10_LIBRARY_DIRS} "${USB_10_ROOT}" "$ENV{USB_10_ROOT}"
-               PATH_SUFFIXES lib)
-
-  include(FindPackageHandleStandardArgs)
-  find_package_handle_standard_args(USB_10 DEFAULT_MSG USB_10_LIBRARY USB_10_INCLUDE_DIR)
-
-  if(NOT USB_10_FOUND)
-    message(STATUS "OpenNI disabled because libusb-1.0 not found.")
-    return()
-  else()
-    include_directories(SYSTEM ${USB_10_INCLUDE_DIR})
-  endif()
-endif()
-
 pkg_check_modules(PC_OPENNI QUIET libopenni)
 
 set(OPENNI_DEFINITIONS ${PC_OPENNI_CFLAGS_OTHER})
@@ -76,7 +52,8 @@ if(OPENNI_INCLUDE_DIR AND OPENNI_LIBRARY)
 
   # Libraries
   if(CMAKE_SYSTEM_NAME STREQUAL "Darwin")
-    set(OPENNI_LIBRARIES ${OPENNI_LIBRARY} ${LIBUSB_1_LIBRARIES})
+    find_package(libusb REQUIRED)
+    set(OPENNI_LIBRARIES ${OPENNI_LIBRARY} libusb::libusb)
   else()
     set(OPENNI_LIBRARIES ${OPENNI_LIBRARY})
   endif()
@@ -85,12 +62,24 @@ if(OPENNI_INCLUDE_DIR AND OPENNI_LIBRARY)
 
 endif()
 
+if(EXISTS "${OPENNI_INCLUDE_DIR}/XnVersion.h")
+  file(STRINGS "${OPENNI_INCLUDE_DIR}/XnVersion.h" _contents REGEX "^#define[ \t]+XN_[A-Z]+_VERSION[ \t]+[0-9]+")
+  if(_contents)
+    string(REGEX REPLACE ".*#define[ \t]+XN_MAJOR_VERSION[ \t]+([0-9]+).*" "\\1" OPENNI_VERSION_MAJOR "${_contents}")
+    string(REGEX REPLACE ".*#define[ \t]+XN_MINOR_VERSION[ \t]+([0-9]+).*" "\\1" OPENNI_VERSION_MINOR "${_contents}")
+    string(REGEX REPLACE ".*#define[ \t]+XN_MAINTENANCE_VERSION[ \t]+([0-9]+).*" "\\1" OPENNI_VERSION_PATCH "${_contents}")
+    string(REGEX REPLACE ".*#define[ \t]+XN_BUILD_VERSION[ \t]+([0-9]+).*" "\\1" OPENNI_VERSION_BUILD "${_contents}")
+    set(OPENNI_VERSION "${OPENNI_VERSION_MAJOR}.${OPENNI_VERSION_MINOR}.${OPENNI_VERSION_PATCH}.${OPENNI_VERSION_BUILD}")
+  endif()
+endif()
+
 include(FindPackageHandleStandardArgs)
 find_package_handle_standard_args(OpenNI
   FOUND_VAR OPENNI_FOUND
   REQUIRED_VARS OPENNI_LIBRARIES OPENNI_INCLUDE_DIRS
+  VERSION_VAR OPENNI_VERSION
 )
 
 if(OPENNI_FOUND)
-  message(STATUS "OpenNI found (include: ${OPENNI_INCLUDE_DIRS}, lib: ${OPENNI_LIBRARIES})")
+  message(STATUS "OpenNI found (version: ${OPENNI_VERSION}, include: ${OPENNI_INCLUDE_DIRS}, lib: ${OPENNI_LIBRARIES})")
 endif()
diff --git a/cmake/Modules/FindOpenNI2.cmake b/cmake/Modules/FindOpenNI2.cmake
index 10263311..14ca206e 100644
--- a/cmake/Modules/FindOpenNI2.cmake
+++ b/cmake/Modules/FindOpenNI2.cmake
@@ -11,30 +11,6 @@
 #  OPENNI2_DEFINITIONS         Compiler flags for OpenNI2
 
 find_package(PkgConfig QUIET)
-
-# Find LibUSB
-if(NOT WIN32)
-  pkg_check_modules(PC_USB_10 libusb-1.0)
-  find_path(USB_10_INCLUDE_DIR libusb-1.0/libusb.h
-            HINTS ${PC_USB_10_INCLUDEDIR} ${PC_USB_10_INCLUDE_DIRS} "${USB_10_ROOT}" "$ENV{USB_10_ROOT}"
-            PATH_SUFFIXES libusb-1.0)
-
-  find_library(USB_10_LIBRARY
-               NAMES usb-1.0
-               HINTS ${PC_USB_10_LIBDIR} ${PC_USB_10_LIBRARY_DIRS} "${USB_10_ROOT}" "$ENV{USB_10_ROOT}"
-               PATH_SUFFIXES lib)
-
-  include(FindPackageHandleStandardArgs)
-  find_package_handle_standard_args(USB_10 DEFAULT_MSG USB_10_LIBRARY USB_10_INCLUDE_DIR)
-
-  if(NOT USB_10_FOUND)
-    message(STATUS "OpenNI 2 disabled because libusb-1.0 not found.")
-    return()
-  else()
-    include_directories(SYSTEM ${USB_10_INCLUDE_DIR})
-  endif()
-endif()
-
 pkg_check_modules(PC_OPENNI2 QUIET libopenni2)
 
 set(OPENNI2_DEFINITIONS ${PC_OPENNI_CFLAGS_OTHER})
@@ -66,7 +42,8 @@ if(OPENNI2_INCLUDE_DIR AND OPENNI2_LIBRARY)
 
   # Libraries
   if(CMAKE_SYSTEM_NAME STREQUAL "Darwin")
-    set(OPENNI2_LIBRARIES ${OPENNI2_LIBRARY} ${LIBUSB_1_LIBRARIES})
+    find_package(libusb REQUIRED)
+    set(OPENNI2_LIBRARIES ${OPENNI2_LIBRARY} libusb::libusb)
   else()
     set(OPENNI2_LIBRARIES ${OPENNI2_LIBRARY})
   endif()
@@ -78,12 +55,24 @@ if(OPENNI2_INCLUDE_DIR AND OPENNI2_LIBRARY)
 
 endif()
 
+if(EXISTS "${OPENNI2_INCLUDE_DIR}/OniVersion.h")
+  file(STRINGS "${OPENNI2_INCLUDE_DIR}/OniVersion.h" _contents REGEX "^#define[ \t]+ONI_VERSION_[A-Z]+[ \t]+[0-9]+")
+  if(_contents)
+    string(REGEX REPLACE ".*#define[ \t]+ONI_VERSION_MAJOR[ \t]+([0-9]+).*" "\\1" OPENNI2_VERSION_MAJOR "${_contents}")
+    string(REGEX REPLACE ".*#define[ \t]+ONI_VERSION_MINOR[ \t]+([0-9]+).*" "\\1" OPENNI2_VERSION_MINOR "${_contents}")
+    string(REGEX REPLACE ".*#define[ \t]+ONI_VERSION_MAINTENANCE[ \t]+([0-9]+).*" "\\1" OPENNI2_VERSION_PATCH "${_contents}")
+    string(REGEX REPLACE ".*#define[ \t]+ONI_VERSION_BUILD[ \t]+([0-9]+).*" "\\1" OPENNI2_VERSION_BUILD "${_contents}")
+    set(OPENNI2_VERSION "${OPENNI2_VERSION_MAJOR}.${OPENNI2_VERSION_MINOR}.${OPENNI2_VERSION_PATCH}.${OPENNI2_VERSION_BUILD}")
+  endif()
+endif()
+
 include(FindPackageHandleStandardArgs)
 find_package_handle_standard_args(OpenNI2
   FOUND_VAR OPENNI2_FOUND
   REQUIRED_VARS OPENNI2_LIBRARIES OPENNI2_INCLUDE_DIRS
+  VERSION_VAR OPENNI2_VERSION
 )
 
 if(OPENNI2_FOUND)
-  message(STATUS "OpenNI2 found (include: ${OPENNI2_INCLUDE_DIRS}, lib: ${OPENNI2_LIBRARIES})")
+  message(STATUS "OpenNI2 found (version: ${OPENNI2_VERSION}, include: ${OPENNI2_INCLUDE_DIRS}, lib: ${OPENNI2_LIBRARIES})")
 endif()
diff --git a/cmake/Modules/FindQhull.cmake b/cmake/Modules/FindQhull.cmake
index 7f3cf90d..f9f62845 100644
--- a/cmake/Modules/FindQhull.cmake
+++ b/cmake/Modules/FindQhull.cmake
@@ -9,14 +9,12 @@
 # If QHULL_USE_STATIC is specified then look for static libraries ONLY else
 # look for shared ones
 
-set(QHULL_MAJOR_VERSION 6)
-
 if(QHULL_USE_STATIC)
-  set(QHULL_RELEASE_NAME qhullstatic)
-  set(QHULL_DEBUG_NAME qhullstatic_d)
+  set(QHULL_RELEASE_NAME qhullstatic_r)
+  set(QHULL_DEBUG_NAME qhullstatic_rd)
 else()
-  set(QHULL_RELEASE_NAME qhull_p qhull${QHULL_MAJOR_VERSION} qhull)
-  set(QHULL_DEBUG_NAME qhull_p_d qhull${QHULL_MAJOR_VERSION}_d qhull_d${QHULL_MAJOR_VERSION} qhull_d)
+  set(QHULL_RELEASE_NAME qhull_r qhull)
+  set(QHULL_DEBUG_NAME qhull_rd qhull_d)
 endif()
 
 find_file(QHULL_HEADER
@@ -30,10 +28,8 @@ set(QHULL_HEADER "${QHULL_HEADER}" CACHE INTERNAL "QHull header" FORCE )
 if(QHULL_HEADER)
   get_filename_component(qhull_header ${QHULL_HEADER} NAME_WE)
   if("${qhull_header}" STREQUAL "qhull")
-    set(HAVE_QHULL_2011 OFF)
     get_filename_component(QHULL_INCLUDE_DIR ${QHULL_HEADER} PATH)
   elseif("${qhull_header}" STREQUAL "libqhull")
-    set(HAVE_QHULL_2011 ON)
     get_filename_component(QHULL_INCLUDE_DIR ${QHULL_HEADER} PATH)
     get_filename_component(QHULL_INCLUDE_DIR ${QHULL_INCLUDE_DIR} PATH)
   endif()
diff --git a/cmake/Modules/FindRSSDK.cmake b/cmake/Modules/FindRSSDK.cmake
index de0cf2cf..162ee7d4 100644
--- a/cmake/Modules/FindRSSDK.cmake
+++ b/cmake/Modules/FindRSSDK.cmake
@@ -67,5 +67,5 @@ find_package_handle_standard_args(RSSDK
 )
 
 if(MSVC)
-  set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} /NODEFAULTLIB:LIBCMTD")
+  string(APPEND CMAKE_SHARED_LINKER_FLAGS " /NODEFAULTLIB:LIBCMTD")
 endif()
diff --git a/cmake/Modules/Findlibusb-1.0.cmake b/cmake/Modules/Findlibusb-1.0.cmake
deleted file mode 100644
index 564eb8e3..00000000
--- a/cmake/Modules/Findlibusb-1.0.cmake
+++ /dev/null
@@ -1,72 +0,0 @@
-# - Try to find libusb-1.0
-# Once done this will define
-#
-#  LIBUSB_1_FOUND - system has libusb
-#  LIBUSB_1_INCLUDE_DIRS - the libusb include directory
-#  LIBUSB_1_LIBRARIES - Link these to use libusb
-#  LIBUSB_1_DEFINITIONS - Compiler switches required for using libusb
-#
-#  Adapted from cmake-modules Google Code project
-#
-#  Copyright (c) 2006 Andreas Schneider <mail@cynapses.org>
-#
-#  (Changes for libusb) Copyright (c) 2008 Kyle Machulis <kyle@nonpolynomial.com>
-#
-#  Point Cloud Library (PCL) - www.pointclouds.org
-#  Copyright (c) 2012, Willow Garage, Inc. (use of FindPackageHandleStandardArgs)
-#
-# Redistribution and use is allowed according to the terms of the New BSD license.
-#
-# CMake-Modules Project New BSD License
-#
-# 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 CMake-Modules Project 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.
-#
-
-if(LIBUSB_1_LIBRARIES AND LIBUSB_1_INCLUDE_DIRS)
-  # in cache already
-  set(LIBUSB_FOUND TRUE)
-  set(LIBUSB_1_FOUND TRUE)
-else()
-  find_path(LIBUSB_1_INCLUDE_DIR
-            NAMES libusb-1.0/libusb.h
-            PATHS /usr/include /usr/local/include /opt/local/include /sw/include
-            PATH_SUFFIXES libusb-1.0)
-
-  # We need to look for libusb-1.0 too because find_library does not attempt to find
-  # library files with a "lib" prefix implicitly on Windows
-  find_library(LIBUSB_1_LIBRARY
-               NAMES usb-1.0 libusb-1.0
-               PATHS /usr/lib /usr/local/lib /opt/local/lib /sw/lib)
-
-  set(LIBUSB_1_INCLUDE_DIRS ${LIBUSB_1_INCLUDE_DIR})
-  set(LIBUSB_1_LIBRARIES ${LIBUSB_1_LIBRARY})
-
-  include(FindPackageHandleStandardArgs)
-  find_package_handle_standard_args(libusb-1.0 DEFAULT_MSG LIBUSB_1_LIBRARY LIBUSB_1_INCLUDE_DIR)
-
-  # show the LIBUSB_1_INCLUDE_DIRS and LIBUSB_1_LIBRARIES variables only in the advanced view
-  mark_as_advanced(LIBUSB_1_INCLUDE_DIRS LIBUSB_1_LIBRARIES)
-endif()
diff --git a/cmake/Modules/Findlibusb.cmake b/cmake/Modules/Findlibusb.cmake
new file mode 100644
index 00000000..90277400
--- /dev/null
+++ b/cmake/Modules/Findlibusb.cmake
@@ -0,0 +1,73 @@
+#.rst:
+# Findlibusb
+# --------
+#
+# Try to find libusb library and headers.
+#
+# IMPORTED Targets
+# ^^^^^^^^^^^^^^^^
+#
+# This module defines the :prop_tgt:`IMPORTED` targets:
+#
+# ``libusb::libusb``
+#  Defined if the system has libusb.
+#
+# Result Variables
+# ^^^^^^^^^^^^^^^^
+#
+# This module sets the following variables:
+#
+# ::
+#
+#   LIBUSB_FOUND               True in case libusb is found, otherwise false
+#   LIBUSB_ROOT                Path to the root of found libusb installation
+#
+# Example usage
+# ^^^^^^^^^^^^^
+#
+# ::
+#
+#     find_package(libusb REQUIRED)
+#
+#     add_executable(foo foo.cc)
+#     target_link_libraries(foo libusb::libusb)
+#
+
+# Early return if libusb target is already defined. This makes it safe to run
+# this script multiple times.
+if(TARGET libusb::libusb)
+  return()
+endif()
+
+find_package(PkgConfig QUIET)
+if(libusb_FIND_VERSION)
+  pkg_check_modules(PC_libusb libusb-1.0>=${libusb_FIND_VERSION})
+else()
+  pkg_check_modules(PC_libusb libusb-1.0)
+endif()
+
+find_path(libusb_INCLUDE_DIR
+  NAMES
+    libusb-1.0/libusb.h
+  PATHS
+    ${PC_libusb_INCLUDEDIR}
+)
+
+find_library(libusb_LIBRARIES
+  NAMES
+    usb-1.0
+    libusb
+  PATHS
+    ${PC_libusb_LIBRARY_DIRS}
+)
+  
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(libusb DEFAULT_MSG libusb_LIBRARIES libusb_INCLUDE_DIR)
+
+mark_as_advanced(libusb_INCLUDE_DIRS libusb_LIBRARIES)
+
+if(LIBUSB_FOUND)
+  add_library(libusb::libusb UNKNOWN IMPORTED)
+  set_target_properties(libusb::libusb PROPERTIES INTERFACE_INCLUDE_DIRECTORIES "${libusb_INCLUDE_DIR}")
+  set_target_properties(libusb::libusb PROPERTIES IMPORTED_LOCATION "${libusb_LIBRARIES}")
+endif()
diff --git a/cmake/pcl_all_in_one_installer.cmake b/cmake/pcl_all_in_one_installer.cmake
index f5c0e6b0..9d006a35 100644
--- a/cmake/pcl_all_in_one_installer.cmake
+++ b/cmake/pcl_all_in_one_installer.cmake
@@ -70,8 +70,7 @@ if(WITH_OPENNI)
       COMPONENT OpenNI
     )
     list(APPEND PCL_3RDPARTY_COMPONENTS OpenNI)
-    set(CPACK_NSIS_EXTRA_INSTALL_COMMANDS
-      "${CPACK_NSIS_EXTRA_INSTALL_COMMANDS}\n    ExecWait 'msiexec /i \\\"$INSTDIR\\\\3rdParty\\\\OpenNI\\\\${OPENNI_PACKAGE}\\\" /quiet '")
+    string(APPEND CPACK_NSIS_EXTRA_INSTALL_COMMANDS "\n    ExecWait 'msiexec /i \\\"$INSTDIR\\\\3rdParty\\\\OpenNI\\\\${OPENNI_PACKAGE}\\\" /quiet '")
   else()
     message("WARNING : Could not download ${OPENNI_URL}, error code : ${_error_code}, error message : ${_error_message}")
   endif()
@@ -89,13 +88,12 @@ if(WITH_OPENNI)
       COMPONENT OpenNI
     )
     list(APPEND PCL_3RDPARTY_COMPONENTS OpenNI)
-    set(CPACK_NSIS_EXTRA_INSTALL_COMMANDS
-      "${CPACK_NSIS_EXTRA_INSTALL_COMMANDS}\n    ExecWait 'msiexec /i \\\"$INSTDIR\\\\3rdParty\\\\OpenNI\\\\${OPENNI_SENSOR_PACKAGE}\\\" /quiet '")
+    string(APPEND CPACK_NSIS_EXTRA_INSTALL_COMMANDS "\n    ExecWait 'msiexec /i \\\"$INSTDIR\\\\3rdParty\\\\OpenNI\\\\${OPENNI_SENSOR_PACKAGE}\\\" /quiet '")
   else()
     message("WARNING : Could not download ${OPENNI_SENSOR_URL}, error code : ${_error_code}, error message : ${_error_message}")
   endif()
   list(REMOVE_DUPLICATES PCL_3RDPARTY_COMPONENTS)
-  set(CPACK_NSIS_EXTRA_INSTALL_COMMANDS "${CPACK_NSIS_EXTRA_INSTALL_COMMANDS}\n  noinstall_openni_packages:\n")
+  string(APPEND CPACK_NSIS_EXTRA_INSTALL_COMMANDS "\n  noinstall_openni_packages:\n")
 endif()
 
 if(WITH_OPENNI2)
@@ -133,8 +131,7 @@ if(WITH_OPENNI2)
         COMPONENT OpenNI2
       )
       list(APPEND PCL_3RDPARTY_COMPONENTS OpenNI2)
-      set(CPACK_NSIS_EXTRA_INSTALL_COMMANDS
-        "${CPACK_NSIS_EXTRA_INSTALL_COMMANDS}\n    ExecWait 'msiexec /i \\\"$INSTDIR\\\\3rdParty\\\\OpenNI2\\\\${OPENNI2_PACKAGE}\\\" /quiet '")
+      string(APPEND CPACK_NSIS_EXTRA_INSTALL_COMMANDS "\n    ExecWait 'msiexec /i \\\"$INSTDIR\\\\3rdParty\\\\OpenNI2\\\\${OPENNI2_PACKAGE}\\\" /quiet '")
     else()
       message("WARNING : Could not unzip ${OPENNI2_ZIP}, error code : ${_error_code}, error message : ${_error_message}")
     endif()
@@ -142,5 +139,5 @@ if(WITH_OPENNI2)
     message("WARNING : Could not download ${OPENNI2_ZIP_URL}, error code : ${_error_code}, error message : ${_error_message}")
   endif()
   list(REMOVE_DUPLICATES PCL_3RDPARTY_COMPONENTS)
-  set(CPACK_NSIS_EXTRA_INSTALL_COMMANDS "${CPACK_NSIS_EXTRA_INSTALL_COMMANDS}\n  noinstall_openni2_packages:\n")
+  string(APPEND CPACK_NSIS_EXTRA_INSTALL_COMMANDS "\n  noinstall_openni2_packages:\n")
 endif()
diff --git a/cmake/pcl_cpack.cmake b/cmake/pcl_cpack.cmake
index a759b193..5f036cd9 100644
--- a/cmake/pcl_cpack.cmake
+++ b/cmake/pcl_cpack.cmake
@@ -38,13 +38,13 @@ if(WIN32)
     set(CPACK_NSIS_PACKAGE_NAME "${PROJECT_NAME}-${PCL_VERSION_PRETTY}-AllInOne")
   endif()
   if(MSVC_VERSION EQUAL 1900)
-    set(CPACK_NSIS_PACKAGE_NAME "${CPACK_NSIS_PACKAGE_NAME}-msvc2015-${win_system_name}")
+    string(APPEND CPACK_NSIS_PACKAGE_NAME "-msvc2015-${win_system_name}")
   elseif(MSVC_VERSION MATCHES "^191[0-9]$")
-    set(CPACK_NSIS_PACKAGE_NAME "${CPACK_NSIS_PACKAGE_NAME}-msvc2017-${win_system_name}")
+    string(APPEND CPACK_NSIS_PACKAGE_NAME "-msvc2017-${win_system_name}")
   elseif(MSVC_VERSION MATCHES "^192[0-9]$")
-    set(CPACK_NSIS_PACKAGE_NAME "${CPACK_NSIS_PACKAGE_NAME}-msvc2019-${win_system_name}")
+    string(APPEND CPACK_NSIS_PACKAGE_NAME "-msvc2019-${win_system_name}")
   else()
-    set(CPACK_NSIS_PACKAGE_NAME "${CPACK_NSIS_PACKAGE_NAME}-${win_system_name}")
+    string(APPEND CPACK_NSIS_PACKAGE_NAME "-${win_system_name}")
   endif()
   set(CPACK_PACKAGE_FILE_NAME ${CPACK_NSIS_PACKAGE_NAME})
   # force CPACK_PACKAGE_INSTALL_REGISTRY_KEY because of a known limitation in cmake/cpack to be fixed in next releases
@@ -82,29 +82,29 @@ macro(PCL_MAKE_CPACK_INPUT)
 
     # add documentation
     if(WITH_DOCS)
-        set(CPACK_COMPONENTS_ALL "${CPACK_COMPONENTS_ALL} doc")
-        set(PCL_CPACK_COMPONENTS "${PCL_CPACK_COMPONENTS}\nset(CPACK_COMPONENT_DOC_GROUP \"PCL\")\n")
-        set(PCL_CPACK_COMPONENTS "${PCL_CPACK_COMPONENTS}\nset(CPACK_COMPONENT_DOC_DISPLAY_NAME \"Documentation\")\n")
-        set(PCL_CPACK_COMPONENTS "${PCL_CPACK_COMPONENTS}\nset(CPACK_COMPONENT_DOC_DESCRIPTION \"API documentation and tutorials\")\n")
+        string(APPEND CPACK_COMPONENTS_ALL " doc")
+        string(APPEND PCL_CPACK_COMPONENTS "\nset(CPACK_COMPONENT_DOC_GROUP \"PCL\")\n")
+        string(APPEND PCL_CPACK_COMPONENTS "\nset(CPACK_COMPONENT_DOC_DISPLAY_NAME \"Documentation\")\n")
+        string(APPEND PCL_CPACK_COMPONENTS "\nset(CPACK_COMPONENT_DOC_DESCRIPTION \"API documentation and tutorials\")\n")
     endif()
     # add PCLConfig
-    set(CPACK_COMPONENTS_ALL "${CPACK_COMPONENTS_ALL} pclconfig")
-    set(PCL_CPACK_COMPONENTS "${PCL_CPACK_COMPONENTS}\nset(CPACK_COMPONENT_PCLCONFIG_GROUP \"PCL\")\n")
-    set(PCL_CPACK_COMPONENTS "${PCL_CPACK_COMPONENTS}\nset(CPACK_COMPONENT_PCLCONFIG_DISPLAY_NAME \"PCLConfig\")\n")
-    set(PCL_CPACK_COMPONENTS "${PCL_CPACK_COMPONENTS}\nset(CPACK_COMPONENT_PCLCONFIG_DESCRIPTION \"Helper cmake configuration scripts used by find_package(PCL)\")\n")
+    string(APPEND CPACK_COMPONENTS_ALL " pclconfig")
+    string(APPEND PCL_CPACK_COMPONENTS "\nset(CPACK_COMPONENT_PCLCONFIG_GROUP \"PCL\")\n")
+    string(APPEND PCL_CPACK_COMPONENTS "\nset(CPACK_COMPONENT_PCLCONFIG_DISPLAY_NAME \"PCLConfig\")\n")
+    string(APPEND PCL_CPACK_COMPONENTS "\nset(CPACK_COMPONENT_PCLCONFIG_DESCRIPTION \"Helper cmake configuration scripts used by find_package(PCL)\")\n")
 
     # add 3rdParty libs
     if(BUILD_all_in_one_installer)
-        set(PCL_CPACK_COMPONENTS "${PCL_CPACK_COMPONENTS}\nset(CPACK_COMPONENT_GROUP_THIRDPARTY_DISPLAY_NAME \"3rd Party Libraries\")")
-        set(PCL_CPACK_COMPONENTS "${PCL_CPACK_COMPONENTS}\nset(CPACK_COMPONENT_GROUP_THIRDPARTY_DESCRIPTION \"3rd Party Libraries\")")
+        string(APPEND PCL_CPACK_COMPONENTS "\nset(CPACK_COMPONENT_GROUP_THIRDPARTY_DISPLAY_NAME \"3rd Party Libraries\")")
+        string(APPEND PCL_CPACK_COMPONENTS "\nset(CPACK_COMPONENT_GROUP_THIRDPARTY_DESCRIPTION \"3rd Party Libraries\")")
         foreach(dep ${PCL_3RDPARTY_COMPONENTS})
             string(TOUPPER ${dep} DEP)
-            set(PCL_CPACK_COMPONENTS "${PCL_CPACK_COMPONENTS}\nset(CPACK_COMPONENT_${DEP}_GROUP \"ThirdParty\")")
-            set(CPACK_COMPONENTS_ALL "${CPACK_COMPONENTS_ALL} ${dep}")
+            string(APPEND PCL_CPACK_COMPONENTS "\nset(CPACK_COMPONENT_${DEP}_GROUP \"ThirdParty\")")
+            string(APPEND CPACK_COMPONENTS_ALL " ${dep}")
         endforeach()
     endif()
 
-    set(PCL_CPACK_COMPONENTS "${PCL_CPACK_COMPONENTS}\nset(CPACK_COMPONENTS_ALL${CPACK_COMPONENTS_ALL})\n")
+    string(APPEND PCL_CPACK_COMPONENTS "\nset(CPACK_COMPONENTS_ALL${CPACK_COMPONENTS_ALL})\n")
     configure_file(${_cpack_cfg_in} ${PCL_CPACK_CFG_FILE} @ONLY)
 endmacro()
 
@@ -116,7 +116,7 @@ macro(PCL_CPACK_MAKE_COMPS_OPTS _var _current)
     foreach(_ss ${PCL_CPACK_SUBSYSTEMS})
         PCL_GET_SUBSYS_STATUS(_status ${_ss})
         if(_status)
-            set(_comps_list "${_comps_list} pcl_${_ss}")
+            string(APPEND _comps_list " pcl_${_ss}")
             PCL_CPACK_ADD_COMP_INFO(${_var} ${_ss})
         endif()
     endforeach()
@@ -134,7 +134,7 @@ macro(PCL_CPACK_ADD_COMP_INFO _var _ss)
     set(_deps_str)
     GET_IN_MAP(_deps PCL_SUBSYS_DEPS ${_ss})
     foreach(_dep ${_deps})
-        set(_deps_str "${_deps_str} pcl_${_dep}")
+        string(APPEND _deps_str " pcl_${_dep}")
     endforeach()
     set(${_var}
         "${${_var}}set(CPACK_COMPONENT_PCL_${_comp_name}_DEPENDS ${_deps_str})\n")
diff --git a/cmake/pcl_find_avx.cmake b/cmake/pcl_find_avx.cmake
new file mode 100644
index 00000000..d3187ea7
--- /dev/null
+++ b/cmake/pcl_find_avx.cmake
@@ -0,0 +1,41 @@
+###############################################################################
+# Check for the presence of AVX and figure out the flags to use for it.
+function(PCL_CHECK_FOR_AVX)
+  set(AVX_FLAGS)
+
+  include(CheckCXXSourceRuns)
+  
+  check_cxx_source_runs("    
+    #include <immintrin.h>
+    int main()
+    {
+      __m256i a = {0};
+      a = _mm256_abs_epi16(a);
+      return 0;
+    }"
+  HAVE_AVX2)
+
+  if(NOT HAVE_AVX2)
+    check_cxx_source_runs("
+      #include <immintrin.h>
+      int main()
+      {
+        __m256 a;
+        a = _mm256_set1_ps(0);
+        return 0;
+      }"
+    HAVE_AVX)
+  endif()
+
+# Setting the /arch defines __AVX(2)__, see here https://docs.microsoft.com/en-us/cpp/build/reference/arch-x64?view=msvc-160
+# AVX2 extends and includes AVX.
+# Setting these defines allows the compiler to use AVX instructions as well as code guarded with the defines.
+# TODO: Add AVX512 variant if needed.
+  if(MSVC)
+    if(HAVE_AVX2)
+      set(AVX_FLAGS "/arch:AVX2" PARENT_SCOPE)
+    elseif(HAVE_AVX)
+      set(AVX_FLAGS "/arch:AVX" PARENT_SCOPE)
+    endif()
+  endif()
+endfunction()
diff --git a/cmake/pcl_find_boost.cmake b/cmake/pcl_find_boost.cmake
index 13f193e1..ae6b9335 100644
--- a/cmake/pcl_find_boost.cmake
+++ b/cmake/pcl_find_boost.cmake
@@ -5,7 +5,7 @@ if(PCL_BUILD_WITH_BOOST_DYNAMIC_LINKING_WIN32 AND WIN32)
   set(Boost_USE_STATIC_LIBS OFF)
   set(Boost_USE_STATIC OFF)
   set(Boost_USE_MULTITHREAD ON)
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBOOST_ALL_DYN_LINK -DBOOST_ALL_NO_LIB")
+  string(APPEND CMAKE_CXX_FLAGS " -DBOOST_ALL_DYN_LINK -DBOOST_ALL_NO_LIB")
 else()
   if(NOT PCL_SHARED_LIBS OR WIN32)
     set(Boost_USE_STATIC_LIBS ON)
@@ -14,23 +14,22 @@ else()
 endif()
 
 set(Boost_ADDITIONAL_VERSIONS
-  "1.73.0" "1.73" "1.72.0" "1.72" "1.71.0" "1.71" "1.70.0" "1.70"
-  "1.69.0" "1.69" "1.68.0" "1.68" "1.67.0" "1.67" "1.66.0" "1.66" "1.65.1" "1.65.0" "1.65"
-  "1.64.0" "1.64" "1.63.0" "1.63" "1.62.0" "1.62" "1.61.0" "1.61" "1.60.0" "1.60"
-  "1.59.0" "1.59" "1.58.0" "1.58" "1.57.0" "1.57" "1.56.0" "1.56" "1.55.0" "1.55")
+  "1.76.0" "1.76" "1.75.0" "1.75" 
+  "1.74.0" "1.74" "1.73.0" "1.73" "1.72.0" "1.72" "1.71.0" "1.71" "1.70.0" "1.70"
+  "1.69.0" "1.69" "1.68.0" "1.68" "1.67.0" "1.67" "1.66.0" "1.66" "1.65.1" "1.65.0" "1.65")
 
 # Disable the config mode of find_package(Boost)
 set(Boost_NO_BOOST_CMAKE ON)
 
 # Optional boost modules
-find_package(Boost 1.55.0 QUIET COMPONENTS serialization mpi)
+find_package(Boost 1.65.0 QUIET COMPONENTS serialization mpi)
 if(Boost_SERIALIZATION_FOUND)
   set(BOOST_SERIALIZATION_FOUND TRUE)
 endif()
 
 # Required boost modules
 set(BOOST_REQUIRED_MODULES filesystem date_time iostreams system)
-find_package(Boost 1.55.0 REQUIRED COMPONENTS ${BOOST_REQUIRED_MODULES})
+find_package(Boost 1.65.0 REQUIRED COMPONENTS ${BOOST_REQUIRED_MODULES})
 
 if(Boost_FOUND)
   set(BOOST_FOUND TRUE)
diff --git a/cmake/pcl_find_cuda.cmake b/cmake/pcl_find_cuda.cmake
index 42b9e16c..ab1810a1 100644
--- a/cmake/pcl_find_cuda.cmake
+++ b/cmake/pcl_find_cuda.cmake
@@ -31,11 +31,11 @@ if(CUDA_FOUND)
   # https://docs.nvidia.com/cuda/cuda-toolkit-release-notes/index.html#deprecated-features
   
   if(NOT ${CUDA_VERSION_STRING} VERSION_LESS "11.0")
-    set(__cuda_arch_bin "5.2 5.3 6.0 6.1 7.0 7.2 7.5")
+    set(__cuda_arch_bin "3.5 3.7 5.0 5.2 5.3 6.0 6.1 6.2 7.0 7.2 7.5 8.0 8.6")
   elseif(NOT ${CUDA_VERSION_STRING} VERSION_LESS "10.0")
-    set(__cuda_arch_bin "3.0 3.5 5.0 5.2 5.3 6.0 6.1 7.0 7.2 7.5")
+    set(__cuda_arch_bin "3.0 3.2 3.5 3.7 5.0 5.2 5.3 6.0 6.1 6.2 7.0 7.2 7.5")
   elseif(NOT ${CUDA_VERSION_STRING} VERSION_LESS "9.0")
-    set(__cuda_arch_bin "3.0 3.5 5.0 5.2 5.3 6.0 6.1 7.0 7.2")
+    set(__cuda_arch_bin "3.0 3.2 3.5 3.7 5.0 5.2 5.3 6.0 6.1 6.2 7.0 7.2")
   endif()
 
   set(CUDA_ARCH_BIN ${__cuda_arch_bin} CACHE STRING "Specify 'real' GPU architectures to build binaries for, BIN(PTX) format is supported")
diff --git a/cmake/pcl_find_sse.cmake b/cmake/pcl_find_sse.cmake
index e6604d32..541ec580 100644
--- a/cmake/pcl_find_sse.cmake
+++ b/cmake/pcl_find_sse.cmake
@@ -1,210 +1,257 @@
 ###############################################################################
 # Check for the presence of SSE and figure out the flags to use for it.
-macro(PCL_CHECK_FOR_SSE)
-    set(SSE_FLAGS)
-    set(SSE_DEFINITIONS)
-
-    if(NOT CMAKE_CROSSCOMPILING)
-        # Test GCC/G++ and CLANG
-        if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG)
-            include(CheckCXXCompilerFlag)
-            check_cxx_compiler_flag("-march=native" HAVE_MARCH)
-            if(HAVE_MARCH)
-                list(APPEND SSE_FLAGS "-march=native")
-            else()
-                list(APPEND SSE_FLAGS "-mtune=native")
-            endif()
-            message(STATUS "Using CPU native flags for SSE optimization: ${SSE_FLAGS}")
-        endif()
-    endif()
-
-    # Unfortunately we need to check for SSE to enable "-mfpmath=sse" alongside
-    # "-march=native". The reason for this is that by default, 32bit architectures
-    # tend to use the x87 FPU (which has 80 bit internal precision), thus leading
-    # to different results than 64bit architectures which are using SSE2 (64 bit internal
-    # precision). One solution would be to use "-ffloat-store" on 32bit (see
-    # http://gcc.gnu.org/onlinedocs/gcc/Optimize-Options.html), but that slows code down,
-    # so the preferred solution is to try "-mpfmath=sse" first.
-    include(CheckCXXSourceRuns)
-    set(CMAKE_REQUIRED_FLAGS)
+function(PCL_CHECK_FOR_SSE)
+  set(SSE_FLAGS)
+  set(SSE_DEFINITIONS)
 
-    check_cxx_source_runs("
-        // Intel compiler defines an incompatible _mm_malloc signature
-        #if defined(__INTEL_COMPILER)
-            #include <malloc.h>
-        #else
-            #include <mm_malloc.h>
-        #endif
-        int main()
-        {
-          void* mem = _mm_malloc (100, 16);
-          return 0;
-        }"
-        HAVE_MM_MALLOC)
+  if(NOT CMAKE_CROSSCOMPILING)
+    # Test GCC/G++ and CLANG
+    if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG)
+      include(CheckCXXCompilerFlag)
+      check_cxx_compiler_flag("-march=native" HAVE_MARCH)
+      if(HAVE_MARCH)
+          list(APPEND SSE_FLAGS "-march=native")
+      else()
+          list(APPEND SSE_FLAGS "-mtune=native")
+      endif()
+      message(STATUS "Using CPU native flags for SSE optimization: ${SSE_FLAGS}")
+    endif()
+  endif()
 
-    check_cxx_source_runs("
-        #include <stdlib.h>
-        int main()
-        {
-          void* mem;
-          return posix_memalign (&mem, 16, 100);
-        }"
-        HAVE_POSIX_MEMALIGN)
+  # Unfortunately we need to check for SSE to enable "-mfpmath=sse" alongside
+  # "-march=native". The reason for this is that by default, 32bit architectures
+  # tend to use the x87 FPU (which has 80 bit internal precision), thus leading
+  # to different results than 64bit architectures which are using SSE2 (64 bit internal
+  # precision). One solution would be to use "-ffloat-store" on 32bit (see
+  # http://gcc.gnu.org/onlinedocs/gcc/Optimize-Options.html), but that slows code down,
+  # so the preferred solution is to try "-mpfmath=sse" first.
+  include(CheckCXXSourceRuns)
+  set(CMAKE_REQUIRED_FLAGS)
+  set(SSE_LEVEL 0)
+
+  check_cxx_source_runs("
+    // Intel compiler defines an incompatible _mm_malloc signature
+    #if defined(__INTEL_COMPILER)
+        #include <malloc.h>
+    #else
+        #include <mm_malloc.h>
+    #endif
+    int main()
+    {
+      void* mem = _mm_malloc (100, 16);
+      return 0;
+    }"
+    HAVE_MM_MALLOC)
+
+  check_cxx_source_runs("
+    #include <stdlib.h>
+    int main()
+    {
+      void* mem;
+      return posix_memalign (&mem, 16, 100);
+    }"
+    HAVE_POSIX_MEMALIGN)
 
+  if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG)
+    set(CMAKE_REQUIRED_FLAGS "-msse4.2")
+  endif()
+
+  check_cxx_source_runs("
+    #include <emmintrin.h>
+    #include <nmmintrin.h>
+    int main ()
+    {
+      long long a[2] = {  1, 2 };
+      long long b[2] = { -1, 3 };
+      long long c[2];
+      __m128i va = _mm_loadu_si128 ((__m128i*)a);
+      __m128i vb = _mm_loadu_si128 ((__m128i*)b);
+      __m128i vc = _mm_cmpgt_epi64 (va, vb);
+
+      _mm_storeu_si128 ((__m128i*)c, vc);
+      if (c[0] == -1LL && c[1] == 0LL)
+        return (0);
+      else
+        return (1);
+    }"
+    HAVE_SSE4_2_EXTENSIONS)
+        
+  if(HAVE_SSE4_2_EXTENSIONS)
+    set(SSE_LEVEL 4.2)
+  endif()
+
+  if(SSE_LEVEL LESS 4.2)
     if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG)
-        set(CMAKE_REQUIRED_FLAGS "-msse4.2")
+      set(CMAKE_REQUIRED_FLAGS "-msse4.1")
     endif()
 
     check_cxx_source_runs("
-        #include <emmintrin.h>
-        #include <nmmintrin.h>
-        int main ()
-        {
-          long long a[2] = {  1, 2 };
-          long long b[2] = { -1, 3 };
-          long long c[2];
-          __m128i va = _mm_loadu_si128 ((__m128i*)a);
-          __m128i vb = _mm_loadu_si128 ((__m128i*)b);
-          __m128i vc = _mm_cmpgt_epi64 (va, vb);
-
-          _mm_storeu_si128 ((__m128i*)c, vc);
-          if (c[0] == -1LL && c[1] == 0LL)
-            return (0);
-          else
-            return (1);
-        }"
-        HAVE_SSE4_2_EXTENSIONS)
+      #include <smmintrin.h>
+      int main ()
+      {
+        __m128 a, b;
+        float vals[4] = {1, 2, 3, 4};
+        const int mask = 123;
+        a = _mm_loadu_ps (vals);
+        b = a;
+        b = _mm_dp_ps (a, a, mask);
+        _mm_storeu_ps (vals,b);
+        return (0);
+      }"
+      HAVE_SSE4_1_EXTENSIONS)
+      
+    if(HAVE_SSE4_1_EXTENSIONS)
+      set(SSE_LEVEL 4.1)
+    endif()
+  endif()
 
+  if(SSE_LEVEL LESS 4.1)
     if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG)
-        set(CMAKE_REQUIRED_FLAGS "-msse4.1")
+      set(CMAKE_REQUIRED_FLAGS "-mssse3")
     endif()
-
+    
     check_cxx_source_runs("
-        #include <smmintrin.h>
-        int main ()
-        {
-          __m128 a, b;
-          float vals[4] = {1, 2, 3, 4};
-          const int mask = 123;
-          a = _mm_loadu_ps (vals);
-          b = a;
-          b = _mm_dp_ps (a, a, mask);
-          _mm_storeu_ps (vals,b);
-          return (0);
-        }"
-        HAVE_SSE4_1_EXTENSIONS)
+      #include <tmmintrin.h>
+      int main ()
+      {
+        __m128i a, b;
+        int vals[4] = {-1, -2, -3, -4};
+        a = _mm_loadu_si128 ((const __m128i*)vals);
+        b = _mm_abs_epi32 (a);
+        _mm_storeu_si128 ((__m128i*)vals, b);
+        return (0);
+      }"
+      HAVE_SSSE3_EXTENSIONS)
 
+    if(HAVE_SSSE3_EXTENSIONS)
+      set(SSE_LEVEL 3.1)
+    endif()
+  endif()
+
+  if(SSE_LEVEL LESS 3.1)
     if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG)
-        set(CMAKE_REQUIRED_FLAGS "-mssse3")
+      set(CMAKE_REQUIRED_FLAGS "-msse3")
     endif()
 
     check_cxx_source_runs("
-        #include <tmmintrin.h>
-        int main ()
-        {
-          __m128i a, b;
-          int vals[4] = {-1, -2, -3, -4};
-          a = _mm_loadu_si128 ((const __m128i*)vals);
-          b = _mm_abs_epi32 (a);
-          _mm_storeu_si128 ((__m128i*)vals, b);
+      #include <pmmintrin.h>
+      int main ()
+      {
+          __m128d a, b;
+          double vals[2] = {0};
+          a = _mm_loadu_pd (vals);
+          b = _mm_hadd_pd (a,a);
+          _mm_storeu_pd (vals, b);
           return (0);
-        }"
-        HAVE_SSSE3_EXTENSIONS)
+      }"
+      HAVE_SSE3_EXTENSIONS)
 
-    if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG)
-        set(CMAKE_REQUIRED_FLAGS "-msse3")
+    if(HAVE_SSE3_EXTENSIONS)
+      set( SSE_LEVEL 3.0)
     endif()
+  endif()
 
-    check_cxx_source_runs("
-        #include <pmmintrin.h>
-        int main ()
-        {
-            __m128d a, b;
-            double vals[2] = {0};
-            a = _mm_loadu_pd (vals);
-            b = _mm_hadd_pd (a,a);
-            _mm_storeu_pd (vals, b);
-            return (0);
-        }"
-        HAVE_SSE3_EXTENSIONS)
-
+  if(SSE_LEVEL LESS 3.0)
     if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG)
-        set(CMAKE_REQUIRED_FLAGS "-msse2")
+      set(CMAKE_REQUIRED_FLAGS "-msse2")
     elseif(MSVC AND NOT CMAKE_CL_64)
-        set(CMAKE_REQUIRED_FLAGS "/arch:SSE2")
+      set(CMAKE_REQUIRED_FLAGS "/arch:SSE2")
     endif()
 
     check_cxx_source_runs("
-        #include <emmintrin.h>
-        int main ()
-        {
-            __m128d a, b;
-            double vals[2] = {0};
-            a = _mm_loadu_pd (vals);
-            b = _mm_add_pd (a,a);
-            _mm_storeu_pd (vals,b);
-            return (0);
-        }"
-        HAVE_SSE2_EXTENSIONS)
+      #include <emmintrin.h>
+      int main ()
+      {
+          __m128d a, b;
+          double vals[2] = {0};
+          a = _mm_loadu_pd (vals);
+          b = _mm_add_pd (a,a);
+          _mm_storeu_pd (vals,b);
+          return (0);
+      }"
+      HAVE_SSE2_EXTENSIONS)
+
+    if(HAVE_SSE2_EXTENSIONS)
+      set(SSE_LEVEL 2.0)
+    endif()
+  endif()
 
+  if(SSE_LEVEL LESS 2.0)
     if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG)
-        set(CMAKE_REQUIRED_FLAGS "-msse")
+      set(CMAKE_REQUIRED_FLAGS "-msse")
     elseif(MSVC AND NOT CMAKE_CL_64)
-        set(CMAKE_REQUIRED_FLAGS "/arch:SSE")
+      set(CMAKE_REQUIRED_FLAGS "/arch:SSE")
     endif()
 
     check_cxx_source_runs("
-        #include <xmmintrin.h>
-        int main ()
-        {
-            __m128 a, b;
-            float vals[4] = {0};
-            a = _mm_loadu_ps (vals);
-            b = a;
-            b = _mm_add_ps (a,b);
-            _mm_storeu_ps (vals,b);
-            return (0);
-        }"
-        HAVE_SSE_EXTENSIONS)
-
-    set(CMAKE_REQUIRED_FLAGS)
+      #include <xmmintrin.h>
+      int main ()
+      {
+          __m128 a, b;
+          float vals[4] = {0};
+          a = _mm_loadu_ps (vals);
+          b = a;
+          b = _mm_add_ps (a,b);
+          _mm_storeu_ps (vals,b);
+          return (0);
+      }"
+      HAVE_SSE_EXTENSIONS)
 
-    if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG)
-        if(HAVE_SSE4_2_EXTENSIONS)
-            list(APPEND SSE_FLAGS "-msse4.2" "-mfpmath=sse")
-        elseif(HAVE_SSE4_1_EXTENSIONS)
-            list(APPEND SSE_FLAGS "-msse4.1" "-mfpmath=sse")
-        elseif(HAVE_SSSE3_EXTENSIONS)
-            list(APPEND SSE_FLAGS "-mssse3" "-mfpmath=sse")
-        elseif(HAVE_SSE3_EXTENSIONS)
-            list(APPEND SSE_FLAGS "-msse3" "-mfpmath=sse")
-        elseif(HAVE_SSE2_EXTENSIONS)
-            list(APPEND SSE_FLAGS "-msse2" "-mfpmath=sse")
-        elseif(HAVE_SSE_EXTENSIONS)
-            list(APPEND SSE_FLAGS "-msse" "-mfpmath=sse")
-        else()
-            # Setting -ffloat-store to alleviate 32bit vs 64bit discrepancies on non-SSE
-            # platforms.
-            list(APPEND SSE_FLAGS "-ffloat-store")
-        endif()
-    elseif(MSVC AND NOT CMAKE_CL_64)
-        if(HAVE_SSE2_EXTENSIONS)
-            list(APPEND SSE_FLAGS "/arch:SSE2")
-        elseif(HAVE_SSE_EXTENSIONS)
-            list(APPEND SSE_FLAGS "/arch:SSE")
-        endif()
-    endif()
-
-    if(MSVC)
-        if(HAVE_SSSE3_EXTENSIONS)
-            set(SSE_DEFINITIONS "${SSE_DEFINITIONS} -D__SSSE3__")
-        endif()
-        if(HAVE_SSE2_EXTENSIONS)
-            set(SSE_DEFINITIONS "${SSE_DEFINITIONS} -D__SSE2__")
-        endif()
-        if(HAVE_SSE_EXTENSIONS)
-            set(SSE_DEFINITIONS "${SSE_DEFINITIONS} -D__SSE__")
-        endif()
-    endif()
-    string(REPLACE ";" " " SSE_FLAGS_STR "${SSE_FLAGS}")
-endmacro()
+    if(HAVE_SSE_EXTENSIONS)
+      set(SSE_LEVEL 1.0)
+    endif()
+  endif()
+  
+  if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG)
+    if(SSE_LEVEL GREATER_EQUAL 1.0)
+      if(SSE_LEVEL GREATER_EQUAL 4.2)
+        set(SSE_FLAGS "-msse4.2")
+      elseif(SSE_LEVEL GREATER_EQUAL 4.1)
+        set(SSE_FLAGS "-msse4.1")
+      elseif(SSE_LEVEL GREATER_EQUAL 3.1)
+        set(SSE_FLAGS "-msse3")
+      elseif(SSE_LEVEL GREATER_EQUAL 3.0)
+        set(SSE_FLAGS "-msse3")
+      elseif(SSE_LEVEL GREATER_EQUAL 2.0)
+        set(SSE_FLAGS "-msse2")
+      else()
+        set(SSE_FLAGS "-msse")
+      endif()
+      string(APPEND SSE_FLAGS " -mfpmath=sse")
+    else()
+      # Setting -ffloat-store to alleviate 32bit vs 64bit discrepancies on non-SSE
+      # platforms.
+      list(APPEND SSE_FLAGS "-ffloat-store")
+    endif()
+  elseif(MSVC AND NOT CMAKE_SIZEOF_VOID_P)
+    if(SSE_LEVEL GREATER_EQUAL 2.0)
+      set( SSE_FLAGS "/arch:SSE2")
+    elseif(SSE_LEVEL GREATER_EQUAL 1.0)
+      set( SSE_FLAGS "/arch:SSE")
+    endif()
+  elseif(MSVC)
+    if(SSE_LEVEL GREATER_EQUAL 4.2)
+      string(APPEND SSE_DEFINITIONS " -D__SSE4_2__")
+    endif()
+    if(SSE_LEVEL GREATER_EQUAL 4.1)
+      string(APPEND SSE_DEFINITIONS " -D__SSE4_1__")
+    endif()
+    if(SSE_LEVEL GREATER_EQUAL 3.1)
+      string(APPEND SSE_DEFINITIONS " -D__SSSE3__")
+    endif()
+    if(SSE_LEVEL GREATER_EQUAL 3.0)
+      string(APPEND SSE_DEFINITIONS " -D__SSE3__")
+    endif()
+    if(SSE_LEVEL GREATER_EQUAL 2.0)
+      string(APPEND SSE_DEFINITIONS " -D__SSE2__")
+    endif()
+    if(SSE_LEVEL GREATER_EQUAL 1.0)
+      string(APPEND SSE_DEFINITIONS " -D__SSE__")
+    endif()
+  endif()
+
+  set(SSE_FLAGS ${SSE_FLAGS} PARENT_SCOPE)
+  set(SSE_DEFINITIONS ${SSE_DEFINITIONS} PARENT_SCOPE)
+
+  unset(CMAKE_REQUIRED_FLAGS)
+endfunction()
diff --git a/cmake/pcl_find_vtk.cmake b/cmake/pcl_find_vtk.cmake
new file mode 100644
index 00000000..d1037b4e
--- /dev/null
+++ b/cmake/pcl_find_vtk.cmake
@@ -0,0 +1,153 @@
+function(checkVTKComponents)
+  cmake_parse_arguments(PARAM "" "MISSING_COMPONENTS" "COMPONENTS" ${ARGN})
+
+  set(vtkMissingComponents)
+  
+  foreach(vtkComponent ${PARAM_COMPONENTS})
+    if (VTK_VERSION VERSION_LESS 9.0)
+      if (NOT TARGET ${vtkComponent})
+        list(APPEND vtkMissingComponents ${vtkComponent})
+      endif()
+    else()
+      if (NOT TARGET VTK::${vtkComponent})
+        list(APPEND vtkMissingComponents ${vtkComponent})
+      endif()
+    endif()
+  endforeach()
+  
+  set(${PARAM_MISSING_COMPONENTS} ${vtkMissingComponents} PARENT_SCOPE)
+endfunction()
+
+# Start with a generic call to find any VTK version we are supporting, so we retrieve
+# the version of VTK. As the module names were changed from VTK 8.2 to 9.0, we don't
+# search explicitly for modules. Furthermore we don't pass required minimum version 6.2
+# to find_package because then it only accept versions with same major version.
+find_package(VTK)
+
+if(NOT VTK_FOUND)
+    return()
+endif()
+
+if(VTK_FOUND AND (VTK_VERSION VERSION_LESS 6.2))
+  message(WARNING "The minimum required version of VTK is 6.2, but found ${VTK_VERSION}")
+  set(VTK_FOUND FALSE)
+  return()
+endif()
+
+set(NON_PREFIX_PCL_VTK_COMPONENTS
+  ChartsCore
+  CommonColor
+  CommonComputationalGeometry
+  CommonCore
+  CommonDataModel
+  CommonExecutionModel
+  CommonMath
+  CommonMisc
+  CommonTransforms
+  FiltersCore
+  FiltersExtraction
+  FiltersGeneral
+  FiltersGeometry
+  FiltersModeling
+  FiltersSources
+  ImagingCore
+  ImagingSources
+  InteractionStyle
+  InteractionWidgets
+  IOCore
+  IOGeometry
+  IOImage
+  IOLegacy
+  IOPLY
+  RenderingAnnotation
+  RenderingCore
+  RenderingContext2D
+  RenderingLOD
+  RenderingFreeType
+  ViewsCore
+  ViewsContext2D
+)
+
+#If VTK version 6 use OpenGL
+if(VTK_VERSION VERSION_LESS 7.0)
+  set(VTK_RENDERING_BACKEND "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.")
+
+#If VTK version 7,8 or 9 use OpenGL2
+else()
+  set(VTK_RENDERING_BACKEND "OpenGL2")
+  set(VTK_RENDERING_BACKEND_OPENGL_VERSION "2")
+endif()
+
+list(APPEND NON_PREFIX_PCL_VTK_COMPONENTS Rendering${VTK_RENDERING_BACKEND})
+
+#Append vtk to components if version is <9.0
+if(VTK_VERSION VERSION_LESS 9.0)
+  foreach(vtkComponent ${NON_PREFIX_PCL_VTK_COMPONENTS})
+    set(vtkComponent "vtk${vtkComponent}")
+    list(APPEND PCL_VTK_COMPONENTS ${vtkComponent})
+  endforeach()
+else()
+  set(PCL_VTK_COMPONENTS ${NON_PREFIX_PCL_VTK_COMPONENTS})
+endif()
+
+# Check if requested modules are available
+checkVTKComponents(COMPONENTS ${PCL_VTK_COMPONENTS} MISSING_COMPONENTS vtkMissingComponents)
+
+if (vtkMissingComponents)
+  set(VTK_FOUND FALSE)
+  message(WARNING "Missing vtk modules: ${vtkMissingComponents}")
+endif()
+
+if("vtkGUISupportQt" IN_LIST VTK_MODULES_ENABLED AND "vtkRenderingQt" IN_LIST VTK_MODULES_ENABLED)
+  set(HAVE_QVTK TRUE)
+  #PCL_VTK_COMPONENTS is used in the PCLConfig.cmake to refind the required modules.
+  #Pre vtk 9.0, all vtk libraries are linked into pcl_visualizer.
+  #Subprojects can link against pcl_visualizer and directly use VTK-QT libraries.
+  list(APPEND PCL_VTK_COMPONENTS vtkRenderingQt vtkGUISupportQt)
+elseif("GUISupportQt" IN_LIST VTK_AVAILABLE_COMPONENTS AND "RenderingQt" IN_LIST VTK_AVAILABLE_COMPONENTS)
+  set(HAVE_QVTK TRUE)
+  #PCL_VTK_COMPONENTS is used in the PCLConfig.cmake to refind the required modules.
+  #Post vtk 9.0, only required libraries are linked against pcl_visualizer.
+  #Subprojects need to manually link to VTK-QT libraries.
+  list(APPEND PCL_VTK_COMPONENTS RenderingQt GUISupportQt)
+else()
+  unset(HAVE_QVTK)
+endif()
+
+if(PCL_SHARED_LIBS OR (NOT (PCL_SHARED_LIBS) AND NOT (VTK_BUILD_SHARED_LIBS)))
+  if(VTK_VERSION VERSION_LESS 9.0)
+    if(VTK_USE_FILE)
+      include(${VTK_USE_FILE})
+    endif()
+  endif()
+  
+  if(APPLE)
+    option(VTK_USE_COCOA "Use Cocoa for VTK render windows" ON)
+    mark_as_advanced(VTK_USE_COCOA)
+  endif()
+else()
+  set(VTK_FOUND OFF)
+  message("Warning: You are to build PCL in STATIC but VTK is SHARED!")
+  message("Warning: VTK disabled!")
+endif()
+
+message(STATUS "VTK version: ${VTK_VERSION}")
+message(STATUS "VTK rendering backend: ${VTK_RENDERING_BACKEND}")
+
+if(HAVE_QVTK)
+  message(STATUS "VTK Qt support: YES")
+else()
+  message(STATUS "VTK Qt support: NOTFOUND")
+endif()
+
+if(VTK_INCLUDE_DIRS)
+  message(STATUS "VTK include: ${VTK_INCLUDE_DIRS}")
+ENDIF()
+
+if(VTK_LIBRARIES)
+  message(STATUS "VTK libs: ${VTK_LIBRARIES}")
+endif()
diff --git a/cmake/pcl_options.cmake b/cmake/pcl_options.cmake
index 52b590f9..6570d75f 100644
--- a/cmake/pcl_options.cmake
+++ b/cmake/pcl_options.cmake
@@ -22,8 +22,10 @@ mark_as_advanced(PCL_SHARED_LIBS)
 option(PCL_BUILD_WITH_BOOST_DYNAMIC_LINKING_WIN32 "Build against a dynamically linked Boost on Win32 platforms." OFF)
 mark_as_advanced(PCL_BUILD_WITH_BOOST_DYNAMIC_LINKING_WIN32)
 
-# Build with dynamic linking for FLANN (advanced users)
-option(PCL_BUILD_WITH_FLANN_DYNAMIC_LINKING_WIN32 "Build against a dynamically linked FLANN on Win32 platforms." OFF)
+# Build with shared/static linking for FLANN (advanced users)
+set(PCL_FLANN_REQUIRED_TYPE "DONTCARE" CACHE STRING "Select build type to use (STATIC/SHARED).")
+set_property(CACHE PCL_FLANN_REQUIRED_TYPE PROPERTY STRINGS DONTCARE SHARED STATIC)
+
 mark_as_advanced(PCL_BUILD_WITH_FLANN_DYNAMIC_LINKING_WIN32)
 
 # Build with dynamic linking for QHull (advanced users)
@@ -42,6 +44,12 @@ mark_as_advanced(PCL_NO_PRECOMPILE)
 option(PCL_ENABLE_SSE "Enable or Disable SSE optimizations." ON)
 mark_as_advanced(PCL_ENABLE_SSE)
 
+if(WIN32)
+  # Enable or Disable the check for AVX optimizations
+  option(PCL_ENABLE_AVX "Enable or Disable AVX optimizations." ON)
+  mark_as_advanced(PCL_ENABLE_AVX)
+endif()
+
 # Allow the user to enable compiler cache
 option(PCL_ENABLE_CCACHE "Enable using compiler cache for compilation" OFF)
 mark_as_advanced(PCL_ENABLE_CCACHE)
@@ -81,3 +89,6 @@ endif()
 # (Used to prevent gpu tests from executing in CI where GPU hardware is unavailable)
 option(PCL_DISABLE_GPU_TESTS "Disable running GPU tests. If disabled, tests will still be built." OFF)
 
+# Set whether visualizations tests should be run
+# (Used to prevent visualizations tests from executing in CI where visualization is unavailable)
+option(PCL_DISABLE_VISUALIZATION_TESTS "Disable running visualizations tests. If disabled, tests will still be built." OFF)
diff --git a/cmake/pcl_pclconfig.cmake b/cmake/pcl_pclconfig.cmake
index 2332eafc..42880bc6 100644
--- a/cmake/pcl_pclconfig.cmake
+++ b/cmake/pcl_pclconfig.cmake
@@ -22,35 +22,35 @@ foreach(_ss ${PCL_SUBSYSTEMS_MODULES})
   string(REGEX MATCH "^tests_" _is_test ${_ss})
 
   if(_status AND NOT _is_test)
-    set(PCLCONFIG_AVAILABLE_COMPONENTS "${PCLCONFIG_AVAILABLE_COMPONENTS} ${_ss}")
-    set(PCLCONFIG_AVAILABLE_COMPONENTS_LIST "${PCLCONFIG_AVAILABLE_COMPONENTS_LIST}\n# - ${_ss}")
+    string(APPEND PCLCONFIG_AVAILABLE_COMPONENTS " ${_ss}")
+    string(APPEND PCLCONFIG_AVAILABLE_COMPONENTS_LIST "\n# - ${_ss}")
     GET_IN_MAP(_deps PCL_SUBSYS_DEPS ${_ss})
     if(_deps)
-      set(PCLCONFIG_INTERNAL_DEPENDENCIES "${PCLCONFIG_INTERNAL_DEPENDENCIES}set(pcl_${_ss}_int_dep ")
+      string(APPEND PCLCONFIG_INTERNAL_DEPENDENCIES "set(pcl_${_ss}_int_dep ")
       foreach(_dep ${_deps})
-        set(PCLCONFIG_INTERNAL_DEPENDENCIES "${PCLCONFIG_INTERNAL_DEPENDENCIES}${_dep} ")
+        string(APPEND PCLCONFIG_INTERNAL_DEPENDENCIES "${_dep} ")
       endforeach()
-      set(PCLCONFIG_INTERNAL_DEPENDENCIES "${PCLCONFIG_INTERNAL_DEPENDENCIES})\n")
+      string(APPEND PCLCONFIG_INTERNAL_DEPENDENCIES ")\n")
     endif()
     GET_IN_MAP(_ext_deps PCL_SUBSYS_EXT_DEPS ${_ss})
     if(_ext_deps)
-      set(PCLCONFIG_EXTERNAL_DEPENDENCIES "${PCLCONFIG_EXTERNAL_DEPENDENCIES}set(pcl_${_ss}_ext_dep ")
+      string(APPEND PCLCONFIG_EXTERNAL_DEPENDENCIES "set(pcl_${_ss}_ext_dep ")
       foreach(_ext_dep ${_ext_deps})
-        set(PCLCONFIG_EXTERNAL_DEPENDENCIES "${PCLCONFIG_EXTERNAL_DEPENDENCIES}${_ext_dep} ")
+        string(APPEND PCLCONFIG_EXTERNAL_DEPENDENCIES "${_ext_dep} ")
       endforeach()
-      set(PCLCONFIG_EXTERNAL_DEPENDENCIES "${PCLCONFIG_EXTERNAL_DEPENDENCIES})\n")
+      string(APPEND PCLCONFIG_EXTERNAL_DEPENDENCIES ")\n")
     endif()
     GET_IN_MAP(_opt_deps PCL_SUBSYS_OPT_DEPS ${_ss})
     if(_opt_deps)
-      set(PCLCONFIG_OPTIONAL_DEPENDENCIES "${PCLCONFIG_OPTIONAL_DEPENDENCIES}set(pcl_${_ss}_opt_dep ")
+      string(APPEND PCLCONFIG_OPTIONAL_DEPENDENCIES "set(pcl_${_ss}_opt_dep ")
       foreach(_opt_dep ${_opt_deps})
         string(TOUPPER "WITH_${_opt_dep}" _tmp)
         string(REGEX REPLACE "-(.*)" "" _condition ${_tmp}) #libusb-1.0 case
         if(${_condition})
-          set(PCLCONFIG_OPTIONAL_DEPENDENCIES "${PCLCONFIG_OPTIONAL_DEPENDENCIES}${_opt_dep} ")
+          string(APPEND PCLCONFIG_OPTIONAL_DEPENDENCIES "${_opt_dep} ")
         endif()
       endforeach()
-      set(PCLCONFIG_OPTIONAL_DEPENDENCIES "${PCLCONFIG_OPTIONAL_DEPENDENCIES})\n")
+      string(APPEND PCLCONFIG_OPTIONAL_DEPENDENCIES ")\n")
     endif()
 
     #look for subsystems
@@ -60,15 +60,15 @@ foreach(_ss ${PCL_SUBSYSTEMS_MODULES})
       foreach(_sub ${${PCL_SUBSYS_SUBSYS}})
         PCL_GET_SUBSUBSYS_STATUS(_sub_status ${_ss} ${_sub})
         if(_sub_status)
-          set(PCLCONFIG_AVAILABLE_COMPONENTS "${PCLCONFIG_AVAILABLE_COMPONENTS} ${_sub}")
-          set(PCLCONFIG_AVAILABLE_COMPONENTS_LIST "${PCLCONFIG_AVAILABLE_COMPONENTS_LIST}\n# - ${_sub}")
+          string(APPEND PCLCONFIG_AVAILABLE_COMPONENTS " ${_sub}")
+          string(APPEND PCLCONFIG_AVAILABLE_COMPONENTS_LIST "\n# - ${_sub}")
           GET_IN_MAP(_deps PCL_SUBSYS_DEPS ${_ss}_${sub})
           if(_deps)
-            set(PCLCONFIG_INTERNAL_DEPENDENCIES "${PCLCONFIG_INTERNAL_DEPENDENCIES}set(pcl_${_sub}_int_dep ")
+            string(APPEND PCLCONFIG_INTERNAL_DEPENDENCIES "set(pcl_${_sub}_int_dep ")
             foreach(_dep ${_deps})
-              set(PCLCONFIG_INTERNAL_DEPENDENCIES "${PCLCONFIG_INTERNAL_DEPENDENCIES}${_dep} ")
+              string(APPEND PCLCONFIG_INTERNAL_DEPENDENCIES "${_dep} ")
             endforeach()
-            set(PCLCONFIG_INTERNAL_DEPENDENCIES "${PCLCONFIG_INTERNAL_DEPENDENCIES})\n")
+            string(APPEND PCLCONFIG_INTERNAL_DEPENDENCIES ")\n")
           endif()
         endif()
       endforeach()
@@ -79,10 +79,10 @@ endforeach()
 #Boost modules
 set(PCLCONFIG_AVAILABLE_BOOST_MODULES "system filesystem date_time iostreams")
 if(Boost_SERIALIZATION_FOUND)
-  set(PCLCONFIG_AVAILABLE_BOOST_MODULES "${PCLCONFIG_AVAILABLE_BOOST_MODULES} serialization")
+  string(APPEND PCLCONFIG_AVAILABLE_BOOST_MODULES " serialization")
 endif()
 if(Boost_CHRONO_FOUND)
-  set(PCLCONFIG_AVAILABLE_BOOST_MODULES "${PCLCONFIG_AVAILABLE_BOOST_MODULES} chrono")
+  string(APPEND PCLCONFIG_AVAILABLE_BOOST_MODULES " chrono")
 endif()
 
 configure_file("${PCL_SOURCE_DIR}/PCLConfig.cmake.in"
diff --git a/cmake/pcl_targets.cmake b/cmake/pcl_targets.cmake
index 88ce7f90..91f24041 100644
--- a/cmake/pcl_targets.cmake
+++ b/cmake/pcl_targets.cmake
@@ -114,7 +114,8 @@ macro(PCL_SUBSYS_DEPEND _var _name)
     if(SUBSYS_EXT_DEPS)
       foreach(_dep ${SUBSYS_EXT_DEPS})
         string(TOUPPER "${_dep}_found" EXT_DEP_FOUND)
-        if(NOT EXT_DEP_FOUND)
+        #Variable EXT_DEP_FOUND expands to ie. QHULL_FOUND which in turn is then used to see if the EXT_DEPS is found.
+        if(NOT ${EXT_DEP_FOUND})
           set(${_var} FALSE)
           PCL_SET_SUBSYS_STATUS(${_name} FALSE "Requires external library ${_dep}.")
         endif()
@@ -169,7 +170,7 @@ macro(PCL_SUBSUBSYS_DEPEND _var _parent _name)
     if(SUBSUBSYS_EXT_DEPS)
       foreach(_dep ${SUBSUBSYS_EXT_DEPS})
         string(TOUPPER "${_dep}_found" EXT_DEP_FOUND)
-        if(NOT EXT_DEP_FOUND)
+        if(NOT ${EXT_DEP_FOUND})
           set(${_var} FALSE)
           PCL_SET_SUBSYS_STATUS(${_parent}_${_name} FALSE "Requires external library ${_dep}.")
         endif()
@@ -410,6 +411,46 @@ macro(PCL_ADD_TEST _name _exename)
   add_dependencies(tests ${_exename})
 endmacro()
 
+###############################################################################
+# Add a benchmark target.
+# _name The benchmark name.
+# ARGN :
+#    FILES the source files for the benchmark
+#    ARGUMENTS Arguments for benchmark executable
+#    LINK_WITH link benchmark executable with libraries
+function(PCL_ADD_BENCHMARK _name)
+  set(options)
+  set(oneValueArgs)
+  set(multiValueArgs FILES ARGUMENTS LINK_WITH)
+  cmake_parse_arguments(PCL_ADD_BENCHMARK "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN})
+
+  add_executable(benchmark_${_name} ${PCL_ADD_BENCHMARK_FILES})
+  set_target_properties(benchmark_${_name} PROPERTIES FOLDER "Benchmarks")
+  target_link_libraries(benchmark_${_name} benchmark::benchmark ${PCL_ADD_BENCHMARK_LINK_WITH})
+  set_target_properties(benchmark_${_name} PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR})
+  
+  #Only applies to MSVC
+  if(MSVC)
+    #Requires CMAKE version 3.13.0
+    get_target_property(BenchmarkArgumentWarningShown run_benchmarks PCL_BENCHMARK_ARGUMENTS_WARNING_SHOWN)
+    if(CMAKE_VERSION VERSION_LESS "3.13.0" AND (NOT BenchmarkArgumentWarningShown))
+      message(WARNING "Arguments for benchmark projects are not added - this requires at least CMake 3.13. Can be added manually in \"Project settings -> Debugging -> Command arguments\"")
+      set_target_properties(run_benchmarks PROPERTIES PCL_BENCHMARK_ARGUMENTS_WARNING_SHOWN TRUE)
+    else()
+      #Only add if there are arguments to test
+      if(PCL_ADD_BENCHMARK_ARGUMENTS)
+        string (REPLACE ";" " " PCL_ADD_BENCHMARK_ARGUMENTS_STR "${PCL_ADD_BENCHMARK_ARGUMENTS}")
+        set_target_properties(benchmark_${_name} PROPERTIES VS_DEBUGGER_COMMAND_ARGUMENTS ${PCL_ADD_BENCHMARK_ARGUMENTS_STR})
+      endif()
+    endif()
+  endif()
+  
+  add_custom_target(run_benchmark_${_name} benchmark_${_name} ${PCL_ADD_BENCHMARK_ARGUMENTS})
+  set_target_properties(run_benchmark_${_name} PROPERTIES FOLDER "Benchmarks")
+  
+  add_dependencies(run_benchmarks run_benchmark_${_name})
+endfunction()
+
 ###############################################################################
 # Add an example target.
 # _name The example name.
@@ -486,11 +527,11 @@ function(PCL_MAKE_PKGCONFIG _name)
   set(PKG_LIBFLAGS ${PKGCONFIG_LIB_FLAGS})
   LIST_TO_STRING(PKG_EXTERNAL_DEPS "${PKGCONFIG_EXT_DEPS}")
   foreach(_dep ${PKGCONFIG_PCL_DEPS})
-    set(PKG_EXTERNAL_DEPS "${PKG_EXTERNAL_DEPS} pcl_${_dep}-${PCL_VERSION_MAJOR}.${PCL_VERSION_MINOR}")
+    string(APPEND PKG_EXTERNAL_DEPS " pcl_${_dep}-${PCL_VERSION_MAJOR}.${PCL_VERSION_MINOR}")
   endforeach()
   set(PKG_INTERNAL_DEPS "")
   foreach(_dep ${PKGCONFIG_INT_DEPS})
-    set(PKG_INTERNAL_DEPS "${PKG_INTERNAL_DEPS} -l${_dep}")
+    string(APPEND PKG_INTERNAL_DEPS " -l${_dep}")
   endforeach()
 
   set(_pc_file ${CMAKE_CURRENT_BINARY_DIR}/${_name}-${PCL_VERSION_MAJOR}.${PCL_VERSION_MINOR}.pc)
@@ -661,6 +702,8 @@ endmacro()
 ###############################################################################
 # Write a report on the build/not-build status of the subsystems
 macro(PCL_WRITE_STATUS_REPORT)
+  message(STATUS "PCL build with following flags:")
+  message(STATUS "${CMAKE_CXX_FLAGS}")
   message(STATUS "The following subsystems will be built:")
   foreach(_ss ${PCL_SUBSYSTEMS})
     PCL_GET_SUBSYS_STATUS(_status ${_ss})
@@ -672,11 +715,11 @@ macro(PCL_WRITE_STATUS_REPORT)
         foreach(_sub ${${PCL_SUBSYS_SUBSYS}})
           PCL_GET_SUBSYS_STATUS(_sub_status ${_ss}_${_sub})
           if(_sub_status)
-            set(will_build "${will_build}\n       |_ ${_sub}")
+            string(APPEND will_build "\n       |_ ${_sub}")
           endif()
         endforeach()
         if(NOT ("${will_build}" STREQUAL ""))
-          set(message_text  "${message_text}\n       building: ${will_build}")
+          string(APPEND message_text "\n       building: ${will_build}")
         endif()
         set(wont_build)
         foreach(_sub ${${PCL_SUBSYS_SUBSYS}})
@@ -684,11 +727,11 @@ macro(PCL_WRITE_STATUS_REPORT)
           PCL_GET_SUBSYS_HYPERSTATUS(_sub_hyper_status ${_ss}_${sub})
           if(NOT _sub_status OR ("${_sub_hyper_status}" STREQUAL "AUTO_OFF"))
             GET_IN_MAP(_reason PCL_SUBSYS_REASONS ${_ss}_${_sub})
-            set(wont_build "${wont_build}\n       |_ ${_sub}: ${_reason}")
+            string(APPEND wont_build "\n       |_ ${_sub}: ${_reason}")
           endif()
         endforeach()
         if(NOT ("${wont_build}" STREQUAL ""))
-          set(message_text  "${message_text}\n       not building: ${wont_build}")
+          string(APPEND message_text "\n       not building: ${wont_build}")
         endif()
       endif()
       message(STATUS "${message_text}")
@@ -809,8 +852,7 @@ macro (PCL_ADD_DOC _subsys)
     endif()
     set(DOC_SOURCE_DIR "\"${CMAKE_CURRENT_SOURCE_DIR}\"\\")
     foreach(dep ${dependencies})
-      set(DOC_SOURCE_DIR
-          "${DOC_SOURCE_DIR}\n\t\t\t\t\t\t\t\t\t\t\t\t \"${PCL_SOURCE_DIR}/${dep}\"\\")
+      string(APPEND DOC_SOURCE_DIR "\n\t\t\t\t\t\t\t\t\t\t\t\t \"${PCL_SOURCE_DIR}/${dep}\"\\")
     endforeach()
     file(MAKE_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/html")
     set(doxyfile "${CMAKE_CURRENT_BINARY_DIR}/doxyfile")
diff --git a/common/include/pcl/ModelCoefficients.h b/common/include/pcl/ModelCoefficients.h
index 23fedbad..84f7336b 100644
--- a/common/include/pcl/ModelCoefficients.h
+++ b/common/include/pcl/ModelCoefficients.h
@@ -1,6 +1,5 @@
 #pragma once
 
-#include <string>
 #include <vector>
 #include <ostream>
 
diff --git a/common/include/pcl/PCLImage.h b/common/include/pcl/PCLImage.h
index 1e8886ca..475acb8c 100644
--- a/common/include/pcl/PCLImage.h
+++ b/common/include/pcl/PCLImage.h
@@ -5,6 +5,7 @@
 #include <ostream>  // for ostream
 
 #include <pcl/PCLHeader.h>   // for PCLHeader
+#include <pcl/types.h> //for index_t
 
 namespace pcl
 {
@@ -12,12 +13,12 @@ namespace pcl
   {
      ::pcl::PCLHeader  header;
 
-    std::uint32_t height = 0;
-    std::uint32_t width = 0;
+    uindex_t height = 0;
+    uindex_t width = 0;
     std::string encoding;
 
     std::uint8_t is_bigendian = 0;
-    std::uint32_t step = 0;
+    uindex_t step = 0;
 
     std::vector<std::uint8_t> data;
 
diff --git a/common/include/pcl/PCLPointCloud2.h b/common/include/pcl/PCLPointCloud2.h
index 63b32762..50520f66 100644
--- a/common/include/pcl/PCLPointCloud2.h
+++ b/common/include/pcl/PCLPointCloud2.h
@@ -8,6 +8,7 @@
 #include <pcl/pcl_macros.h>  // for PCL_EXPORTS
 #include <pcl/PCLHeader.h>
 #include <pcl/PCLPointField.h>
+#include <pcl/types.h>
 
 namespace pcl
 {
@@ -16,15 +17,15 @@ namespace pcl
   {
     ::pcl::PCLHeader header;
 
-    std::uint32_t height = 0;
-    std::uint32_t width = 0;
+    uindex_t height = 0;
+    uindex_t width = 0;
 
     std::vector<::pcl::PCLPointField>  fields;
 
     static_assert(BOOST_ENDIAN_BIG_BYTE || BOOST_ENDIAN_LITTLE_BYTE, "unable to determine system endianness");
     std::uint8_t is_bigendian = BOOST_ENDIAN_BIG_BYTE;
-    std::uint32_t point_step = 0;
-    std::uint32_t row_step = 0;
+    uindex_t point_step = 0;
+    uindex_t row_step = 0;
 
     std::vector<std::uint8_t> data;
 
diff --git a/common/include/pcl/PCLPointField.h b/common/include/pcl/PCLPointField.h
index 09f90ff8..73d82973 100644
--- a/common/include/pcl/PCLPointField.h
+++ b/common/include/pcl/PCLPointField.h
@@ -2,6 +2,7 @@
 
 #include <pcl/memory.h>       // for shared_ptr
 #include <pcl/type_traits.h>  // for asEnum_v
+#include <pcl/types.h>        // for index_t
 
 #include <string>   // for string
 #include <ostream>  // for ostream
@@ -12,9 +13,9 @@ namespace pcl
   {
     std::string name;
 
-    std::uint32_t offset = 0;
+    uindex_t offset = 0;
     std::uint8_t datatype = 0;
-    std::uint32_t count = 0;
+    uindex_t count = 0;
 
     enum PointFieldTypes { INT8 = traits::asEnum_v<std::int8_t>,
                            UINT8 = traits::asEnum_v<std::uint8_t>,
diff --git a/common/include/pcl/PointIndices.h b/common/include/pcl/PointIndices.h
index 2a648aa2..53e19dfb 100644
--- a/common/include/pcl/PointIndices.h
+++ b/common/include/pcl/PointIndices.h
@@ -1,7 +1,5 @@
 #pragma once
 
-#include <string>
-#include <vector>
 #include <ostream>
 
 // Include the correct Header path here
diff --git a/common/include/pcl/PolygonMesh.h b/common/include/pcl/PolygonMesh.h
index f62d7b34..55d69995 100644
--- a/common/include/pcl/PolygonMesh.h
+++ b/common/include/pcl/PolygonMesh.h
@@ -1,7 +1,6 @@
 #pragma once
 
 #include <algorithm>
-#include <string>
 #include <vector>
 #include <ostream>
 
@@ -31,6 +30,8 @@ namespace pcl
     static bool
     concatenate (pcl::PolygonMesh &mesh1, const pcl::PolygonMesh &mesh2)
     {
+      const auto point_offset = mesh1.cloud.width * mesh1.cloud.height;
+      
       bool success = pcl::PCLPointCloud2::concatenate(mesh1.cloud, mesh2.cloud);
       if (success == false) {
         return false;
@@ -38,7 +39,6 @@ namespace pcl
       // Make the resultant polygon mesh take the newest stamp
       mesh1.header.stamp = std::max(mesh1.header.stamp, mesh2.header.stamp);
 
-      const auto point_offset = mesh1.cloud.width * mesh1.cloud.height;
       std::transform(mesh2.polygons.begin (),
                      mesh2.polygons.end (),
                      std::back_inserter (mesh1.polygons),
diff --git a/common/include/pcl/Vertices.h b/common/include/pcl/Vertices.h
index 2b175692..fe5b5d84 100644
--- a/common/include/pcl/Vertices.h
+++ b/common/include/pcl/Vertices.h
@@ -2,9 +2,8 @@
 
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
+#include <pcl/types.h>
 
-#include <string>
-#include <vector>
 #include <ostream>
 
 namespace pcl
@@ -17,7 +16,7 @@ namespace pcl
     Vertices ()
     {}
 
-    std::vector<std::uint32_t> vertices;
+    Indices vertices;
 
   public:
     using Ptr = shared_ptr<Vertices>;
diff --git a/common/include/pcl/common/boost.h b/common/include/pcl/common/boost.h
index 09908e61..232d1df5 100644
--- a/common/include/pcl/common/boost.h
+++ b/common/include/pcl/common/boost.h
@@ -41,6 +41,7 @@
 #ifdef __GNUC__
 #pragma GCC system_header 
 #endif
+PCL_DEPRECATED_HEADER(1, 15, "Please include the needed boost headers directly.")
 
 #ifndef Q_MOC_RUN
 // Marking all Boost headers as system headers to remove warnings
diff --git a/common/include/pcl/common/colors.h b/common/include/pcl/common/colors.h
index 5490c06b..7c1fc883 100644
--- a/common/include/pcl/common/colors.h
+++ b/common/include/pcl/common/colors.h
@@ -40,6 +40,9 @@
 #include <pcl/pcl_macros.h>
 #include <pcl/point_types.h>
 
+#include <type_traits>  // for is_floating_point
+#include <array>        // for std::array especially in Clang Darwin and MSVC
+
 namespace pcl
 {
 
@@ -83,4 +86,85 @@ namespace pcl
   using GlasbeyLUT = ColorLUT<pcl::LUT_GLASBEY>;
   using ViridisLUT = ColorLUT<pcl::LUT_VIRIDIS>;
 
-}
+  /**
+   * @brief Returns a Look-Up Table useful in converting RGB to sRGB
+   * @tparam T floating point type with resultant value
+   * @tparam bits depth of RGB
+   * @return 1-D LUT for converting R, G or B into Rs, Gs or Bs
+   * @remarks sRGB was proposed by Stokes et al. as a uniform default color
+   * space for the internet
+   * M. Stokes, M. Anderson, S. Chandrasekar, and R. Motta: A standard default colorspace for the internet - sRGB (Nov 1996)
+   * IEC 61966-2.1 Default RGB Colour Space - sRGB (International Electrotechnical Commission, Geneva, Switzerland, 1999)
+   * www.srgb.com, www.color.org/srgb.html
+   */
+  template <typename T, std::uint8_t bits = 8>
+  PCL_EXPORTS inline std::array<T, 1 << bits>
+  RGB2sRGB_LUT() noexcept
+  {
+    static_assert(std::is_floating_point<T>::value, "LUT value must be a floating point");
+
+    constexpr const std::size_t size = 1 << bits;
+
+    static const auto sRGB_LUT = [&]() {
+      // MSVC wouldn't take `size` here instead of the expression
+      std::array<T, 1 << bits> LUT;
+      for (std::size_t i = 0; i < size; ++i) {
+        T f = static_cast<T>(i) / static_cast<T>(size - 1);
+        if (f > 0.04045) {
+          // ((f + 0.055)/1.055)^2.4
+          LUT[i] = static_cast<T>(
+              std::pow((f + static_cast<T>(0.055)) / static_cast<T>(1.055),
+                       static_cast<T>(2.4)));
+        }
+        else {
+          // f / 12.92
+          LUT[i] = f / static_cast<T>(12.92);
+        }
+      }
+      return LUT;
+    }();
+    return sRGB_LUT;
+  }
+
+  /**
+   * @brief Returns a Look-Up Table useful in converting scaled CIE XYZ into CIE L*a*b*
+   * @details The function assumes that the XYZ values are
+   *   * not normalized using reference illuminant
+   *   * scaled such that reference illuminant has Xn = Yn = Zn = discretizations
+   * @tparam T floating point type with resultant value
+   * @tparam discretizations number of levels for the LUT
+   * @return 1-D LUT with results of f(X/Xn)
+   * @note This function doesn't convert from CIE XYZ to CIE L*a*b*. The actual conversion
+   * is as follows:
+   * L* = 116 * [f(Y/Yn) - 16/116]
+   * a* = 500 * [f(X/Xn) - f(Y/Yn)]
+   * b* = 200 * [f(Y/Yn) - f(Z/Zn)]
+   * Where, Xn, Yn and Zn are values of the reference illuminant (at prescribed angle)
+   *        f is appropriate function such that L* = 100, a* = b* = 0 for white color
+   * Reference: Billmeyer and Saltzman’s Principles of Color Technology
+   */
+  template <typename T, std::size_t discretizations = 4000>
+  PCL_EXPORTS inline const std::array<T, discretizations>&
+  XYZ2LAB_LUT() noexcept
+  {
+    static_assert(std::is_floating_point<T>::value, "LUT value must be a floating point");
+
+    static const auto f_LUT = [&]() {
+      std::array<T, discretizations> LUT;
+      for (std::size_t i = 0; i < discretizations; ++i) {
+        T f = static_cast<T>(i) / static_cast<T>(discretizations);
+        if (f > static_cast<T>(0.008856)) {
+          // f^(1/3)
+          LUT[i] = static_cast<T>(std::pow(f, (static_cast<T>(1) / static_cast<T>(3))));
+        }
+        else {
+          // 7.87 * f + 16/116
+          LUT[i] =
+              static_cast<T>(7.87) * f + (static_cast<T>(16) / static_cast<T>(116));
+        }
+      }
+      return LUT;
+    }();
+    return f_LUT;
+  }
+}  // namespace pcl
diff --git a/common/include/pcl/common/common.h b/common/include/pcl/common/common.h
index c5e01b60..f21f0968 100644
--- a/common/include/pcl/common/common.h
+++ b/common/include/pcl/common/common.h
@@ -37,7 +37,16 @@
 
 #pragma once
 
-#include <pcl/pcl_base.h>
+#ifdef __SSE__
+#include <xmmintrin.h> // for __m128
+#endif // ifdef __SSE__
+#ifdef __AVX__
+#include <immintrin.h> // for __m256
+#endif // ifdef __AVX__
+
+#include <pcl/point_cloud.h> // for PointCloud
+#include <pcl/PointIndices.h> // for PointIndices
+namespace pcl { struct PCLPointCloud2; }
 
 /**
   * \file pcl/common/common.h
@@ -51,6 +60,7 @@ namespace pcl
   /** \brief Compute the smallest angle between two 3D vectors in radians (default) or degree.
     * \param v1 the first 3D vector (represented as a \a Eigen::Vector4f)
     * \param v2 the second 3D vector (represented as a \a Eigen::Vector4f)
+    * \param in_degree determine if angle should be in radians or degrees
     * \return the angle between v1 and v2 in radians or degrees
     * \note Handles rounding error for parallel and anti-parallel vectors
     * \ingroup common
@@ -68,6 +78,67 @@ namespace pcl
   inline double
   getAngle3D (const Eigen::Vector3f &v1, const Eigen::Vector3f &v2, const bool in_degree = false);
 
+#ifdef __SSE__
+  /** \brief Compute the approximate arccosine of four values at once using SSE instructions.
+    *
+    * The approximation used is \f$ (1.59121552+x*(-0.15461442+x*0.05354897))*\sqrt{0.89286965-0.89282669*x}+0.06681017+x*(-0.09402311+x*0.02708663) \f$.
+    * The average error is 0.00012 rad. This approximation is more accurate than other approximations of acos, but also uses a few more operations.
+    * \param x four floats, each should be in [0; 1]. They must not be greater than 1 since acos is undefined there.
+    *          They should not be less than 0 because there the approximation is less precise
+    * \return the four arccosines, each in [0; pi/2]
+    * \ingroup common
+    */
+  inline __m128
+  acos_SSE (const __m128 &x);
+
+  /** \brief Similar to getAngle3D, but four times in parallel using SSE instructions.
+    *
+    * This behaves like \f$ min(getAngle3D(dot_product), \pi-getAngle3D(dot_product)) \f$.
+    * All vectors must be normalized (length is 1.0).
+    * Since an approximate acos is used, the results may be slightly imprecise.
+    * \param[in] the x components of the first four vectors
+    * \param[in] the y components of the first four vectors
+    * \param[in] the z components of the first four vectors
+    * \param[in] the x components of the second four vectors
+    * \param[in] the y components of the second four vectors
+    * \param[in] the z components of the second four vectors
+    * \return the four angles in radians in [0; pi/2]
+    * \ingroup common
+    */
+  inline __m128
+  getAcuteAngle3DSSE (const __m128 &x1, const __m128 &y1, const __m128 &z1, const __m128 &x2, const __m128 &y2, const __m128 &z2);
+#endif // ifdef __SSE__
+
+#ifdef __AVX__
+  /** \brief Compute the approximate arccosine of eight values at once using AVX instructions.
+    *
+    * The approximation used is \f$ (1.59121552+x*(-0.15461442+x*0.05354897))*\sqrt{0.89286965-0.89282669*x}+0.06681017+x*(-0.09402311+x*0.02708663) \f$.
+    * The average error is 0.00012 rad. This approximation is more accurate than other approximations of acos, but also uses a few more operations.
+    * \param x eight floats, each should be in [0; 1]. They must not be greater than 1 since acos is undefined there.
+    *          They should not be less than 0 because there the approximation is less precise
+    * \return the eight arccosines, each in [0; pi/2]
+    * \ingroup common
+    */
+  inline __m256
+  acos_AVX (const __m256 &x);
+
+  /** \brief Similar to getAngle3D, but eight times in parallel using AVX instructions.
+    *
+    * This behaves like \f$ min(getAngle3D(dot_product), \pi-getAngle3D(dot_product)) \f$.
+    * All vectors must be normalized (length is 1.0).
+    * Since an approximate acos is used, the results may be slightly imprecise.
+    * \param[in] the x components of the first eight vectors
+    * \param[in] the y components of the first eight vectors
+    * \param[in] the z components of the first eight vectors
+    * \param[in] the x components of the second eight vectors
+    * \param[in] the y components of the second eight vectors
+    * \param[in] the z components of the second eight vectors
+    * \return the eight angles in radians in [0; pi/2]
+    * \ingroup common
+    */
+  inline __m256
+  getAcuteAngle3DAVX (const __m256 &x1, const __m256 &y1, const __m256 &z1, const __m256 &x2, const __m256 &y2, const __m256 &z2);
+#endif // ifdef __AVX__
 
   /** \brief Compute both the mean and the standard deviation of an array of values
     * \param values the array of values
@@ -110,18 +181,18 @@ namespace pcl
                   const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt);
 
   /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud
-    * \param cloud the point cloud data message
-    * \param min_pt the resultant minimum bounds
-    * \param max_pt the resultant maximum bounds
+    * \param[in] cloud the point cloud data message
+    * \param[out] min_pt the resultant minimum bounds
+    * \param[out] max_pt the resultant maximum bounds
     * \ingroup common
     */
   template <typename PointT> inline void 
   getMinMax3D (const pcl::PointCloud<PointT> &cloud, PointT &min_pt, PointT &max_pt);
   
   /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud
-    * \param cloud the point cloud data message
-    * \param min_pt the resultant minimum bounds
-    * \param max_pt the resultant maximum bounds
+    * \param[in] cloud the point cloud data message
+    * \param[out] min_pt the resultant minimum bounds
+    * \param[out] max_pt the resultant maximum bounds
     * \ingroup common
     */
   template <typename PointT> inline void 
@@ -129,10 +200,10 @@ namespace pcl
                Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt);
 
   /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud
-    * \param cloud the point cloud data message
-    * \param indices the vector of point indices to use from \a cloud
-    * \param min_pt the resultant minimum bounds
-    * \param max_pt the resultant maximum bounds
+    * \param[in] cloud the point cloud data message
+    * \param[in] indices the vector of point indices to use from \a cloud
+    * \param[out] min_pt the resultant minimum bounds
+    * \param[out] max_pt the resultant maximum bounds
     * \ingroup common
     */
   template <typename PointT> inline void 
@@ -140,10 +211,10 @@ namespace pcl
                Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt);
 
   /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud
-    * \param cloud the point cloud data message
-    * \param indices the vector of point indices to use from \a cloud
-    * \param min_pt the resultant minimum bounds
-    * \param max_pt the resultant maximum bounds
+    * \param[in] cloud the point cloud data message
+    * \param[in] indices the vector of point indices to use from \a cloud
+    * \param[out] min_pt the resultant minimum bounds
+    * \param[out] max_pt the resultant maximum bounds
     * \ingroup common
     */
   template <typename PointT> inline void 
@@ -199,6 +270,41 @@ namespace pcl
   PCL_EXPORTS void
   getMeanStdDev (const std::vector<float> &values, double &mean, double &stddev);
 
+  /** \brief Compute the median of a list of values (fast). If the number of values is even, take the mean of the two middle values.
+    * This function can be used like this:
+    * \code{.cpp}
+    * std::vector<double> vector{1.0, 25.0, 9.0, 4.0, 16.0};
+    * const double median = pcl::computeMedian (vector.begin (), vector.end (), static_cast<double(*)(double)>(std::sqrt)); // = 3
+    * \endcode
+    * \param[in,out] begin,end Iterators that mark the beginning and end of the value range. These values will be reordered!
+    * \param[in] f A lamda, function pointer, or similar that is implicitly applied to all values before median computation. In reality, it will be applied lazily (i.e. at most twice) and thus may not change the sorting order (e.g. monotonic functions like sqrt are allowed)
+    * \return the median
+    * \ingroup common
+    */
+  template<typename IteratorT, typename Functor> inline auto
+  computeMedian (IteratorT begin, IteratorT end, Functor f) noexcept -> typename std::result_of<Functor(decltype(*begin))>::type
+  {
+    const std::size_t size = std::distance(begin, end);
+    const std::size_t mid = size/2;
+    if (size%2==0)
+    { // Even number of values
+      std::nth_element (begin, begin + (mid-1), end);
+      return (f(begin[mid-1]) + f(*(std::min_element (begin + mid, end)))) / 2.0;
+    }
+    else
+    { // Odd number of values
+      std::nth_element (begin, begin + mid, end);
+      return f(begin[mid]);
+    }
+  }
+
+  /** \brief Compute the median of a list of values (fast). See the other overloaded function for more information.
+    */
+  template<typename IteratorT> inline auto
+  computeMedian (IteratorT begin, IteratorT end) noexcept -> typename std::iterator_traits<IteratorT>::value_type
+  {
+    return computeMedian (begin, end, [](const auto& x){return x;});
+  }
 }
 /*@}*/
 #include <pcl/common/impl/common.hpp>
diff --git a/common/include/pcl/common/concatenate.h b/common/include/pcl/common/concatenate.h
index 5abc2326..f7510812 100644
--- a/common/include/pcl/common/concatenate.h
+++ b/common/include/pcl/common/concatenate.h
@@ -38,8 +38,6 @@
 
 #pragma once
 
-#include <pcl/conversions.h>
-
 #include <type_traits>
 
 namespace pcl
diff --git a/common/include/pcl/common/eigen.h b/common/include/pcl/common/eigen.h
index f5219b39..70339fee 100644
--- a/common/include/pcl/common/eigen.h
+++ b/common/include/pcl/common/eigen.h
@@ -52,17 +52,11 @@
 #  pragma disable_warn
 #endif
 
-#include <cmath>
 #include <pcl/ModelCoefficients.h>
 
 #include <Eigen/StdVector>
-#include <Eigen/Core>
-#include <Eigen/Eigenvalues>
 #include <Eigen/Geometry>
-#include <Eigen/SVD>
 #include <Eigen/LU>
-#include <Eigen/Dense>
-#include <Eigen/Eigenvalues>
 
 namespace pcl
 {
@@ -265,18 +259,6 @@ namespace pcl
   template <typename Scalar> void
   getEulerAngles (const Eigen::Transform<Scalar, 3, Eigen::Affine> &t, Scalar &roll, Scalar &pitch, Scalar &yaw);
 
-  inline void
-  getEulerAngles (const Eigen::Affine3f &t, float &roll, float &pitch, float &yaw)
-  {
-    getEulerAngles<float> (t, roll, pitch, yaw);
-  }
-
-  inline void
-  getEulerAngles (const Eigen::Affine3d &t, double &roll, double &pitch, double &yaw)
-  {
-    getEulerAngles<double> (t, roll, pitch, yaw);
-  }
-
   /** Extract x,y,z and the Euler angles (XYZ-convention) from the given transformation
     * \param[in] t the input transformation matrix
     * \param[out] x the resulting x translation
@@ -292,22 +274,6 @@ namespace pcl
                                 Scalar &x, Scalar &y, Scalar &z,
                                 Scalar &roll, Scalar &pitch, Scalar &yaw);
 
-  inline void
-  getTranslationAndEulerAngles (const Eigen::Affine3f &t,
-                                float &x, float &y, float &z,
-                                float &roll, float &pitch, float &yaw)
-  {
-    getTranslationAndEulerAngles<float> (t, x, y, z, roll, pitch, yaw);
-  }
-
-  inline void
-  getTranslationAndEulerAngles (const Eigen::Affine3d &t,
-                                double &x, double &y, double &z,
-                                double &roll, double &pitch, double &yaw)
-  {
-    getTranslationAndEulerAngles<double> (t, x, y, z, roll, pitch, yaw);
-  }
-
   /** \brief Create a transformation from the given translation and Euler angles (XYZ-convention)
     * \param[in] x the input x translation
     * \param[in] y the input y translation
@@ -429,22 +395,6 @@ namespace pcl
     point_out = (transformation * point).template head<3> ();
   }
 
-  inline void
-  transformPoint (const Eigen::Vector3f &point_in,
-                        Eigen::Vector3f &point_out,
-                  const Eigen::Affine3f &transformation)
-  {
-    transformPoint<float> (point_in, point_out, transformation);
-  }
-
-  inline void
-  transformPoint (const Eigen::Vector3d &point_in,
-                        Eigen::Vector3d &point_out,
-                  const Eigen::Affine3d &transformation)
-  {
-    transformPoint<double> (point_in, point_out, transformation);
-  }
-
 /** \brief Transform a vector using an affine matrix
   * \param[in] vector_in the vector to be transformed
   * \param[out] vector_out the transformed vector
@@ -460,22 +410,6 @@ namespace pcl
     vector_out = transformation.linear () * vector_in;
   }
 
-  inline void
-  transformVector (const Eigen::Vector3f &vector_in,
-                         Eigen::Vector3f &vector_out,
-                   const Eigen::Affine3f &transformation)
-  {
-    transformVector<float> (vector_in, vector_out, transformation);
-  }
-
-  inline void
-  transformVector (const Eigen::Vector3d &vector_in,
-                         Eigen::Vector3d &vector_out,
-                   const Eigen::Affine3d &transformation)
-  {
-    transformVector<double> (vector_in, vector_out, transformation);
-  }
-
 /** \brief Transform a line using an affine matrix
   * \param[in] line_in the line to be transformed
   * \param[out] line_out the transformed line
@@ -492,22 +426,6 @@ namespace pcl
                        Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_out,
                  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);
 
-  inline bool
-  transformLine (const Eigen::VectorXf &line_in,
-                       Eigen::VectorXf &line_out,
-                 const Eigen::Affine3f &transformation)
-  {
-    return (transformLine<float> (line_in, line_out, transformation));
-  }
-
-  inline bool
-  transformLine (const Eigen::VectorXd &line_in,
-                       Eigen::VectorXd &line_out,
-                 const Eigen::Affine3d &transformation)
-  {
-    return (transformLine<double> (line_in, line_out, transformation));
-  }
-
 /** \brief Transform plane vectors using an affine matrix
   * \param[in] plane_in the plane coefficients to be transformed
   * \param[out] plane_out the transformed plane coefficients to fill
@@ -522,22 +440,6 @@ namespace pcl
                         Eigen::Matrix<Scalar, 4, 1> &plane_out,
                   const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);
 
-  inline void
-  transformPlane (const Eigen::Matrix<double, 4, 1> &plane_in,
-                        Eigen::Matrix<double, 4, 1> &plane_out,
-                  const Eigen::Transform<double, 3, Eigen::Affine> &transformation)
-  {
-    transformPlane<double> (plane_in, plane_out, transformation);
-  }
-
-  inline void
-  transformPlane (const Eigen::Matrix<float, 4, 1> &plane_in,
-                        Eigen::Matrix<float, 4, 1> &plane_out,
-                  const Eigen::Transform<float, 3, Eigen::Affine> &transformation)
-  {
-    transformPlane<float> (plane_in, plane_out, transformation);
-  }
-
 /** \brief Transform plane vectors using an affine matrix
   * \param[in] plane_in the plane coefficients to be transformed
   * \param[out] plane_out the transformed plane coefficients to fill
@@ -549,26 +451,10 @@ namespace pcl
   * \warning ModelCoefficients stores floats only !
   */
   template<typename Scalar> void
-  transformPlane (const pcl::ModelCoefficients::Ptr plane_in,
+  transformPlane (const pcl::ModelCoefficients::ConstPtr plane_in,
                         pcl::ModelCoefficients::Ptr plane_out,
                   const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);
 
-  inline void
-  transformPlane (const pcl::ModelCoefficients::Ptr plane_in,
-                        pcl::ModelCoefficients::Ptr plane_out,
-                  const Eigen::Transform<double, 3, Eigen::Affine> &transformation)
-  {
-    transformPlane<double> (plane_in, plane_out, transformation);
-  }
-
-  inline void
-  transformPlane (const pcl::ModelCoefficients::Ptr plane_in,
-                        pcl::ModelCoefficients::Ptr plane_out,
-                  const Eigen::Transform<float, 3, Eigen::Affine> &transformation)
-  {
-    transformPlane<float> (plane_in, plane_out, transformation);
-  }
-
 /** \brief Check coordinate system integrity
   * \param[in] line_x the first axis
   * \param[in] line_y the second axis
@@ -598,24 +484,6 @@ namespace pcl
                          const Scalar norm_limit = 1e-3,
                          const Scalar dot_limit = 1e-3);
 
-  inline bool
-  checkCoordinateSystem (const Eigen::Matrix<double, Eigen::Dynamic, 1> &line_x,
-                         const Eigen::Matrix<double, Eigen::Dynamic, 1> &line_y,
-                         const double norm_limit = 1e-3,
-                         const double dot_limit = 1e-3)
-  {
-    return (checkCoordinateSystem<double> (line_x, line_y, norm_limit, dot_limit));
-  }
-
-  inline bool
-  checkCoordinateSystem (const Eigen::Matrix<float, Eigen::Dynamic, 1> &line_x,
-                         const Eigen::Matrix<float, Eigen::Dynamic, 1> &line_y,
-                         const float norm_limit = 1e-3,
-                         const float dot_limit = 1e-3)
-  {
-    return (checkCoordinateSystem<float> (line_x, line_y, norm_limit, dot_limit));
-  }
-
 /** \brief Check coordinate system integrity
   * \param[in] origin the origin of the coordinate system
   * \param[in] x_direction the first axis
@@ -691,26 +559,6 @@ namespace pcl
                                       const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_y,
                                       Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);
 
-  inline bool
-  transformBetween2CoordinateSystems (const Eigen::Matrix<double, Eigen::Dynamic, 1> from_line_x,
-                                      const Eigen::Matrix<double, Eigen::Dynamic, 1> from_line_y,
-                                      const Eigen::Matrix<double, Eigen::Dynamic, 1> to_line_x,
-                                      const Eigen::Matrix<double, Eigen::Dynamic, 1> to_line_y,
-                                      Eigen::Transform<double, 3, Eigen::Affine> &transformation)
-  {
-    return (transformBetween2CoordinateSystems<double> (from_line_x, from_line_y, to_line_x, to_line_y, transformation));
-  }
-
-  inline bool
-  transformBetween2CoordinateSystems (const Eigen::Matrix<float, Eigen::Dynamic, 1> from_line_x,
-                                      const Eigen::Matrix<float, Eigen::Dynamic, 1> from_line_y,
-                                      const Eigen::Matrix<float, Eigen::Dynamic, 1> to_line_x,
-                                      const Eigen::Matrix<float, Eigen::Dynamic, 1> to_line_y,
-                                      Eigen::Transform<float, 3, Eigen::Affine> &transformation)
-  {
-    return (transformBetween2CoordinateSystems<float> (from_line_x, from_line_y, to_line_x, to_line_y, transformation));
-  }
-
 }
 
 #include <pcl/common/impl/eigen.hpp>
diff --git a/common/include/pcl/common/gaussian.h b/common/include/pcl/common/gaussian.h
index 539a46ba..9c7e0130 100644
--- a/common/include/pcl/common/gaussian.h
+++ b/common/include/pcl/common/gaussian.h
@@ -39,9 +39,10 @@
 
 #pragma once
 
-#include <pcl/common/eigen.h>
 #include <pcl/point_cloud.h>
 
+#include <Eigen/Core> // for VectorXf
+
 #include <functional>
 
 namespace pcl
diff --git a/common/include/pcl/common/geometry.h b/common/include/pcl/common/geometry.h
index c7b613e1..37b0eefc 100644
--- a/common/include/pcl/common/geometry.h
+++ b/common/include/pcl/common/geometry.h
@@ -149,7 +149,7 @@ namespace pcl
       }
       else
       {
-        PCL_WARN ("[pcl::randomOrthogonalAxis] provided axis has norm < 1E-8f");
+        PCL_WARN ("[pcl::randomOrthogonalAxis] provided axis has norm < 1E-8f\n");
       }
 
       rand_ortho_axis.normalize ();
diff --git a/common/include/pcl/common/impl/centroid.hpp b/common/include/pcl/common/impl/centroid.hpp
index d82a8bd5..7cf88a67 100644
--- a/common/include/pcl/common/impl/centroid.hpp
+++ b/common/include/pcl/common/impl/centroid.hpp
@@ -138,7 +138,7 @@ compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
   // If the data is dense, we don't need to check for NaN
   if (cloud.is_dense)
   {
-    for (const int& index : indices)
+    for (const auto& index : indices)
     {
       centroid[0] += cloud[index].x;
       centroid[1] += cloud[index].y;
@@ -150,7 +150,7 @@ compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
   }
   // NaN or Inf values could exist => check for them
     unsigned cp = 0;
-  for (const int& index : indices)
+  for (const auto& index : indices)
   {
     // Check if the point is invalid
     if (!isFinite (cloud [index]))
@@ -300,7 +300,7 @@ computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
   {
     point_count = 0;
     // For each point in the cloud
-    for (const int &index : indices)
+    for (const auto &index : indices)
     {
       // Check if the point is invalid
       if (!isFinite (cloud[index]))
@@ -430,7 +430,7 @@ computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
   if (cloud.is_dense)
   {
     point_count = static_cast<unsigned int> (indices.size ());
-    for (const int &index : indices)
+    for (const auto &index : indices)
     {
       //const PointT& point = cloud[*iIt];
       accu [0] += cloud[index].x * cloud[index].x;
@@ -444,7 +444,7 @@ computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
   else
   {
     point_count = 0;
-    for (const int &index : indices)
+    for (const auto &index : indices)
     {
       if (!isFinite (cloud[index]))
         continue;
@@ -558,7 +558,7 @@ computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
   if (cloud.is_dense)
   {
     point_count = indices.size ();
-    for (const int &index : indices)
+    for (const auto &index : indices)
     {
       //const PointT& point = cloud[*iIt];
       accu [0] += cloud[index].x * cloud[index].x;
@@ -575,7 +575,7 @@ computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
   else
   {
     point_count = 0;
-    for (const int &index : indices)
+    for (const auto &index : indices)
     {
       if (!isFinite (cloud[index]))
         continue;
@@ -904,10 +904,10 @@ computeCentroid (const pcl::PointCloud<PointInT>& cloud,
   pcl::CentroidPoint<PointInT> cp;
 
   if (cloud.is_dense)
-    for (const int &index : indices)
+    for (const auto &index : indices)
       cp.add (cloud[index]);
   else
-    for (const int &index : indices)
+    for (const auto &index : indices)
       if (pcl::isFinite (cloud[index]))
         cp.add (cloud[index]);
 
diff --git a/common/include/pcl/common/impl/common.hpp b/common/include/pcl/common/impl/common.hpp
index 79a6616d..78df9b7a 100644
--- a/common/include/pcl/common/impl/common.hpp
+++ b/common/include/pcl/common/impl/common.hpp
@@ -67,6 +67,58 @@ pcl::getAngle3D (const Eigen::Vector3f &v1, const Eigen::Vector3f &v2, const boo
   return (in_degree ? std::acos (rad) * 180.0 / M_PI : std::acos (rad));
 }
 
+#ifdef __SSE__
+inline __m128
+pcl::acos_SSE (const __m128 &x)
+{
+  /*
+  This python code generates the coefficients:
+  import math, numpy, scipy.optimize
+  def get_error(S):
+      err_sum=0.0
+      for x in numpy.arange(0.0, 1.0, 0.0025):
+          if (S[3]+S[4]*x)<0.0:
+              err_sum+=10.0
+          else:
+              err_sum+=((S[0]+x*(S[1]+x*S[2]))*numpy.sqrt(S[3]+S[4]*x)+S[5]+x*(S[6]+x*S[7])-math.acos(x))**2.0
+      return err_sum/400.0
+
+  print(scipy.optimize.minimize(fun=get_error, x0=[1.57, 0.0, 0.0, 1.0, -1.0, 0.0, 0.0, 0.0], method='Nelder-Mead', options={'maxiter':42000, 'maxfev':42000, 'disp':True, 'xatol':1e-6, 'fatol':1e-6}))
+  */
+  const __m128 mul_term = _mm_add_ps (_mm_set1_ps (1.59121552f), _mm_mul_ps (x, _mm_add_ps (_mm_set1_ps (-0.15461442f), _mm_mul_ps (x, _mm_set1_ps (0.05354897f)))));
+  const __m128 add_term = _mm_add_ps (_mm_set1_ps (0.06681017f), _mm_mul_ps (x, _mm_add_ps (_mm_set1_ps (-0.09402311f), _mm_mul_ps (x, _mm_set1_ps (0.02708663f)))));
+  return _mm_add_ps (_mm_mul_ps (mul_term, _mm_sqrt_ps (_mm_add_ps (_mm_set1_ps (0.89286965f), _mm_mul_ps (_mm_set1_ps (-0.89282669f), x)))), add_term);
+}
+
+inline __m128
+pcl::getAcuteAngle3DSSE (const __m128 &x1, const __m128 &y1, const __m128 &z1, const __m128 &x2, const __m128 &y2, const __m128 &z2)
+{
+  const __m128 dot_product = _mm_add_ps (_mm_add_ps (_mm_mul_ps (x1, x2), _mm_mul_ps (y1, y2)), _mm_mul_ps (z1, z2));
+  // The andnot-function realizes an abs-operation: the sign bit is removed
+  // -0.0f (negative zero) means that all bits are 0, only the sign bit is 1
+  return acos_SSE (_mm_min_ps (_mm_set1_ps (1.0f), _mm_andnot_ps (_mm_set1_ps (-0.0f), dot_product)));
+}
+#endif // ifdef __SSE__
+
+#ifdef __AVX__
+inline __m256
+pcl::acos_AVX (const __m256 &x)
+{
+  const __m256 mul_term = _mm256_add_ps (_mm256_set1_ps (1.59121552f), _mm256_mul_ps (x, _mm256_add_ps (_mm256_set1_ps (-0.15461442f), _mm256_mul_ps (x, _mm256_set1_ps (0.05354897f)))));
+  const __m256 add_term = _mm256_add_ps (_mm256_set1_ps (0.06681017f), _mm256_mul_ps (x, _mm256_add_ps (_mm256_set1_ps (-0.09402311f), _mm256_mul_ps (x, _mm256_set1_ps (0.02708663f)))));
+  return _mm256_add_ps (_mm256_mul_ps (mul_term, _mm256_sqrt_ps (_mm256_add_ps (_mm256_set1_ps (0.89286965f), _mm256_mul_ps (_mm256_set1_ps (-0.89282669f), x)))), add_term);
+}
+
+inline __m256
+pcl::getAcuteAngle3DAVX (const __m256 &x1, const __m256 &y1, const __m256 &z1, const __m256 &x2, const __m256 &y2, const __m256 &z2)
+{
+  const __m256 dot_product = _mm256_add_ps (_mm256_add_ps (_mm256_mul_ps (x1, x2), _mm256_mul_ps (y1, y2)), _mm256_mul_ps (z1, z2));
+  // The andnot-function realizes an abs-operation: the sign bit is removed
+  // -0.0f (negative zero) means that all bits are 0, only the sign bit is 1
+  return acos_AVX (_mm256_min_ps (_mm256_set1_ps (1.0f), _mm256_andnot_ps (_mm256_set1_ps (-0.0f), dot_product)));
+}
+#endif // ifdef __AVX__
+
 //////////////////////////////////////////////////////////////////////////////////////////////
 inline void
 pcl::getMeanStd (const std::vector<float> &values, double &mean, double &stddev)
@@ -242,35 +294,8 @@ pcl::getMaxDistance (const pcl::PointCloud<PointT> &cloud, const Indices &indice
 template <typename PointT> inline void
 pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, PointT &min_pt, PointT &max_pt)
 {
-  Eigen::Array4f min_p, max_p;
-  min_p.setConstant (FLT_MAX);
-  max_p.setConstant (-FLT_MAX);
-
-  // If the data is dense, we don't need to check for NaN
-  if (cloud.is_dense)
-  {
-    for (const auto& point: cloud.points)
-    {
-      const auto pt = point.getArray4fMap ();
-      min_p = min_p.min (pt);
-      max_p = max_p.max (pt);
-    }
-  }
-  // NaN or Inf values could exist => check for them
-  else
-  {
-    for (const auto& point: cloud.points)
-    {
-      // Check if the point is invalid
-      if (!std::isfinite (point.x) ||
-          !std::isfinite (point.y) ||
-          !std::isfinite (point.z))
-        continue;
-      const auto pt = point.getArray4fMap ();
-      min_p = min_p.min (pt);
-      max_p = max_p.max (pt);
-    }
-  }
+  Eigen::Vector4f min_p, max_p;
+  pcl::getMinMax3D (cloud, min_p, max_p);
   min_pt.x = min_p[0]; min_pt.y = min_p[1]; min_pt.z = min_p[2];
   max_pt.x = max_p[0]; max_pt.y = max_p[1]; max_pt.z = max_p[2];
 }
@@ -279,18 +304,17 @@ pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, PointT &min_pt, PointT &
 template <typename PointT> inline void
 pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
 {
-  Eigen::Array4f min_p, max_p;
-  min_p.setConstant (FLT_MAX);
-  max_p.setConstant (-FLT_MAX);
+  min_pt.setConstant (FLT_MAX);
+  max_pt.setConstant (-FLT_MAX);
 
   // If the data is dense, we don't need to check for NaN
   if (cloud.is_dense)
   {
     for (const auto& point: cloud.points)
     {
-      const auto pt = point.getArray4fMap ();
-      min_p = min_p.min (pt);
-      max_p = max_p.max (pt);
+      const pcl::Vector4fMapConst pt = point.getVector4fMap ();
+      min_pt = min_pt.cwiseMin (pt);
+      max_pt = max_pt.cwiseMax (pt);
     }
   }
   // NaN or Inf values could exist => check for them
@@ -303,13 +327,11 @@ pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, Eigen::Vector4f &min_pt,
           !std::isfinite (point.y) ||
           !std::isfinite (point.z))
         continue;
-      const auto pt = point.getArray4fMap ();
-      min_p = min_p.min (pt);
-      max_p = max_p.max (pt);
+      const pcl::Vector4fMapConst pt = point.getVector4fMap ();
+      min_pt = min_pt.cwiseMin (pt);
+      max_pt = max_pt.cwiseMax (pt);
     }
   }
-  min_pt = min_p;
-  max_pt = max_p;
 }
 
 
@@ -318,37 +340,7 @@ template <typename PointT> inline void
 pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, const pcl::PointIndices &indices,
                   Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
 {
-  Eigen::Array4f min_p, max_p;
-  min_p.setConstant (FLT_MAX);
-  max_p.setConstant (-FLT_MAX);
-
-  // If the data is dense, we don't need to check for NaN
-  if (cloud.is_dense)
-  {
-    for (const int &index : indices.indices)
-    {
-      pcl::Array4fMapConst pt = cloud[index].getArray4fMap ();
-      min_p = min_p.min (pt);
-      max_p = max_p.max (pt);
-    }
-  }
-  // NaN or Inf values could exist => check for them
-  else
-  {
-    for (const int &index : indices.indices)
-    {
-      // Check if the point is invalid
-      if (!std::isfinite (cloud[index].x) || 
-          !std::isfinite (cloud[index].y) || 
-          !std::isfinite (cloud[index].z))
-        continue;
-      pcl::Array4fMapConst pt = cloud[index].getArray4fMap ();
-      min_p = min_p.min (pt);
-      max_p = max_p.max (pt);
-    }
-  }
-  min_pt = min_p;
-  max_pt = max_p;
+  pcl::getMinMax3D (cloud, indices.indices, min_pt, max_pt);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
@@ -362,26 +354,26 @@ pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, const Indices &indices,
   // If the data is dense, we don't need to check for NaN
   if (cloud.is_dense)
   {
-    for (const int &index : indices)
+    for (const auto &index : indices)
     {
-      pcl::Array4fMapConst pt = cloud[index].getArray4fMap ();
-      min_pt = min_pt.array ().min (pt);
-      max_pt = max_pt.array ().max (pt);
+      const pcl::Vector4fMapConst pt = cloud[index].getVector4fMap ();
+      min_pt = min_pt.cwiseMin (pt);
+      max_pt = max_pt.cwiseMax (pt);
     }
   }
   // NaN or Inf values could exist => check for them
   else
   {
-    for (const int &index : indices)
+    for (const auto &index : indices)
     {
       // Check if the point is invalid
       if (!std::isfinite (cloud[index].x) || 
           !std::isfinite (cloud[index].y) || 
           !std::isfinite (cloud[index].z))
         continue;
-      pcl::Array4fMapConst pt = cloud[index].getArray4fMap ();
-      min_pt = min_pt.array ().min (pt);
-      max_pt = max_pt.array ().max (pt);
+      const pcl::Vector4fMapConst pt = cloud[index].getVector4fMap ();
+      min_pt = min_pt.cwiseMin (pt);
+      max_pt = max_pt.cwiseMax (pt);
     }
   }
 }
diff --git a/common/include/pcl/common/impl/eigen.hpp b/common/include/pcl/common/impl/eigen.hpp
index 9388ce1f..fbc9eb8a 100644
--- a/common/include/pcl/common/impl/eigen.hpp
+++ b/common/include/pcl/common/impl/eigen.hpp
@@ -772,7 +772,7 @@ transformPlane (const Eigen::Matrix<Scalar, 4, 1> &plane_in,
 
 
 template <typename Scalar> void
-transformPlane (const pcl::ModelCoefficients::Ptr plane_in,
+transformPlane (const pcl::ModelCoefficients::ConstPtr plane_in,
                       pcl::ModelCoefficients::Ptr plane_out,
                 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
 {
@@ -780,7 +780,7 @@ transformPlane (const pcl::ModelCoefficients::Ptr plane_in,
   Eigen::Matrix < Scalar, 4, 1 > v_plane_in (values.data ());
   pcl::transformPlane (v_plane_in, v_plane_in, transformation);
   plane_out->values.resize (4);
-  std::copy_n(v_plane_in.data (), 4, plane_in->values.begin ());
+  std::copy_n(v_plane_in.data (), 4, plane_out->values.begin ());
 }
 
 
diff --git a/common/include/pcl/common/impl/gaussian.hpp b/common/include/pcl/common/impl/gaussian.hpp
index 403231dc..3b9c20b2 100644
--- a/common/include/pcl/common/impl/gaussian.hpp
+++ b/common/include/pcl/common/impl/gaussian.hpp
@@ -40,7 +40,6 @@
 #pragma once
 
 #include <pcl/common/gaussian.h>
-#include <pcl/exceptions.h>
 
 namespace pcl
 {
@@ -58,7 +57,7 @@ GaussianKernel::convolveRows(const pcl::PointCloud<PointT> &input,
   {
     output.width = input.width;
     output.height = input.height;
-    output.points.resize (input.height * input.width);
+    output.resize (input.height * input.width);
   }
 
   int i;
@@ -91,7 +90,7 @@ GaussianKernel::convolveCols(const pcl::PointCloud<PointT> &input,
   {
     output.width = input.width;
     output.height = input.height;
-    output.points.resize (input.height * input.width);
+    output.resize (input.height * input.width);
   }
 
   int j;
diff --git a/common/include/pcl/common/impl/intensity.hpp b/common/include/pcl/common/impl/intensity.hpp
index 526bcd94..687d934c 100644
--- a/common/include/pcl/common/impl/intensity.hpp
+++ b/common/include/pcl/common/impl/intensity.hpp
@@ -276,6 +276,40 @@ namespace pcl
       }
     };
 
+    template<>
+    struct IntensityFieldAccessor<pcl::PointXYZLAB>
+    {
+      inline float
+      operator () (const pcl::PointXYZLAB &p) const
+      {
+        return (p.L);
+      }
+
+      inline void
+      get (const pcl::PointXYZLAB &p, float &intensity) const
+      {
+        intensity = p.L;
+      }
+
+      inline void
+      set (pcl::PointXYZLAB &p, float intensity) const
+      {
+        p.L = intensity;
+      }
+
+      inline void
+      demean (pcl::PointXYZLAB& p, float value) const
+      {
+        p.L -= value;
+      }
+
+      inline void
+      add (pcl::PointXYZLAB& p, float value) const
+      {
+        p.L += value;
+      }
+    };
+
     template<>
     struct IntensityFieldAccessor<pcl::PointXYZHSV>
     {
diff --git a/common/include/pcl/common/impl/io.hpp b/common/include/pcl/common/impl/io.hpp
index 293f61be..d6d93aaf 100644
--- a/common/include/pcl/common/impl/io.hpp
+++ b/common/include/pcl/common/impl/io.hpp
@@ -40,6 +40,7 @@
 
 #pragma once
 
+#include <pcl/conversions.h> // for FieldAdder
 #include <pcl/common/concatenate.h>
 #include <pcl/common/copy_point.h>
 #include <pcl/common/io.h>
@@ -116,6 +117,28 @@ getFieldsList (const pcl::PointCloud<PointT> &)
   return (result);
 }
 
+namespace detail
+{
+
+  template <typename PointInT, typename PointOutT> void
+  copyPointCloudMemcpy (const pcl::PointCloud<PointInT> &cloud_in,
+                        pcl::PointCloud<PointOutT> &cloud_out)
+  {
+    // Iterate over each point, if the point types of two clouds are different
+    for (std::size_t i = 0; i < cloud_in.size (); ++i)
+      copyPoint (cloud_in[i], cloud_out[i]);
+  }
+
+
+  template <typename PointT> void
+  copyPointCloudMemcpy (const pcl::PointCloud<PointT> &cloud_in,
+                        pcl::PointCloud<PointT> &cloud_out)
+  {
+    // Use std::copy directly, if the point types of two clouds are same
+    std::copy (&cloud_in[0], (&cloud_in[0]) + cloud_in.size (), &cloud_out[0]);
+  }
+
+} // namespace detail
 
 template <typename PointInT, typename PointOutT> void
 copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
@@ -128,18 +151,10 @@ copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
   cloud_out.is_dense = cloud_in.is_dense;
   cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
   cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
-  cloud_out.points.resize (cloud_in.size ());
-
-  if (cloud_in.points.empty ())
-    return;
+  cloud_out.resize (cloud_in.size ());
 
-  if (isSamePointType<PointInT, PointOutT> ())
-    // Copy the whole memory block
-    memcpy (&cloud_out[0], &cloud_in[0], cloud_in.size () * sizeof (PointInT));
-  else
-    // Iterate over each point
-    for (std::size_t i = 0; i < cloud_in.size (); ++i)
-      copyPoint (cloud_in[i], cloud_out[i]);
+  if (!cloud_in.empty ())
+    detail::copyPointCloudMemcpy (cloud_in, cloud_out);
 }
 
 
@@ -156,7 +171,8 @@ copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
   }
 
   // Allocate enough space and copy the basics
-  cloud_out.points.resize (indices.size ());
+  cloud_out.clear ();
+  cloud_out.reserve (indices.size ());
   cloud_out.header   = cloud_in.header;
   cloud_out.width    = indices.size ();
   cloud_out.height   = 1;
@@ -165,8 +181,8 @@ copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
   cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
 
   // Iterate over each point
-  for (std::size_t i = 0; i < indices.size (); ++i)
-    cloud_out[i] = cloud_in[indices[i]];
+  for (const auto& index : indices)
+    cloud_out.transient_push_back (cloud_in[index]);
 }
 
 
@@ -176,7 +192,7 @@ copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
                 pcl::PointCloud<PointOutT> &cloud_out)
 {
   // Allocate enough space and copy the basics
-  cloud_out.points.resize (indices.size ());
+  cloud_out.resize (indices.size ());
   cloud_out.header   = cloud_in.header;
   cloud_out.width    = indices.size ();
   cloud_out.height   = 1;
@@ -195,25 +211,7 @@ copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
                 const pcl::PointIndices &indices,
                      pcl::PointCloud<PointT> &cloud_out)
 {
-  // Do we want to copy everything?
-  if (indices.indices.size () == cloud_in.size ())
-  {
-    cloud_out = cloud_in;
-    return;
-  }
-
-  // Allocate enough space and copy the basics
-  cloud_out.points.resize (indices.indices.size ());
-  cloud_out.header   = cloud_in.header;
-  cloud_out.width    = indices.indices.size ();
-  cloud_out.height   = 1;
-  cloud_out.is_dense = cloud_in.is_dense;
-  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
-  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
-
-  // Iterate over each point
-  for (std::size_t i = 0; i < indices.indices.size (); ++i)
-    cloud_out[i] = cloud_in[indices.indices[i]];
+  copyPointCloud (cloud_in, indices.indices, cloud_out);
 }
 
 
@@ -231,7 +229,7 @@ copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
                 const std::vector<pcl::PointIndices> &indices,
                 pcl::PointCloud<PointT> &cloud_out)
 {
-  int nr_p = 0;
+  std::size_t nr_p = 0;
   for (const auto &index : indices)
     nr_p += index.indices.size ();
 
@@ -243,7 +241,8 @@ copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
   }
 
   // Allocate enough space and copy the basics
-  cloud_out.points.resize (nr_p);
+  cloud_out.clear ();
+  cloud_out.reserve (nr_p);
   cloud_out.header   = cloud_in.header;
   cloud_out.width    = nr_p;
   cloud_out.height   = 1;
@@ -252,15 +251,13 @@ copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
   cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
 
   // Iterate over each cluster
-  int cp = 0;
   for (const auto &cluster_index : indices)
   {
     // Iterate over each idx
     for (const auto &index : cluster_index.indices)
     {
       // Iterate over each dimension
-      cloud_out[cp] = cloud_in[index];
-      cp++;
+      cloud_out.transient_push_back (cloud_in[index]);
     }
   }
 }
@@ -282,7 +279,7 @@ copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
   }
 
   // Allocate enough space and copy the basics
-  cloud_out.points.resize (nr_p);
+  cloud_out.resize (nr_p);
   cloud_out.header   = cloud_in.header;
   cloud_out.width    = nr_p;
   cloud_out.height   = 1;
@@ -319,7 +316,7 @@ concatenateFields (const pcl::PointCloud<PointIn1T> &cloud1_in,
   }
 
   // Resize the output dataset
-  cloud_out.points.resize (cloud1_in.size ());
+  cloud_out.resize (cloud1_in.size ());
   cloud_out.header   = cloud1_in.header;
   cloud_out.width    = cloud1_in.width;
   cloud_out.height   = cloud1_in.height;
diff --git a/common/include/pcl/common/impl/pca.hpp b/common/include/pcl/common/impl/pca.hpp
index 98b6cb47..fac30862 100644
--- a/common/include/pcl/common/impl/pca.hpp
+++ b/common/include/pcl/common/impl/pca.hpp
@@ -38,6 +38,8 @@
 
 #pragma once
 
+#include <Eigen/Eigenvalues> // for SelfAdjointEigenSolver
+
 #include <pcl/common/centroid.h>
 #include <pcl/common/eigen.h>
 #include <pcl/common/pca.h>
diff --git a/common/include/pcl/common/impl/transformation_from_correspondences.hpp b/common/include/pcl/common/impl/transformation_from_correspondences.hpp
index 71849fff..71b6054b 100644
--- a/common/include/pcl/common/impl/transformation_from_correspondences.hpp
+++ b/common/include/pcl/common/impl/transformation_from_correspondences.hpp
@@ -37,7 +37,6 @@
 
 #pragma once
 
-#include <pcl/common/eigen.h>
 #include <pcl/common/transformation_from_correspondences.h>
 
 
diff --git a/common/include/pcl/common/impl/transforms.hpp b/common/include/pcl/common/impl/transforms.hpp
index a523f4f9..7c51e8fb 100644
--- a/common/include/pcl/common/impl/transforms.hpp
+++ b/common/include/pcl/common/impl/transforms.hpp
@@ -220,25 +220,23 @@ struct Transformer<double>
 template <typename PointT, typename Scalar> void
 transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
                      pcl::PointCloud<PointT> &cloud_out,
-                     const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
+                     const Eigen::Matrix<Scalar, 4, 4> &transform,
                      bool copy_all_fields)
 {
   if (&cloud_in != &cloud_out)
   {
     cloud_out.header   = cloud_in.header;
     cloud_out.is_dense = cloud_in.is_dense;
-    cloud_out.width    = cloud_in.width;
-    cloud_out.height   = cloud_in.height;
-    cloud_out.points.reserve (cloud_in.size ());
+    cloud_out.reserve (cloud_in.size ());
     if (copy_all_fields)
-      cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
+      cloud_out.assign (cloud_in.begin (), cloud_in.end (), cloud_in.width);
     else
-      cloud_out.points.resize (cloud_in.size ());
+      cloud_out.resize (cloud_in.width, cloud_in.height);
     cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
     cloud_out.sensor_origin_      = cloud_in.sensor_origin_;
   }
 
-  pcl::detail::Transformer<Scalar> tf (transform.matrix ());
+  pcl::detail::Transformer<Scalar> tf (transform);
   if (cloud_in.is_dense)
   {
     // If the dataset is dense, simply transform it!
@@ -265,7 +263,7 @@ template <typename PointT, typename Scalar> void
 transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
                      const Indices &indices,
                      pcl::PointCloud<PointT> &cloud_out,
-                     const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
+                     const Eigen::Matrix<Scalar, 4, 4> &transform,
                      bool copy_all_fields)
 {
   std::size_t npts = indices.size ();
@@ -274,11 +272,11 @@ transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
   cloud_out.header   = cloud_in.header;
   cloud_out.width    = static_cast<int> (npts);
   cloud_out.height   = 1;
-  cloud_out.points.resize (npts);
+  cloud_out.resize (npts);
   cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
   cloud_out.sensor_origin_      = cloud_in.sensor_origin_;
 
-  pcl::detail::Transformer<Scalar> tf (transform.matrix ());
+  pcl::detail::Transformer<Scalar> tf (transform);
   if (cloud_in.is_dense)
   {
     // If the dataset is dense, simply transform it!
@@ -311,26 +309,24 @@ transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
 template <typename PointT, typename Scalar> void
 transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
                                 pcl::PointCloud<PointT> &cloud_out,
-                                const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
+                                const Eigen::Matrix<Scalar, 4, 4> &transform,
                                 bool copy_all_fields)
 {
   if (&cloud_in != &cloud_out)
   {
     // Note: could be replaced by cloud_out = cloud_in
     cloud_out.header   = cloud_in.header;
-    cloud_out.width    = cloud_in.width;
-    cloud_out.height   = cloud_in.height;
     cloud_out.is_dense = cloud_in.is_dense;
-    cloud_out.points.reserve (cloud_out.size ());
+    cloud_out.reserve (cloud_in.size ());
     if (copy_all_fields)
-      cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
+      cloud_out.assign (cloud_in.begin (), cloud_in.end (), cloud_in.width);
     else
-      cloud_out.points.resize (cloud_in.size ());
+      cloud_out.resize (cloud_in.width, cloud_in.height);
     cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
     cloud_out.sensor_origin_      = cloud_in.sensor_origin_;
   }
 
-  pcl::detail::Transformer<Scalar> tf (transform.matrix ());
+  pcl::detail::Transformer<Scalar> tf (transform);
   // If the data is dense, we don't need to check for NaN
   if (cloud_in.is_dense)
   {
@@ -360,7 +356,7 @@ template <typename PointT, typename Scalar> void
 transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
                                 const Indices &indices,
                                 pcl::PointCloud<PointT> &cloud_out,
-                                const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
+                                const Eigen::Matrix<Scalar, 4, 4> &transform,
                                 bool copy_all_fields)
 {
   std::size_t npts = indices.size ();
@@ -369,11 +365,11 @@ transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
   cloud_out.header   = cloud_in.header;
   cloud_out.width    = static_cast<int> (npts);
   cloud_out.height   = 1;
-  cloud_out.points.resize (npts);
+  cloud_out.resize (npts);
   cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
   cloud_out.sensor_origin_      = cloud_in.sensor_origin_;
 
-  pcl::detail::Transformer<Scalar> tf (transform.matrix ());
+  pcl::detail::Transformer<Scalar> tf (transform);
   // If the data is dense, we don't need to check for NaN
   if (cloud_in.is_dense)
   {
@@ -479,4 +475,3 @@ getPrincipalTransformation (const pcl::PointCloud<PointT> &cloud,
 }
 
 } // namespace pcl
-
diff --git a/common/include/pcl/common/impl/vector_average.hpp b/common/include/pcl/common/impl/vector_average.hpp
index 196bccf7..5d067d61 100644
--- a/common/include/pcl/common/impl/vector_average.hpp
+++ b/common/include/pcl/common/impl/vector_average.hpp
@@ -37,8 +37,10 @@
 
 #pragma once
 
+#include <pcl/common/eigen.h> // for computeRoots, eigen33
 #include <pcl/common/vector_average.h>
 
+#include <Eigen/Eigenvalues> // for SelfAdjointEigenSolver
 
 namespace pcl
 {
diff --git a/common/include/pcl/common/io.h b/common/include/pcl/common/io.h
index e6850b8d..4e765e4e 100644
--- a/common/include/pcl/common/io.h
+++ b/common/include/pcl/common/io.h
@@ -43,10 +43,8 @@
 #include <numeric>
 #include <string>
 
-#include <pcl/pcl_base.h>
+#include <pcl/point_cloud.h>
 #include <pcl/PointIndices.h>
-#include <pcl/conversions.h>
-#include <pcl/exceptions.h>
 #include <pcl/pcl_macros.h>
 #include <pcl/PolygonMesh.h>
 #include <locale>
@@ -69,18 +67,6 @@ namespace pcl
     return std::distance(cloud.fields.begin (), result);
   }
 
-  /** \brief Get the index of a specified field (i.e., dimension/channel)
-    * \param[in] cloud the point cloud message
-    * \param[in] field_name the string defining the field name
-    * \param[out] fields a vector to the original \a PCLPointField vector that the raw PointCloud message contains
-    * \ingroup common
-    */
-  template <typename PointT>
-  PCL_DEPRECATED(1, 12, "use getFieldIndex<PointT> (field_name, fields) instead")
-  inline int
-  getFieldIndex (const pcl::PointCloud<PointT> &cloud, const std::string &field_name, 
-                 std::vector<pcl::PCLPointField> &fields);
-
   /** \brief Get the index of a specified field (i.e., dimension/channel)
     * \tparam PointT datatype for which fields is being queries
     * \param[in] field_name the string defining the field name
@@ -100,26 +86,6 @@ namespace pcl
   getFieldIndex (const std::string &field_name,
                  const std::vector<pcl::PCLPointField> &fields);
 
-  /** \brief Get the list of available fields (i.e., dimension/channel)
-    * \param[in] cloud the point cloud message
-    * \param[out] fields a vector to the original \a PCLPointField vector that the raw PointCloud message contains
-    * \ingroup common
-    */
-  template <typename PointT>
-  PCL_DEPRECATED(1, 12, "use getFields<PointT> () with return value instead")
-  inline void
-  getFields (const pcl::PointCloud<PointT> &cloud, std::vector<pcl::PCLPointField> &fields);
-
-  /** \brief Get the list of available fields (i.e., dimension/channel)
-    * \tparam PointT datatype whose details are requested
-    * \param[out] fields a vector to the original \a PCLPointField vector that the raw PointCloud message contains
-    * \ingroup common
-    */
-  template <typename PointT>
-  PCL_DEPRECATED(1, 12, "use getFields<PointT> () with return value instead")
-  inline void
-  getFields (std::vector<pcl::PCLPointField> &fields);
-
   /** \brief Get the list of available fields (i.e., dimension/channel)
     * \tparam PointT datatype whose details are requested
     * \ingroup common
@@ -288,10 +254,8 @@ namespace pcl
 
   /** \brief Concatenate two pcl::PCLPointCloud2
     *
-    * \warning This function subtly differs from the deprecated concatenatePointCloud()
-    * The difference is that this function will concatenate IFF the non-skip fields
-    * are in the correct order and same in number. The deprecated function skipped
-    * fields even if both clouds didn't agree on the number of output fields
+    * \warning This function will concatenate IFF the non-skip fields are in the correct
+    * order and same in number.
     * \param[in] cloud1 the first input point cloud dataset
     * \param[in] cloud2 the second input point cloud dataset
     * \param[out] cloud_out the resultant output point cloud dataset
@@ -321,19 +285,6 @@ namespace pcl
     return pcl::PolygonMesh::concatenate(mesh1, mesh2, mesh_out);
   }
 
-  /** \brief Concatenate two pcl::PCLPointCloud2
-    * \param[in] cloud1 the first input point cloud dataset
-    * \param[in] cloud2 the second input point cloud dataset
-    * \param[out] cloud_out the resultant output point cloud dataset
-    * \return true if successful, false otherwise (e.g., name/number of fields differs)
-    * \ingroup common
-    */
-  PCL_DEPRECATED(1, 12, "use pcl::concatenate() instead, but beware of subtle difference in behavior (see documentation)")
-  PCL_EXPORTS bool
-  concatenatePointCloud (const pcl::PCLPointCloud2 &cloud1,
-                         const pcl::PCLPointCloud2 &cloud2,
-                         pcl::PCLPointCloud2 &cloud_out);
-
   /** \brief Extract the indices of a given point cloud as a new point cloud
     * \param[in] cloud_in the input point cloud dataset
     * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
@@ -355,7 +306,7 @@ namespace pcl
     */
   PCL_EXPORTS void
   copyPointCloud (const pcl::PCLPointCloud2 &cloud_in,
-                  const IndicesAllocator< Eigen::aligned_allocator<int> > &indices,
+                  const IndicesAllocator< Eigen::aligned_allocator<index_t> > &indices,
                   pcl::PCLPointCloud2 &cloud_out);
 
   /** \brief Copy fields and point cloud data from \a cloud_in to \a cloud_out
@@ -381,7 +332,7 @@ namespace pcl
     * \note Assumes unique indices.
     * \ingroup common
     */
-  template <typename PointT, typename IndicesVectorAllocator = std::allocator<int>> void
+  template <typename PointT, typename IndicesVectorAllocator = std::allocator<index_t>> void
   copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
                   const IndicesAllocator< IndicesVectorAllocator> &indices,
                   pcl::PointCloud<PointT> &cloud_out);
@@ -426,7 +377,7 @@ namespace pcl
     * \note Assumes unique indices.
     * \ingroup common
     */
-  template <typename PointInT, typename PointOutT, typename IndicesVectorAllocator = std::allocator<int>> void
+  template <typename PointInT, typename PointOutT, typename IndicesVectorAllocator = std::allocator<index_t>> void
   copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
                   const IndicesAllocator<IndicesVectorAllocator> &indices,
                   pcl::PointCloud<PointOutT> &cloud_out);
diff --git a/common/include/pcl/common/point_tests.h b/common/include/pcl/common/point_tests.h
index abd58c35..dc11edef 100644
--- a/common/include/pcl/common/point_tests.h
+++ b/common/include/pcl/common/point_tests.h
@@ -69,8 +69,13 @@ namespace pcl
   template<> inline bool isFinite<pcl::BRISKSignature512>(const pcl::BRISKSignature512&) { return (true); }
   template<> inline bool isFinite<pcl::BorderDescription>(const pcl::BorderDescription &) { return true; }
   template<> inline bool isFinite<pcl::Boundary>(const pcl::Boundary&) { return (true); }
+  template<> inline bool isFinite<pcl::CPPFSignature>(const pcl::CPPFSignature&) { return (true); }
   template<> inline bool isFinite<pcl::ESFSignature640>(const pcl::ESFSignature640&) { return (true); }
   template<> inline bool isFinite<pcl::FPFHSignature33>(const pcl::FPFHSignature33&) { return (true); }
+  template<> inline bool isFinite<pcl::GASDSignature512>(const pcl::GASDSignature512&) { return (true); }
+  template<> inline bool isFinite<pcl::GASDSignature984>(const pcl::GASDSignature984&) { return (true); }
+  template<> inline bool isFinite<pcl::GASDSignature7992>(const pcl::GASDSignature7992&) { return (true); }
+  template<> inline bool isFinite<pcl::GRSDSignature21>(const pcl::GRSDSignature21&) { return (true); }
   template<> inline bool isFinite<pcl::Intensity>(const pcl::Intensity&) { return (true); }
   template<> inline bool isFinite<pcl::IntensityGradient>(const pcl::IntensityGradient&) { return (true); }
   template<> inline bool isFinite<pcl::Label>(const pcl::Label&) { return (true); }
@@ -79,7 +84,12 @@ namespace pcl
   template<> inline bool isFinite<pcl::PFHRGBSignature250>(const pcl::PFHRGBSignature250&) { return (true); }
   template<> inline bool isFinite<pcl::PFHSignature125>(const pcl::PFHSignature125&) { return (true); }
   template<> inline bool isFinite<pcl::PPFRGBSignature>(const pcl::PPFRGBSignature&) { return (true); }
-  template<> inline bool isFinite<pcl::PPFSignature>(const pcl::PPFSignature&) { return (true); }
+  
+  template<> inline bool isFinite<pcl::PPFSignature>(const pcl::PPFSignature& pt)
+  {
+    return std::isfinite(pt.f1) && std::isfinite(pt.f2) && std::isfinite(pt.f3) && std::isfinite(pt.f4) && std::isfinite(pt.alpha_m);
+  }
+
   template<> inline bool isFinite<pcl::PrincipalCurvatures>(const pcl::PrincipalCurvatures&) { return (true); }
   template<> inline bool isFinite<pcl::PrincipalRadiiRSD>(const pcl::PrincipalRadiiRSD&) { return (true); }
   template<> inline bool isFinite<pcl::RGB>(const pcl::RGB&) { return (true); }
diff --git a/common/include/pcl/common/random.h b/common/include/pcl/common/random.h
index 6262172b..6f8a2bc3 100644
--- a/common/include/pcl/common/random.h
+++ b/common/include/pcl/common/random.h
@@ -69,7 +69,7 @@ namespace pcl
     };
 
     /** \brief UniformGenerator class generates a random number from range [min, max] at each run picked
-      * according to a uniform distribution i.e eaach number within [min, max] has almost the same 
+      * according to a uniform distribution i.e each number within [min, max] has almost the same 
       * probability of being drawn.
       *
       * \author Nizar Sallem
diff --git a/common/include/pcl/common/time_trigger.h b/common/include/pcl/common/time_trigger.h
index b9cfa72c..1432cdcf 100644
--- a/common/include/pcl/common/time_trigger.h
+++ b/common/include/pcl/common/time_trigger.h
@@ -39,9 +39,8 @@
 #pragma once
 
 #include <pcl/pcl_macros.h>
-#ifndef Q_MOC_RUN
+
 #include <boost/signals2.hpp>
-#endif
 
 #include <condition_variable>
 #include <functional>
diff --git a/common/include/pcl/common/transformation_from_correspondences.h b/common/include/pcl/common/transformation_from_correspondences.h
index aae7e724..e8762076 100644
--- a/common/include/pcl/common/transformation_from_correspondences.h
+++ b/common/include/pcl/common/transformation_from_correspondences.h
@@ -36,7 +36,8 @@
 
 #pragma once
 
-#include <pcl/common/eigen.h>
+#include <Eigen/Core> // for Vector3f, Matrix
+#include <Eigen/Geometry> // for Affine3f
 
 namespace pcl 
 {
diff --git a/common/include/pcl/common/transforms.h b/common/include/pcl/common/transforms.h
index 59c9e82d..347643f2 100644
--- a/common/include/pcl/common/transforms.h
+++ b/common/include/pcl/common/transforms.h
@@ -60,7 +60,10 @@ namespace pcl
   transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
                        pcl::PointCloud<PointT> &cloud_out, 
                        const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
-                       bool copy_all_fields = true);
+                       bool copy_all_fields = true)
+  {
+    return (transformPointCloud<PointT, Scalar> (cloud_in, cloud_out, transform.matrix (), copy_all_fields));
+  }
 
   template <typename PointT> void 
   transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
@@ -68,7 +71,7 @@ namespace pcl
                        const Eigen::Affine3f &transform,
                        bool copy_all_fields = true)
   {
-    return (transformPointCloud<PointT, float> (cloud_in, cloud_out, transform, copy_all_fields));
+    return (transformPointCloud<PointT, float> (cloud_in, cloud_out, transform.matrix (), copy_all_fields));
   }
 
   /** \brief Apply an affine transform defined by an Eigen Transform
@@ -85,7 +88,10 @@ namespace pcl
                        const Indices &indices,
                        pcl::PointCloud<PointT> &cloud_out, 
                        const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
-                       bool copy_all_fields = true);
+                       bool copy_all_fields = true)
+  {
+    return (transformPointCloud<PointT, Scalar> (cloud_in, indices, cloud_out, transform.matrix (), copy_all_fields));
+  }
 
   template <typename PointT> void 
   transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
@@ -94,7 +100,7 @@ namespace pcl
                        const Eigen::Affine3f &transform,
                        bool copy_all_fields = true)
   {
-    return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform, copy_all_fields));
+    return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform.matrix (), copy_all_fields));
   }
 
   /** \brief Apply an affine transform defined by an Eigen Transform
@@ -113,7 +119,7 @@ namespace pcl
                        const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
                        bool copy_all_fields = true)
   {
-    return (transformPointCloud<PointT, Scalar> (cloud_in, indices.indices, cloud_out, transform, copy_all_fields));
+    return (transformPointCloud<PointT, Scalar> (cloud_in, indices.indices, cloud_out, transform.matrix (), copy_all_fields));
   }
 
   template <typename PointT> void 
@@ -123,7 +129,7 @@ namespace pcl
                        const Eigen::Affine3f &transform,
                        bool copy_all_fields = true)
   {
-    return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform, copy_all_fields));
+    return (transformPointCloud<PointT, float> (cloud_in, indices.indices, cloud_out, transform.matrix (), copy_all_fields));
   }
 
   /** \brief Transform a point cloud and rotate its normals using an Eigen transform.
@@ -139,7 +145,10 @@ namespace pcl
   transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
                                   pcl::PointCloud<PointT> &cloud_out, 
                                   const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
-                                  bool copy_all_fields = true);
+                                  bool copy_all_fields = true)
+  {
+    return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, cloud_out, transform.matrix (), copy_all_fields));
+  }
 
   template <typename PointT> void 
   transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
@@ -147,7 +156,7 @@ namespace pcl
                                   const Eigen::Affine3f &transform,
                                   bool copy_all_fields = true)
   {
-    return (transformPointCloudWithNormals<PointT, float> (cloud_in, cloud_out, transform, copy_all_fields));
+    return (transformPointCloudWithNormals<PointT, float> (cloud_in, cloud_out, transform.matrix (), copy_all_fields));
   }
 
   /** \brief Transform a point cloud and rotate its normals using an Eigen transform.
@@ -164,7 +173,10 @@ namespace pcl
                                   const Indices &indices,
                                   pcl::PointCloud<PointT> &cloud_out, 
                                   const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
-                                  bool copy_all_fields = true);
+                                  bool copy_all_fields = true)
+  {
+    return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, indices, cloud_out, transform.matrix (), copy_all_fields));
+  }
 
   template <typename PointT> void 
   transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
@@ -173,7 +185,7 @@ namespace pcl
                                   const Eigen::Affine3f &transform,
                                   bool copy_all_fields = true)
   {
-    return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform, copy_all_fields));
+    return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform.matrix (), copy_all_fields));
   }
 
   /** \brief Transform a point cloud and rotate its normals using an Eigen transform.
@@ -192,7 +204,7 @@ namespace pcl
                                   const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
                                   bool copy_all_fields = true)
   {
-    return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, indices.indices, cloud_out, transform, copy_all_fields));
+    return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, indices.indices, cloud_out, transform.matrix (), copy_all_fields));
   }
 
 
@@ -203,7 +215,7 @@ namespace pcl
                                   const Eigen::Affine3f &transform,
                                   bool copy_all_fields = true)
   {
-    return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform, copy_all_fields));
+    return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices.indices, cloud_out, transform.matrix (), copy_all_fields));
   }
 
   /** \brief Apply a rigid transform defined by a 4x4 matrix
@@ -219,11 +231,7 @@ namespace pcl
   transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
                        pcl::PointCloud<PointT> &cloud_out, 
                        const Eigen::Matrix<Scalar, 4, 4> &transform,
-                       bool copy_all_fields = true)
-  {
-    Eigen::Transform<Scalar, 3, Eigen::Affine> t (transform);
-    return (transformPointCloud<PointT, Scalar> (cloud_in, cloud_out, t, copy_all_fields));
-  }
+                       bool copy_all_fields = true);
 
   template <typename PointT> void 
   transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
@@ -248,11 +256,7 @@ namespace pcl
                        const Indices &indices,
                        pcl::PointCloud<PointT> &cloud_out, 
                        const Eigen::Matrix<Scalar, 4, 4> &transform,
-                       bool copy_all_fields = true)
-  {
-    Eigen::Transform<Scalar, 3, Eigen::Affine> t (transform);
-    return (transformPointCloud<PointT, Scalar> (cloud_in, indices, cloud_out, t, copy_all_fields));
-  }
+                       bool copy_all_fields = true);
 
   template <typename PointT> void 
   transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
@@ -307,11 +311,7 @@ namespace pcl
   transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
                                   pcl::PointCloud<PointT> &cloud_out, 
                                   const Eigen::Matrix<Scalar, 4, 4> &transform,
-                                  bool copy_all_fields = true)
-  {
-    Eigen::Transform<Scalar, 3, Eigen::Affine> t (transform);
-    return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, cloud_out, t, copy_all_fields));
-  }
+                                  bool copy_all_fields = true);
 
 
   template <typename PointT> void 
@@ -339,11 +339,7 @@ namespace pcl
                                   const Indices &indices,
                                   pcl::PointCloud<PointT> &cloud_out, 
                                   const Eigen::Matrix<Scalar, 4, 4> &transform,
-                                  bool copy_all_fields = true)
-  {
-    Eigen::Transform<Scalar, 3, Eigen::Affine> t (transform);
-    return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, indices, cloud_out, t, copy_all_fields));
-  }
+                                  bool copy_all_fields = true);
 
 
   template <typename PointT> void 
@@ -374,8 +370,7 @@ namespace pcl
                                   const Eigen::Matrix<Scalar, 4, 4> &transform,
                                   bool copy_all_fields = true)
   {
-    Eigen::Transform<Scalar, 3, Eigen::Affine> t (transform);
-    return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, indices, cloud_out, t, copy_all_fields));
+    return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, indices.indices, cloud_out, transform, copy_all_fields));
   }
 
 
@@ -386,7 +381,7 @@ namespace pcl
                                   const Eigen::Matrix4f &transform,
                                   bool copy_all_fields = true)
   {
-    return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform, copy_all_fields));
+    return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices.indices, cloud_out, transform, copy_all_fields));
   }
 
   /** \brief Apply a rigid transform defined by a 3D offset and a quaternion
diff --git a/common/include/pcl/common/vector_average.h b/common/include/pcl/common/vector_average.h
index 3cd07635..10e3626f 100644
--- a/common/include/pcl/common/vector_average.h
+++ b/common/include/pcl/common/vector_average.h
@@ -37,9 +37,10 @@
 
 #pragma once
 
+#include <Eigen/Core> // for Matrix
+
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
-#include <pcl/common/eigen.h>
 
 namespace pcl 
 {
diff --git a/common/include/pcl/console/print.h b/common/include/pcl/console/print.h
index 59617714..0e84427b 100644
--- a/common/include/pcl/console/print.h
+++ b/common/include/pcl/console/print.h
@@ -38,11 +38,22 @@
 #pragma once
 
 #include <cstdio>
-#include <cstdarg>
 
 #include <pcl/pcl_exports.h>
 #include <pcl/pcl_config.h>
 
+// Use e.g. like this:
+// PCL_INFO_STREAM("Info: this is a point: " << pcl::PointXYZ(1.0, 2.0, 3.0) << std::endl);
+// PCL_ERROR_STREAM("Error: an Eigen vector: " << std::endl << Eigen::Vector3f(1.0, 2.0, 3.0) << std::endl);
+#define PCL_LOG_STREAM(LEVEL, STREAM, CSTR, ATTR, FG, ARGS) if(pcl::console::isVerbosityLevelEnabled(pcl::console::LEVEL)) { fflush(stdout); pcl::console::change_text_color(CSTR, pcl::console::ATTR, pcl::console::FG); STREAM << ARGS; pcl::console::reset_text_color(CSTR); }
+#define PCL_ALWAYS_STREAM(ARGS)  PCL_LOG_STREAM(L_ALWAYS,  std::cout, stdout, TT_RESET,  TT_WHITE,  ARGS)
+#define PCL_ERROR_STREAM(ARGS)   PCL_LOG_STREAM(L_ERROR,   std::cerr, stderr, TT_BRIGHT, TT_RED,    ARGS)
+#define PCL_WARN_STREAM(ARGS)    PCL_LOG_STREAM(L_WARN,    std::cerr, stderr, TT_BRIGHT, TT_YELLOW, ARGS)
+#define PCL_INFO_STREAM(ARGS)    PCL_LOG_STREAM(L_INFO,    std::cout, stdout, TT_RESET,  TT_WHITE,  ARGS)
+#define PCL_DEBUG_STREAM(ARGS)   PCL_LOG_STREAM(L_DEBUG,   std::cout, stdout, TT_RESET,  TT_GREEN,  ARGS)
+#define PCL_VERBOSE_STREAM(ARGS) PCL_LOG_STREAM(L_VERBOSE, std::cout, stdout, TT_RESET,  TT_WHITE,  ARGS)
+
+
 #define PCL_ALWAYS(...)  pcl::console::print (pcl::console::L_ALWAYS, __VA_ARGS__)
 #define PCL_ERROR(...)   pcl::console::print (pcl::console::L_ERROR, __VA_ARGS__)
 #define PCL_WARN(...)    pcl::console::print (pcl::console::L_WARN, __VA_ARGS__)
diff --git a/common/include/pcl/conversions.h b/common/include/pcl/conversions.h
index f1904921..7933a090 100644
--- a/common/include/pcl/conversions.h
+++ b/common/include/pcl/conversions.h
@@ -49,11 +49,9 @@
 #include <pcl/point_cloud.h>
 #include <pcl/type_traits.h>
 #include <pcl/for_each_type.h>
-#include <pcl/exceptions.h>
 #include <pcl/console/print.h>
-#ifndef Q_MOC_RUN
+
 #include <boost/foreach.hpp>
-#endif
 
 namespace pcl
 {
@@ -68,10 +66,10 @@ namespace pcl
       template<typename U> void operator() ()
       {
         pcl::PCLPointField f;
-        f.name = traits::name<PointT, U>::value;
-        f.offset = traits::offset<PointT, U>::value;
-        f.datatype = traits::datatype<PointT, U>::value;
-        f.count = traits::datatype<PointT, U>::size;
+        f.name = pcl::traits::name<PointT, U>::value;
+        f.offset = pcl::traits::offset<PointT, U>::value;
+        f.datatype = pcl::traits::datatype<PointT, U>::value;
+        f.count = pcl::traits::datatype<PointT, U>::size;
         fields_.push_back (f);
       }
 
@@ -97,14 +95,14 @@ namespace pcl
           {
             FieldMapping mapping;
             mapping.serialized_offset = field.offset;
-            mapping.struct_offset = traits::offset<PointT, Tag>::value;
-            mapping.size = sizeof (typename traits::datatype<PointT, Tag>::type);
+            mapping.struct_offset = pcl::traits::offset<PointT, Tag>::value;
+            mapping.size = sizeof (typename pcl::traits::datatype<PointT, Tag>::type);
             map_.push_back (mapping);
             return;
           }
         }
         // Disable thrown exception per #595: http://dev.pointclouds.org/issues/595
-        PCL_WARN ("Failed to find match for field '%s'.\n", traits::name<PointT, Tag>::value);
+        PCL_WARN ("Failed to find match for field '%s'.\n", pcl::traits::name<PointT, Tag>::value);
         //throw pcl::InvalidConversionException (ss.str ());
       }
 
@@ -176,7 +174,7 @@ namespace pcl
 
     // Copy point data
     std::uint32_t num_points = msg.width * msg.height;
-    cloud.points.resize (num_points);
+    cloud.resize (num_points);
     std::uint8_t* cloud_data = reinterpret_cast<std::uint8_t*>(&cloud[0]);
 
     // Check if we can copy adjacent points in a single memcpy.  We can do so if there
@@ -205,10 +203,10 @@ namespace pcl
     else
     {
       // If not, memcpy each group of contiguous fields separately
-      for (std::uint32_t row = 0; row < msg.height; ++row)
+      for (index_t row = 0; row < msg.height; ++row)
       {
         const std::uint8_t* row_data = &msg.data[row * msg.row_step];
-        for (std::uint32_t col = 0; col < msg.width; ++col)
+        for (index_t col = 0; col < msg.width; ++col)
         {
           const std::uint8_t* msg_data = row_data + col * msg.point_step;
           for (const detail::FieldMapping& mapping : field_map)
@@ -293,6 +291,7 @@ namespace pcl
     }
 
     // ensor_msgs::image_encodings::BGR8;
+    msg.header = cloud.header;
     msg.encoding = "bgr8";
     msg.step = msg.width * sizeof (std::uint8_t) * 3;
     msg.data.resize (msg.step * msg.height);
@@ -331,6 +330,7 @@ namespace pcl
     int point_step = cloud.point_step;
 
     // pcl::image_encodings::BGR8;
+    msg.header = cloud.header;
     msg.encoding = "bgr8";
     msg.step = static_cast<std::uint32_t>(msg.width * sizeof (std::uint8_t) * 3);
     msg.data.resize (msg.step * msg.height);
diff --git a/common/include/pcl/correspondence.h b/common/include/pcl/correspondence.h
index 01faa1ba..0b8d1f8b 100644
--- a/common/include/pcl/correspondence.h
+++ b/common/include/pcl/correspondence.h
@@ -48,7 +48,6 @@
 #include <Eigen/StdVector>
 #include <Eigen/Geometry>
 #include <pcl/pcl_exports.h>
-#include <pcl/pcl_macros.h>
 
 namespace pcl
 {
diff --git a/common/include/pcl/exceptions.h b/common/include/pcl/exceptions.h
index 10a00798..5053d887 100644
--- a/common/include/pcl/exceptions.h
+++ b/common/include/pcl/exceptions.h
@@ -39,7 +39,6 @@
 
 #include <stdexcept>
 #include <sstream>
-#include <pcl/pcl_macros.h>
 #include <boost/current_function.hpp>
 
 /** PCL_THROW_EXCEPTION a helper macro to be used for throwing exceptions.
diff --git a/common/include/pcl/for_each_type.h b/common/include/pcl/for_each_type.h
index 3abbad74..9e0a6af0 100644
--- a/common/include/pcl/for_each_type.h
+++ b/common/include/pcl/for_each_type.h
@@ -43,7 +43,6 @@
 #pragma GCC system_header 
 #endif
 
-#ifndef Q_MOC_RUN
 #include <boost/mpl/is_sequence.hpp>
 #include <boost/mpl/begin_end.hpp>
 #include <boost/mpl/next_prior.hpp>
@@ -53,7 +52,6 @@
 #include <boost/mpl/contains.hpp>
 #include <boost/mpl/not.hpp>
 #include <boost/mpl/aux_/unwrap.hpp>
-#endif
 
 #include <type_traits>
 
diff --git a/common/include/pcl/impl/pcl_base.hpp b/common/include/pcl/impl/pcl_base.hpp
index ab876ac5..3caf96c4 100644
--- a/common/include/pcl/impl/pcl_base.hpp
+++ b/common/include/pcl/impl/pcl_base.hpp
@@ -100,27 +100,27 @@ pcl::PCLBase<PointT>::setIndices (std::size_t row_start, std::size_t col_start,
 {
   if ((nb_rows > input_->height) || (row_start > input_->height))
   {
-    PCL_ERROR ("[PCLBase::setIndices] cloud is only %d height", input_->height);
+    PCL_ERROR ("[PCLBase::setIndices] cloud is only %d height\n", input_->height);
     return;
   }
 
   if ((nb_cols > input_->width) || (col_start > input_->width))
   {
-    PCL_ERROR ("[PCLBase::setIndices] cloud is only %d width", input_->width);
+    PCL_ERROR ("[PCLBase::setIndices] cloud is only %d width\n", input_->width);
     return;
   }
 
   std::size_t row_end = row_start + nb_rows;
   if (row_end > input_->height)
   {
-    PCL_ERROR ("[PCLBase::setIndices] %d is out of rows range %d", row_end, input_->height);
+    PCL_ERROR ("[PCLBase::setIndices] %d is out of rows range %d\n", row_end, input_->height);
     return;
   }
 
   std::size_t col_end = col_start + nb_cols;
   if (col_end > input_->width)
   {
-    PCL_ERROR ("[PCLBase::setIndices] %d is out of columns range %d", col_end, input_->width);
+    PCL_ERROR ("[PCLBase::setIndices] %d is out of columns range %d\n", col_end, input_->width);
     return;
   }
 
@@ -139,7 +139,10 @@ pcl::PCLBase<PointT>::initCompute ()
 {
   // Check if input was set
   if (!input_)
+  {
+    PCL_ERROR ("[initCompute] No input set.\n");
     return (false);
+  }
 
   // If no point indices have been given, construct a set of indices for the entire input point cloud
   if (!indices_)
diff --git a/common/include/pcl/impl/point_types.hpp b/common/include/pcl/impl/point_types.hpp
index aaba2db3..f17b1bc9 100644
--- a/common/include/pcl/impl/point_types.hpp
+++ b/common/include/pcl/impl/point_types.hpp
@@ -68,6 +68,7 @@
   (pcl::PointXYZRGBA)           \
   (pcl::PointXYZRGB)            \
   (pcl::PointXYZRGBL)           \
+  (pcl::PointXYZLAB)            \
   (pcl::PointXYZHSV)            \
   (pcl::PointXY)                \
   (pcl::InterestPoint)          \
@@ -125,6 +126,7 @@
   (pcl::PointXYZRGBA)         \
   (pcl::PointXYZRGB)          \
   (pcl::PointXYZRGBL)         \
+  (pcl::PointXYZLAB)          \
   (pcl::PointXYZHSV)          \
   (pcl::InterestPoint)        \
   (pcl::PointNormal)          \
@@ -170,8 +172,52 @@
   (pcl::BRISKSignature512)      \
   (pcl::Narf36)
 
+// Define all point types that have descriptorSize() member function
+#define PCL_DESCRIPTOR_FEATURE_POINT_TYPES \
+  (pcl::PFHSignature125)        \
+  (pcl::PFHRGBSignature250)     \
+  (pcl::FPFHSignature33)        \
+  (pcl::VFHSignature308)        \
+  (pcl::GASDSignature512)       \
+  (pcl::GASDSignature984)       \
+  (pcl::GASDSignature7992)      \
+  (pcl::GRSDSignature21)        \
+  (pcl::ESFSignature640)        \
+  (pcl::BRISKSignature512)      \
+  (pcl::Narf36)
+
+
 namespace pcl
 {
+  namespace detail
+  {
+    namespace traits
+    {
+      template<typename FeaturePointT> struct descriptorSize {};
+   
+      template<> struct descriptorSize<PFHSignature125> { static constexpr const int value = 125; };
+      template<> struct descriptorSize<PFHRGBSignature250> { static constexpr const int value = 250; };
+      template<> struct descriptorSize<ShapeContext1980> { static constexpr const int value = 1980; };
+      template<> struct descriptorSize<UniqueShapeContext1960> { static constexpr const int value = 1960; };
+      template<> struct descriptorSize<SHOT352> { static constexpr const int value = 352; };
+      template<> struct descriptorSize<SHOT1344> { static constexpr const int value = 1344; };
+      template<> struct descriptorSize<FPFHSignature33> { static constexpr const int value = 33; };
+      template<> struct descriptorSize<VFHSignature308> { static constexpr const int value = 308; };
+      template<> struct descriptorSize<GRSDSignature21> { static constexpr const int value = 21; };
+      template<> struct descriptorSize<BRISKSignature512> { static constexpr const int value = 512; };
+      template<> struct descriptorSize<ESFSignature640> { static constexpr const int value = 640; };
+      template<> struct descriptorSize<GASDSignature512> { static constexpr const int value = 512; };
+      template<> struct descriptorSize<GASDSignature984> { static constexpr const int value = 984; };
+      template<> struct descriptorSize<GASDSignature7992> { static constexpr const int value = 7992; };
+      template<> struct descriptorSize<GFPFHSignature16> { static constexpr const int value = 16; };
+      template<> struct descriptorSize<Narf36> { static constexpr const int value = 36; };
+      template<int N> struct descriptorSize<Histogram<N>> { static constexpr const int value = N; };
+
+
+      template<typename FeaturePointT>
+      static constexpr int descriptorSize_v = descriptorSize<FeaturePointT>::value;
+    }
+  }
 
   using Array3fMap = Eigen::Map<Eigen::Array3f>;
   using Array3fMapConst = const Eigen::Map<const Eigen::Array3f>;
@@ -558,13 +604,13 @@ namespace pcl
       rgba = p.rgba;
     }
 
-    inline PointXYZRGBA (): PointXYZRGBA (0, 0, 0, 0) {}
+    inline PointXYZRGBA (): PointXYZRGBA (0, 0, 0, 255) {}
 
     inline PointXYZRGBA (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b, std::uint8_t _a):
       PointXYZRGBA (0.f, 0.f, 0.f, _r, _g, _b, _a) {}
 
     inline PointXYZRGBA (float _x, float _y, float _z):
-      PointXYZRGBA (_x, _y, _z, 0, 0, 0, 0) {}
+      PointXYZRGBA (_x, _y, _z, 0, 0, 0, 255) {}
 
     inline PointXYZRGBA (float _x, float _y, float _z, std::uint8_t _r,
                          std::uint8_t _g, std::uint8_t _b, std::uint8_t _a)
@@ -690,6 +736,47 @@ namespace pcl
   };
 
 
+  struct EIGEN_ALIGN16 _PointXYZLAB
+  {
+    PCL_ADD_POINT4D; // this adds the members x,y,z
+    union
+    {
+      struct
+      {
+        float L;
+        float a;
+        float b;
+      };
+      float data_lab[4];
+    };
+    PCL_MAKE_ALIGNED_OPERATOR_NEW
+  };
+
+  PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointXYZLAB& p);
+  /** \brief A point structure representing Euclidean xyz coordinates, and the CIELAB color.
+    * \ingroup common
+  */
+  struct PointXYZLAB : public _PointXYZLAB
+  {
+    inline PointXYZLAB (const _PointXYZLAB &p)
+    {
+      x = p.x; y = p.y; z = p.z; data[3] = 1.0f;
+      L = p.L; a = p.a; b = p.b;
+    }
+
+    inline PointXYZLAB()
+    {
+      x = y = z = 0.0f;
+      data[3] = 1.0f; // important for homogeneous coordinates
+      L = a = b = 0.0f;
+      data_lab[3] = 0.0f;
+    }
+
+    friend std::ostream& operator << (std::ostream& os, const PointXYZLAB& p);
+    PCL_MAKE_ALIGNED_OPERATOR_NEW
+  };
+
+
   struct EIGEN_ALIGN16 _PointXYZHSV
   {
     PCL_ADD_POINT4D;    // This adds the members x,y,z which can also be accessed using the point (which is float[4])
@@ -1164,14 +1251,6 @@ namespace pcl
 
     inline PointWithViewpoint (): PointWithViewpoint (0.f, 0.f, 0.f) {}
 
-    PCL_DEPRECATED(1, 12, "Use ctor accepting all position (x, y, z) data")
-    inline PointWithViewpoint (float _x, float _y = 0.f):
-      PointWithViewpoint (_x, _y, 0.f) {}
-
-    PCL_DEPRECATED(1, 12, "Use ctor accepting all viewpoint (vp_x, vp_y, vp_z) data")
-    inline PointWithViewpoint (float _x, float _y, float _z, float _vp_x, float _vp_y = 0.f):
-      PointWithViewpoint (_x, _y, _z, _vp_x, _vp_y, 0.f) {}
-
     inline PointWithViewpoint (float _x, float _y, float _z): PointWithViewpoint (_x, _y, _z, 0.f, 0.f, 0.f) {}
 
     inline PointWithViewpoint (float _x, float _y, float _z, float _vp_x, float _vp_y, float _vp_z)
@@ -1272,13 +1351,14 @@ namespace pcl
   struct PFHSignature125
   {
     float histogram[125] = {0.f};
-    static int descriptorSize () { return 125; }
+    static constexpr int descriptorSize () { return detail::traits::descriptorSize_v<PFHSignature125>; }
 
     inline PFHSignature125 () = default;
 
     friend std::ostream& operator << (std::ostream& os, const PFHSignature125& p);
   };
 
+
   PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PFHRGBSignature250& p);
   /** \brief A point structure representing the Point Feature Histogram with colors (PFHRGB).
     * \ingroup common
@@ -1286,7 +1366,7 @@ namespace pcl
   struct PFHRGBSignature250
   {
     float histogram[250] = {0.f};
-    static int descriptorSize () { return 250; }
+    static constexpr int descriptorSize () { return detail::traits::descriptorSize_v<PFHRGBSignature250>; }
 
     inline PFHRGBSignature250 () = default;
 
@@ -1373,7 +1453,7 @@ namespace pcl
   {
     float descriptor[1980] = {0.f};
     float rf[9] = {0.f};
-    static int descriptorSize () { return 1980; }
+    static constexpr int descriptorSize () { return detail::traits::descriptorSize_v<ShapeContext1980>; }
 
     inline ShapeContext1980 () = default;
 
@@ -1388,7 +1468,7 @@ namespace pcl
   {
     float descriptor[1960] = {0.f};
     float rf[9] = {0.f};
-    static int descriptorSize () { return 1960; }
+    static constexpr int descriptorSize () { return detail::traits::descriptorSize_v<UniqueShapeContext1960>; }
 
     inline UniqueShapeContext1960 () = default;
 
@@ -1403,7 +1483,7 @@ namespace pcl
   {
     float descriptor[352] = {0.f};
     float rf[9] = {0.f};
-    static int descriptorSize () { return 352; }
+    static constexpr int descriptorSize () { return detail::traits::descriptorSize_v<SHOT352>; }
 
     inline SHOT352 () = default;
 
@@ -1419,7 +1499,7 @@ namespace pcl
   {
     float descriptor[1344] = {0.f};
     float rf[9] = {0.f};
-    static int descriptorSize () { return 1344; }
+    static constexpr int descriptorSize () { return detail::traits::descriptorSize_v<SHOT1344>; }
 
     inline SHOT1344 () = default;
 
@@ -1484,7 +1564,7 @@ namespace pcl
   struct FPFHSignature33
   {
     float histogram[33] = {0.f};
-    static int descriptorSize () { return 33; }
+    static constexpr int descriptorSize () { return detail::traits::descriptorSize_v<FPFHSignature33>; }
 
     inline FPFHSignature33 () = default;
 
@@ -1498,7 +1578,7 @@ namespace pcl
   struct VFHSignature308
   {
     float histogram[308] = {0.f};
-    static int descriptorSize () { return 308; }
+    static constexpr int descriptorSize () { return detail::traits::descriptorSize_v<VFHSignature308>; }
 
     inline VFHSignature308 () = default;
 
@@ -1512,7 +1592,7 @@ namespace pcl
   struct GRSDSignature21
   {
     float histogram[21] = {0.f};
-    static int descriptorSize () { return 21; }
+    static constexpr int descriptorSize () { return detail::traits::descriptorSize_v<GRSDSignature21>; }
 
     inline GRSDSignature21 () = default;
 
@@ -1528,7 +1608,7 @@ namespace pcl
     float scale = 0.f;
     float orientation = 0.f;
     unsigned char descriptor[64] = {0};
-    static int descriptorSize () { return 64; }
+    static constexpr int descriptorSize () { return detail::traits::descriptorSize_v<BRISKSignature512>; }
 
     inline BRISKSignature512 () = default;
 
@@ -1544,7 +1624,7 @@ namespace pcl
   struct ESFSignature640
   {
     float histogram[640] = {0.f};
-    static int descriptorSize () { return 640; }
+    static constexpr int descriptorSize () { return detail::traits::descriptorSize_v<ESFSignature640>; }
 
     inline ESFSignature640 () = default;
 
@@ -1558,7 +1638,7 @@ namespace pcl
   struct GASDSignature512
   {
     float histogram[512] = {0.f};
-    static int descriptorSize() { return 512; }
+    static constexpr int descriptorSize() { return detail::traits::descriptorSize_v<GASDSignature512>; }
 
     inline GASDSignature512 () = default;
 
@@ -1572,7 +1652,7 @@ namespace pcl
   struct GASDSignature984
   {
     float histogram[984] = {0.f};
-    static int descriptorSize() { return 984; }
+    static constexpr int descriptorSize() { return detail::traits::descriptorSize_v<GASDSignature984>; }
 
     inline GASDSignature984 () = default;
 
@@ -1586,7 +1666,7 @@ namespace pcl
   struct GASDSignature7992
   {
     float histogram[7992] = {0.f};
-    static int descriptorSize() { return 7992; }
+    static constexpr int descriptorSize() { return detail::traits::descriptorSize_v<GASDSignature7992>; }
 
     inline GASDSignature7992 () = default;
 
@@ -1600,7 +1680,7 @@ namespace pcl
   struct GFPFHSignature16
   {
     float histogram[16] = {0.f};
-    static int descriptorSize () { return 16; }
+    static constexpr int descriptorSize () { return detail::traits::descriptorSize_v<GFPFHSignature16>; }
 
     inline GFPFHSignature16 () = default;
 
@@ -1615,7 +1695,7 @@ namespace pcl
   {
     float x = 0.f, y = 0.f, z = 0.f, roll = 0.f, pitch = 0.f, yaw = 0.f;
     float descriptor[36] = {0.f};
-    static int descriptorSize () { return 36; }
+    static constexpr int descriptorSize () { return detail::traits::descriptorSize_v<Narf36>; }
 
     inline Narf36 () = default;
 
@@ -1677,7 +1757,7 @@ namespace pcl
   struct Histogram
   {
     float histogram[N];
-    static int descriptorSize () { return N; }
+    static constexpr int descriptorSize () { return detail::traits::descriptorSize_v<Histogram<N>>; }
   };
 
   struct EIGEN_ALIGN16 _PointWithScale
@@ -1826,7 +1906,7 @@ namespace pcl
   operator << (std::ostream& os, const Histogram<N>& p)
   {
     // make constexpr
-    if (N > 0)
+    PCL_IF_CONSTEXPR(N > 0)
     {
         os << "(" << p.histogram[0];
         std::for_each(p.histogram + 1, std::end(p.histogram),
@@ -1890,6 +1970,16 @@ POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZRGBL,
 )
 POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZRGBL, pcl::_PointXYZRGBL)
 
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZLAB,
+    (float, x, x)
+    (float, y, y)
+    (float, z, z)
+    (float, L, L)
+    (float, a, a)
+    (float, b, b)
+)
+POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZLAB, pcl::_PointXYZLAB)
+
 POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZHSV,
     (float, x, x)
     (float, y, y)
diff --git a/common/include/pcl/pcl_base.h b/common/include/pcl/pcl_base.h
index 38f4b0f7..ebf294b3 100644
--- a/common/include/pcl/pcl_base.h
+++ b/common/include/pcl/pcl_base.h
@@ -46,9 +46,6 @@
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
 
-#include <Eigen/StdVector>
-#include <Eigen/Core>
-
 // Point Cloud message includes. Needed everywhere.
 #include <pcl/point_cloud.h>
 #include <pcl/PointIndices.h>
@@ -237,7 +234,7 @@ namespace pcl
       bool fake_indices_;
 
       /** \brief The size of each individual field. */
-      std::vector<int> field_sizes_;
+      std::vector<uindex_t> field_sizes_;
 
       /** \brief The x-y-z fields indices. */
       index_t x_idx_, y_idx_, z_idx_;
diff --git a/common/include/pcl/pcl_macros.h b/common/include/pcl/pcl_macros.h
index 053c3122..a86497da 100644
--- a/common/include/pcl/pcl_macros.h
+++ b/common/include/pcl/pcl_macros.h
@@ -73,6 +73,7 @@
 
 #include <pcl/pcl_config.h>
 
+#include <boost/preprocessor/arithmetic/add.hpp>
 #include <boost/preprocessor/comparison/equal.hpp>
 #include <boost/preprocessor/comparison/less.hpp>
 #include <boost/preprocessor/control/if.hpp>
@@ -123,15 +124,23 @@
 
 /**
  * \brief Tests for Minor < PCL_MINOR_VERSION
+ * \details When PCL VERSION is of format `34.12.99`, this macro behaves as if it is
+ * already `34.13.0`, and allows for smoother transition for maintainers
  */
 #define _PCL_COMPAT_MINOR_VERSION(Minor, IfPass, IfFail)                                \
-  BOOST_PP_IF(BOOST_PP_LESS(PCL_MINOR_VERSION, Minor), IfPass, IfFail)
+  BOOST_PP_IF(BOOST_PP_EQUAL(PCL_REVISION_VERSION, 99),                                 \
+    BOOST_PP_IF(BOOST_PP_LESS(BOOST_PP_ADD(PCL_MINOR_VERSION, 1), Minor), IfPass, IfFail),           \
+    BOOST_PP_IF(BOOST_PP_LESS(PCL_MINOR_VERSION, Minor), IfPass, IfFail))
 
 /**
  * \brief Tests for Major == PCL_MAJOR_VERSION
+ * \details When PCL VERSION is of format `34.99.12`, this macro behaves as if it is
+ * already `35.0.0`, and allows for smoother transition for maintainers
  */
 #define _PCL_COMPAT_MAJOR_VERSION(Major, IfPass, IfFail)                                \
-  BOOST_PP_IF(BOOST_PP_EQUAL(PCL_MAJOR_VERSION, Major), IfPass, IfFail)
+  BOOST_PP_IF(BOOST_PP_EQUAL(PCL_MINOR_VERSION, 99),                                    \
+    BOOST_PP_IF(BOOST_PP_EQUAL(BOOST_PP_ADD(PCL_MAJOR_VERSION, 1), Major), IfPass, IfFail),          \
+    BOOST_PP_IF(BOOST_PP_EQUAL(PCL_MAJOR_VERSION, Major), IfPass, IfFail))
 
 /**
  * \brief macro for compatibility across compilers and help remove old deprecated
@@ -210,20 +219,6 @@
 #endif
 #endif // defined _WIN32
 
-
-template<typename T>
-PCL_DEPRECATED(1, 12, "use std::isnan instead of pcl_isnan")
-bool pcl_isnan (T&& x) { return std::isnan (std::forward<T> (x)); }
-
-template<typename T>
-PCL_DEPRECATED(1, 12, "use std::isfinite instead of pcl_isfinite")
-bool pcl_isfinite (T&& x) { return std::isfinite (std::forward<T> (x)); }
-
-template<typename T>
-PCL_DEPRECATED(1, 12, "use std::isinf instead of pcl_isinf")
-bool pcl_isinf (T&& x) { return std::isinf (std::forward<T> (x)); }
-
-
 #ifndef DEG2RAD
 #define DEG2RAD(x) ((x)*0.017453293)
 #endif
@@ -381,44 +376,48 @@ pcl_round (float number)
   #endif
 #endif
 
+namespace pcl {
+
 inline void*
-aligned_malloc (std::size_t size)
+aligned_malloc(std::size_t size)
 {
-  void *ptr;
-#if   defined (MALLOC_ALIGNED)
-  ptr = std::malloc (size);
-#elif defined (HAVE_POSIX_MEMALIGN)
-  if (posix_memalign (&ptr, 16, size))
+  void* ptr;
+#if defined(MALLOC_ALIGNED)
+  ptr = std::malloc(size);
+#elif defined(HAVE_POSIX_MEMALIGN)
+  if (posix_memalign(&ptr, 16, size))
     ptr = 0;
-#elif defined (HAVE_MM_MALLOC)
-  ptr = _mm_malloc (size, 16);
-#elif defined (_MSC_VER)
-  ptr = _aligned_malloc (size, 16);
-#elif defined (ANDROID)
-  ptr = memalign (16, size);
+#elif defined(HAVE_MM_MALLOC)
+  ptr = _mm_malloc(size, 16);
+#elif defined(_MSC_VER)
+  ptr = _aligned_malloc(size, 16);
+#elif defined(ANDROID)
+  ptr = memalign(16, size);
 #else
-  #error aligned_malloc not supported on your platform
+#error aligned_malloc not supported on your platform
   ptr = 0;
 #endif
   return (ptr);
 }
 
 inline void
-aligned_free (void* ptr)
+aligned_free(void* ptr)
 {
-#if   defined (MALLOC_ALIGNED) || defined (HAVE_POSIX_MEMALIGN)
-  std::free (ptr);
-#elif defined (HAVE_MM_MALLOC)
-  _mm_free (ptr);
-#elif defined (_MSC_VER)
-  _aligned_free (ptr);
-#elif defined (ANDROID)
-  free (ptr);
+#if defined(MALLOC_ALIGNED) || defined(HAVE_POSIX_MEMALIGN)
+  std::free(ptr);
+#elif defined(HAVE_MM_MALLOC)
+  _mm_free(ptr);
+#elif defined(_MSC_VER)
+  _aligned_free(ptr);
+#elif defined(ANDROID)
+  free(ptr);
 #else
-  #error aligned_free not supported on your platform
+#error aligned_free not supported on your platform
 #endif
 }
 
+} // namespace pcl
+
 /**
  * \brief Macro to add a no-op or a fallthrough attribute based on compiler feature
  *
@@ -443,3 +442,9 @@ aligned_free (void* ptr)
 #else
   #define PCL_NODISCARD
 #endif
+
+#ifdef __cpp_if_constexpr
+  #define PCL_IF_CONSTEXPR(x) if constexpr(x)
+#else
+  #define PCL_IF_CONSTEXPR(x) if (x)
+#endif
diff --git a/common/include/pcl/point_cloud.h b/common/include/pcl/point_cloud.h
index 1f4969fb..9440c922 100644
--- a/common/include/pcl/point_cloud.h
+++ b/common/include/pcl/point_cloud.h
@@ -50,8 +50,8 @@
 #include <pcl/pcl_macros.h>
 #include <pcl/type_traits.h>
 #include <pcl/types.h>
+#include <pcl/console/print.h>  // for PCL_WARN
 
-#include <algorithm>
 #include <utility>
 #include <vector>
 
@@ -135,14 +135,6 @@ namespace pcl
       PCL_MAKE_ALIGNED_OPERATOR_NEW
   };
 
-  namespace detail
-  {
-    template <typename PointT>
-    PCL_DEPRECATED(1, 12, "use createMapping() instead")
-    shared_ptr<pcl::MsgFieldMap>&
-    getMapping (pcl::PointCloud<PointT>& p);
-  } // namespace detail
-
   /** \brief PointCloud represents the base class in PCL for storing collections of 3D points.
     *
     * The class is templated, which means you need to specify the type of data
@@ -186,20 +178,6 @@ namespace pcl
         */
       PointCloud () = default;
 
-      /** \brief Copy constructor.
-        * \param[in] pc the cloud to copy into this
-        * \todo Erase once mapping_ is removed.
-        */
-      // Ignore unknown pragma warning on MSVC (4996)
-      #pragma warning(push)
-      #pragma warning(disable: 4068)
-      // Ignore deprecated warning on clang compilers
-      #pragma clang diagnostic push
-      #pragma clang diagnostic ignored "-Wdeprecated-declarations"
-      PointCloud (const PointCloud<PointT> &pc) = default;
-      #pragma clang diagnostic pop
-      #pragma warning(pop)
-
       /** \brief Copy constructor from point cloud subset
         * \param[in] pc the cloud to copy into this
         * \param[in] indices the subset to copy
@@ -258,7 +236,7 @@ namespace pcl
 
         // libstdc++ (GCC) on calling reserve allocates new memory, copies and deallocates old vector
         // This causes a drastic performance hit. Prefer not to use reserve with libstdc++ (default on clang)
-        cloud1.points.insert (cloud1.points.end (), cloud2.points.begin (), cloud2.points.end ());
+        cloud1.insert (cloud1.end (), cloud2.begin (), cloud2.end ());
 
         cloud1.width    = cloud1.size ();
         cloud1.height   = 1;
@@ -337,9 +315,12 @@ namespace pcl
       /** \brief Return an Eigen MatrixXf (assumes float values) mapped to the specified dimensions of the PointCloud.
         * \note This method is for advanced users only! Use with care!
         *
-        * \attention Since 1.4.0, Eigen matrices are forced to Row Major to increase the efficiency of the algorithms in PCL
-        *   This means that the behavior of getMatrixXfMap changed, and is now correctly mapping 1-1 with a PointCloud structure,
-        *   that is: number of points in a cloud = rows in a matrix, number of point dimensions = columns in a matrix
+        * \attention Compile time flags used for Eigen might affect the dimension of the Eigen::Map returned. If Eigen
+        *   is using row major storage, the matrix shape would be (number of Points X elements in a Point) else
+        *   the matrix shape would be (elements in a Point X number of Points). Essentially,
+        *   * Major direction: number of points in cloud
+        *   * Minor direction: number of point dimensions
+        * By default, as of Eigen 3.3, Eigen uses Column major storage
         *
         * \param[in] dim the number of dimensions to consider for each point
         * \param[in] stride the number of values in each point (will be the number of values that separate two of the columns)
@@ -360,9 +341,12 @@ namespace pcl
       /** \brief Return an Eigen MatrixXf (assumes float values) mapped to the specified dimensions of the PointCloud.
         * \note This method is for advanced users only! Use with care!
         *
-        * \attention Since 1.4.0, Eigen matrices are forced to Row Major to increase the efficiency of the algorithms in PCL
-        *   This means that the behavior of getMatrixXfMap changed, and is now correctly mapping 1-1 with a PointCloud structure,
-        *   that is: number of points in a cloud = rows in a matrix, number of point dimensions = columns in a matrix
+        * \attention Compile time flags used for Eigen might affect the dimension of the Eigen::Map returned. If Eigen
+        *   is using row major storage, the matrix shape would be (number of Points X elements in a Point) else
+        *   the matrix shape would be (elements in a Point X number of Points). Essentially,
+        *   * Major direction: number of points in cloud
+        *   * Minor direction: number of point dimensions
+        * By default, as of Eigen 3.3, Eigen uses Column major storage
         *
         * \param[in] dim the number of dimensions to consider for each point
         * \param[in] stride the number of values in each point (will be the number of values that separate two of the columns)
@@ -457,11 +441,11 @@ namespace pcl
 
       //capacity
       inline std::size_t size () const { return points.size (); }
-      index_t max_size() const noexcept { return static_cast<index_t>(points.max_size()); }
+      inline index_t max_size() const noexcept { return static_cast<index_t>(points.max_size()); }
       inline void reserve (std::size_t n) { points.reserve (n); }
       inline bool empty () const { return points.empty (); }
-      PointT* data() noexcept { return points.data(); }
-      const PointT* data() const noexcept { return points.data(); }
+      inline PointT* data() noexcept { return points.data(); }
+      inline const PointT* data() const noexcept { return points.data(); }
 
       /**
        * \brief Resizes the container to contain `count` elements
@@ -484,6 +468,24 @@ namespace pcl
         }
       }
 
+      /**
+       * \brief Resizes the container to contain `new_width * new_height` elements
+       * \details
+       * * If the current size is greater than the requested size, the pointcloud
+       * is reduced to its first requested elements
+       * * If the current size is less then the requested size, additional
+       * default-inserted points are appended
+       * \param[in] new_width new width of the point cloud
+       * \param[in] new_height new height of the point cloud
+       */
+      inline void
+      resize(uindex_t new_width, uindex_t new_height)
+      {
+        points.resize(new_width * new_height);
+        width = new_width;
+        height = new_height;
+      }
+
       /**
        * \brief Resizes the container to contain count elements
        * \details
@@ -496,7 +498,7 @@ namespace pcl
        * \param[in] count new size of the point cloud
        * \param[in] value the value to initialize the new points with
        */
-      void
+      inline void
       resize(index_t count, const PointT& value)
       {
         points.resize(count, value);
@@ -506,6 +508,25 @@ namespace pcl
         }
       }
 
+      /**
+       * \brief Resizes the container to contain count elements
+       * \details
+       * * If the current size is greater then the requested size, the pointcloud
+       * is reduced to its first requested elements
+       * * If the current size is less then the requested size, additional
+       * default-inserted points are appended
+       * \param[in] new_width new width of the point cloud
+       * \param[in] new_height new height of the point cloud
+       * \param[in] value the value to initialize the new points with
+       */
+      inline void
+      resize(index_t new_width, index_t new_height, const PointT& value)
+      {
+        points.resize(new_width * new_height, value);
+        width = new_width;
+        height = new_height;
+      }
+
       //element access
       inline const PointT& operator[] (std::size_t n) const { return (points[n]); }
       inline PointT& operator[] (std::size_t n) { return (points[n]); }
@@ -520,8 +541,10 @@ namespace pcl
        * \brief Replaces the points with `count` copies of `value`
        * \note This breaks the organized structure of the cloud by setting the height to
        * 1!
+       * \param[in] count new size of the point cloud
+       * \param[in] value value each point of the cloud should have
        */
-      void
+      inline void
       assign(index_t count, const PointT& value)
       {
         points.assign(count, value);
@@ -529,6 +552,20 @@ namespace pcl
         height = 1;
       }
 
+      /**
+       * \brief Replaces the points with `new_width * new_height` copies of `value`
+       * \param[in] new_width new width of the point cloud
+       * \param[in] new_height new height of the point cloud
+       * \param[in] value value each point of the cloud should have
+       */
+      inline void
+      assign(index_t new_width, index_t new_height, const PointT& value)
+      {
+        points.assign(new_width * new_height, value);
+        width = new_width;
+        height = new_height;
+      }
+
       /**
        * \brief Replaces the points with copies of those in the range `[first, last)`
        * \details The behavior is undefined if either argument is an iterator into
@@ -536,28 +573,77 @@ namespace pcl
        * \note This breaks the organized structure of the cloud by setting the height to
        * 1!
        */
-      template <class InputIt>
-      void
-      assign(InputIt first, InputIt last)
+      template <class InputIterator>
+      inline void
+      assign(InputIterator first, InputIterator last)
       {
         points.assign(std::move(first), std::move(last));
         width = static_cast<std::uint32_t>(size());
         height = 1;
       }
 
+      /**
+       * \brief Replaces the points with copies of those in the range `[first, last)`
+       * \details The behavior is undefined if either argument is an iterator into
+       * `*this`
+       * \note This calculates the height based on size and width provided. This means
+       * the assignment happens even if the size is not perfectly divisible by width
+       * \param[in] first, last the range from which the points are copied
+       * \param[in] new_width new width of the point cloud
+       */
+      template <class InputIterator>
+      inline void
+      assign(InputIterator first, InputIterator last, index_t new_width)
+      {
+        points.assign(std::move(first), std::move(last));
+        width = new_width;
+        height = size() / width;
+        if (width * height != size()) {
+          PCL_WARN("Mismatch in assignment. Requested width (%zu) doesn't divide "
+                   "provided size (%zu) cleanly. Setting height to 1\n",
+                   static_cast<std::size_t>(width),
+                   static_cast<std::size_t>(size()));
+          width = size();
+          height = 1;
+        }
+      }
+
       /**
        * \brief Replaces the points with the elements from the initializer list `ilist`
        * \note This breaks the organized structure of the cloud by setting the height to
        * 1!
        */
       void
-      assign(std::initializer_list<PointT> ilist)
+      inline assign(std::initializer_list<PointT> ilist)
       {
         points.assign(std::move(ilist));
         width = static_cast<std::uint32_t>(size());
         height = 1;
       }
 
+      /**
+       * \brief Replaces the points with the elements from the initializer list `ilist`
+       * \note This calculates the height based on size and width provided. This means
+       * the assignment happens even if the size is not perfectly divisible by width
+       * \param[in] ilist initializer list from which the points are copied
+       * \param[in] new_width new width of the point cloud
+       */
+      void
+      inline assign(std::initializer_list<PointT> ilist, index_t new_width)
+      {
+        points.assign(std::move(ilist));
+        width = new_width;
+        height = size() / width;
+        if (width * height != size()) {
+          PCL_WARN("Mismatch in assignment. Requested width (%zu) doesn't divide "
+                   "provided size (%zu) cleanly. Setting height to 1\n",
+                   static_cast<std::size_t>(width),
+                   static_cast<std::size_t>(size()));
+          width = size();
+          height = 1;
+        }
+      }
+
       /** \brief Insert a new point in the cloud, at the end of the container.
         * \note This breaks the organized structure of the cloud by setting the height to 1!
         * \param[in] pt the point to insert
@@ -570,6 +656,16 @@ namespace pcl
         height = 1;
       }
 
+      /** \brief Insert a new point in the cloud, at the end of the container.
+        * \note This assumes the user would correct the width and height later on!
+        * \param[in] pt the point to insert
+        */
+      inline void
+      transient_push_back (const PointT& pt)
+      {
+        points.push_back (pt);
+      }
+
       /** \brief Emplace a new point in the cloud, at the end of the container.
         * \note This breaks the organized structure of the cloud by setting the height to 1!
         * \param[in] args the parameters to forward to the point to construct
@@ -584,6 +680,18 @@ namespace pcl
         return points.back();
       }
 
+      /** \brief Emplace a new point in the cloud, at the end of the container.
+        * \note This assumes the user would correct the width and height later on!
+        * \param[in] args the parameters to forward to the point to construct
+        * \return reference to the emplaced point
+        */
+      template <class... Args> inline reference
+      transient_emplace_back (Args&& ...args)
+      {
+        points.emplace_back (std::forward<Args> (args)...);
+        return points.back();
+      }
+
       /** \brief Insert a new point in the cloud, given an iterator.
         * \note This breaks the organized structure of the cloud by setting the height to 1!
         * \param[in] position where to insert the point
@@ -593,12 +701,25 @@ namespace pcl
       inline iterator
       insert (iterator position, const PointT& pt)
       {
-        iterator it = points.insert (position, pt);
+        iterator it = points.insert (std::move(position), pt);
         width = size ();
         height = 1;
         return (it);
       }
 
+      /** \brief Insert a new point in the cloud, given an iterator.
+        * \note This assumes the user would correct the width and height later on!
+        * \param[in] position where to insert the point
+        * \param[in] pt the point to insert
+        * \return returns the new position iterator
+        */
+      inline iterator
+      transient_insert (iterator position, const PointT& pt)
+      {
+        iterator it = points.insert (std::move(position), pt);
+        return (it);
+      }
+
       /** \brief Insert a new point in the cloud N times, given an iterator.
         * \note This breaks the organized structure of the cloud by setting the height to 1!
         * \param[in] position where to insert the point
@@ -608,11 +729,23 @@ namespace pcl
       inline void
       insert (iterator position, std::size_t n, const PointT& pt)
       {
-        points.insert (position, n, pt);
+        points.insert (std::move(position), n, pt);
         width = size ();
         height = 1;
       }
 
+      /** \brief Insert a new point in the cloud N times, given an iterator.
+        * \note This assumes the user would correct the width and height later on!
+        * \param[in] position where to insert the point
+        * \param[in] n the number of times to insert the point
+        * \param[in] pt the point to insert
+        */
+      inline void
+      transient_insert (iterator position, std::size_t n, const PointT& pt)
+      {
+        points.insert (std::move(position), n, pt);
+      }
+
       /** \brief Insert a new range of points in the cloud, at a certain position.
         * \note This breaks the organized structure of the cloud by setting the height to 1!
         * \param[in] position where to insert the data
@@ -622,11 +755,23 @@ namespace pcl
       template <class InputIterator> inline void
       insert (iterator position, InputIterator first, InputIterator last)
       {
-        points.insert (position, first, last);
+        points.insert (std::move(position), std::move(first), std::move(last));
         width = size ();
         height = 1;
       }
 
+      /** \brief Insert a new range of points in the cloud, at a certain position.
+        * \note This assumes the user would correct the width and height later on!
+        * \param[in] position where to insert the data
+        * \param[in] first where to start inserting the points from
+        * \param[in] last where to stop inserting the points from
+        */
+      template <class InputIterator> inline void
+      transient_insert (iterator position, InputIterator first, InputIterator last)
+      {
+        points.insert (std::move(position), std::move(first), std::move(last));
+      }
+
       /** \brief Emplace a new point in the cloud, given an iterator.
         * \note This breaks the organized structure of the cloud by setting the height to 1!
         * \param[in] position iterator before which the point will be emplaced
@@ -636,12 +781,25 @@ namespace pcl
       template <class... Args> inline iterator
       emplace (iterator position, Args&& ...args)
       {
-        iterator it = points.emplace (position, std::forward<Args> (args)...);
+        iterator it = points.emplace (std::move(position), std::forward<Args> (args)...);
         width = size ();
         height = 1;
         return (it);
       }
 
+      /** \brief Emplace a new point in the cloud, given an iterator.
+        * \note This assumes the user would correct the width and height later on!
+        * \param[in] position iterator before which the point will be emplaced
+        * \param[in] args the parameters to forward to the point to construct
+        * \return returns the new position iterator
+        */
+      template <class... Args> inline iterator
+      transient_emplace (iterator position, Args&& ...args)
+      {
+        iterator it = points.emplace (std::move(position), std::forward<Args> (args)...);
+        return (it);
+      }
+
       /** \brief Erase a point in the cloud.
         * \note This breaks the organized structure of the cloud by setting the height to 1!
         * \param[in] position what data point to erase
@@ -650,12 +808,24 @@ namespace pcl
       inline iterator
       erase (iterator position)
       {
-        iterator it = points.erase (position);
+        iterator it = points.erase (std::move(position));
         width = size ();
         height = 1;
         return (it);
       }
 
+      /** \brief Erase a point in the cloud.
+        * \note This assumes the user would correct the width and height later on!
+        * \param[in] position what data point to erase
+        * \return returns the new position iterator
+        */
+      inline iterator
+      transient_erase (iterator position)
+      {
+        iterator it = points.erase (std::move(position));
+        return (it);
+      }
+
       /** \brief Erase a set of points given by a (first, last) iterator pair
         * \note This breaks the organized structure of the cloud by setting the height to 1!
         * \param[in] first where to start erasing points from
@@ -665,12 +835,25 @@ namespace pcl
       inline iterator
       erase (iterator first, iterator last)
       {
-        iterator it = points.erase (first, last);
+        iterator it = points.erase (std::move(first), std::move(last));
         width = size ();
         height = 1;
         return (it);
       }
 
+      /** \brief Erase a set of points given by a (first, last) iterator pair
+        * \note This assumes the user would correct the width and height later on!
+        * \param[in] first where to start erasing points from
+        * \param[in] last where to stop erasing points from
+        * \return returns the new position iterator
+        */
+      inline iterator
+      transient_erase (iterator first, iterator last)
+      {
+        iterator it = points.erase (std::move(first), std::move(last));
+        return (it);
+      }
+
       /** \brief Swap a point cloud with another cloud.
         * \param[in,out] rhs point cloud to swap this with
         */
@@ -703,26 +886,9 @@ namespace pcl
       inline Ptr
       makeShared () const { return Ptr (new PointCloud<PointT> (*this)); }
 
-    protected:
-      /** \brief This is motivated by ROS integration. Users should not need to access mapping_.
-        * \todo Once mapping_ is removed, erase the explicitly defined copy constructor in PointCloud.
-        */
-      PCL_DEPRECATED(1, 12, "rewrite your code to avoid using this protected field") shared_ptr<MsgFieldMap> mapping_;
-
-      friend shared_ptr<MsgFieldMap>& detail::getMapping<PointT>(pcl::PointCloud<PointT> &p);
-
-    public:
       PCL_MAKE_ALIGNED_OPERATOR_NEW
   };
 
-  namespace detail
-  {
-    template <typename PointT> shared_ptr<pcl::MsgFieldMap>&
-    getMapping (pcl::PointCloud<PointT>& p)
-    {
-      return (p.mapping_);
-    }
-  } // namespace detail
 
   template <typename PointT> std::ostream&
   operator << (std::ostream& s, const pcl::PointCloud<PointT> &p)
diff --git a/common/include/pcl/point_struct_traits.h b/common/include/pcl/point_struct_traits.h
index 6640a0c2..1299d813 100644
--- a/common/include/pcl/point_struct_traits.h
+++ b/common/include/pcl/point_struct_traits.h
@@ -37,8 +37,6 @@
 
 #pragma once
 
-// https://bugreports.qt-project.org/browse/QTBUG-22829
-#ifndef Q_MOC_RUN
 #include <boost/mpl/assert.hpp>  // for BOOST_MPL_ASSERT_MSG
 #include <boost/mpl/identity.hpp>  // for boost::mpl::identity
 
@@ -46,7 +44,6 @@
 #include <boost/preprocessor/seq/enum.hpp>  // for BOOST_PP_SEQ_ENUM
 #include <boost/preprocessor/tuple/elem.hpp>  // for BOOST_PP_TUPLE_ELEM
 #include <boost/preprocessor/stringize.hpp> // for BOOST_PP_STRINGIZE
-#endif
 
 // This is required for the workaround at line 84
 #ifdef _MSC_VER
diff --git a/common/include/pcl/point_types.h b/common/include/pcl/point_types.h
index 1d47ba2e..b1f20e0b 100644
--- a/common/include/pcl/point_types.h
+++ b/common/include/pcl/point_types.h
@@ -111,6 +111,11 @@ namespace pcl
     */
   struct PointXYZRGBL;
 
+  /** \brief Members: float x, y, z, L, a, b
+    * \ingroup common
+    */
+  struct PointXYZLAB;
+
   /** \brief Members: float x, y, z, h, s, v
     * \ingroup common
     */
diff --git a/common/include/pcl/point_types_conversion.h b/common/include/pcl/point_types_conversion.h
index 28c8c48e..231b5f20 100644
--- a/common/include/pcl/point_types_conversion.h
+++ b/common/include/pcl/point_types_conversion.h
@@ -43,6 +43,8 @@
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 
+#include <pcl/common/colors.h> // for RGB2sRGB_LUT
+
 namespace pcl
 {
   // r,g,b, i values are from 0 to 255
@@ -134,6 +136,57 @@ namespace pcl
     if (out.h < 0.f) out.h += 360.f;
   }
 
+  /** \brief Convert a XYZRGB-based point type to a XYZLAB
+    * \param[in] in the input XYZRGB(XYZRGBA, XYZRGBL, etc.) point
+    * \param[out] out the output XYZLAB point
+    */
+  template <typename PointT, traits::HasColor<PointT> = true>
+  inline void
+  PointXYZRGBtoXYZLAB (const PointT& in,
+                       PointXYZLAB&  out)
+  {
+    out.x = in.x;
+    out.y = in.y;
+    out.z = in.z;
+    out.data[3] = 1.0; // important for homogeneous coordinates
+
+    // convert sRGB to CIELAB
+    // for sRGB   -> CIEXYZ see http://www.easyrgb.com/index.php?X=MATH&H=02#text2
+    // for CIEXYZ -> CIELAB see http://www.easyrgb.com/index.php?X=MATH&H=07#text7
+    // an overview at: https://www.comp.nus.edu.sg/~leowwk/papers/colordiff.pdf
+
+    const auto& sRGB_LUT = RGB2sRGB_LUT<double, 8>();
+
+    const double R = sRGB_LUT[in.r];
+    const double G = sRGB_LUT[in.g];
+    const double B = sRGB_LUT[in.b];
+
+    // linear sRGB -> CIEXYZ, D65 illuminant, observer at 2 degrees
+    const double X = R * 0.4124 + G * 0.3576 + B * 0.1805;
+    const double Y = R * 0.2126 + G * 0.7152 + B * 0.0722;
+    const double Z = R * 0.0193 + G * 0.1192 + B * 0.9505;
+
+    // normalize X, Y, Z with tristimulus values for Xn, Yn, Zn
+    float f[3] = {static_cast<float>(X), static_cast<float>(Y), static_cast<float>(Z)};
+    f[0] /= 0.95047;
+    f[1] /= 1;
+    f[2] /= 1.08883;
+
+    // CIEXYZ -> CIELAB
+    for (int i = 0; i < 3; ++i) {
+      if (f[i] > 0.008856) {
+        f[i] = std::pow(f[i], 1.0 / 3.0);
+      }
+      else {
+        f[i] = 7.787 * f[i] + 16.0 / 116.0;
+      }
+    }
+
+    out.L = 116.0f * f[1] - 16.0f;
+    out.a = 500.0f * (f[0] - f[1]);
+    out.b = 200.0f * (f[1] - f[2]);
+  }
+
   /** \brief Convert a XYZRGBA point type to a XYZHSV
     * \param[in] in the input XYZRGBA point
     * \param[out] out the output XYZHSV point
@@ -254,7 +307,7 @@ namespace pcl
     {
       Intensity p;
       PointRGBtoI (point, p);
-      out.points.push_back (p);
+      out.push_back (p);
     }
   }
 
@@ -272,7 +325,7 @@ namespace pcl
     {
       Intensity8u p;
       PointRGBtoI (point, p);
-      out.points.push_back (p);
+      out.push_back (p);
     }
   }
 
@@ -290,7 +343,7 @@ namespace pcl
     {
       Intensity32u p;
       PointRGBtoI (point, p);
-      out.points.push_back (p);
+      out.push_back (p);
     }
   }
 
@@ -308,7 +361,7 @@ namespace pcl
     {
       PointXYZHSV p;
       PointXYZRGBtoXYZHSV (point, p);
-      out.points.push_back (p);
+      out.push_back (p);
     }
   }
 
@@ -326,7 +379,7 @@ namespace pcl
     {
       PointXYZHSV p;
       PointXYZRGBAtoXYZHSV (point, p);
-      out.points.push_back (p);
+      out.push_back (p);
     }
   }
 
@@ -344,7 +397,7 @@ namespace pcl
     {
       PointXYZI p;
       PointXYZRGBtoXYZI (point, p);
-      out.points.push_back (p);
+      out.push_back (p);
     }
   }
 
@@ -386,7 +439,7 @@ namespace pcl
         pt.g = image.at (u, v).g;
         pt.b = image.at (u, v).b;
 
-        out.points.push_back (pt);
+        out.push_back (pt);
       }
     }
     out.width = width_;
diff --git a/common/include/pcl/range_image/bearing_angle_image.h b/common/include/pcl/range_image/bearing_angle_image.h
index 0a0a979b..ee6d0673 100644
--- a/common/include/pcl/range_image/bearing_angle_image.h
+++ b/common/include/pcl/range_image/bearing_angle_image.h
@@ -41,7 +41,6 @@
 
 #pragma once
 
-#include <cmath>
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
 
diff --git a/common/include/pcl/range_image/impl/range_image.hpp b/common/include/pcl/range_image/impl/range_image.hpp
index 19d37809..62cfe2ae 100644
--- a/common/include/pcl/range_image/impl/range_image.hpp
+++ b/common/include/pcl/range_image/impl/range_image.hpp
@@ -43,7 +43,7 @@
 #include <pcl/pcl_macros.h>
 #include <pcl/common/distances.h>
 #include <pcl/common/point_tests.h> // for pcl::isFinite
-
+#include <pcl/common/vector_average.h> // for VectorAverage3f
 
 namespace pcl
 {
diff --git a/common/include/pcl/range_image/range_image.h b/common/include/pcl/range_image/range_image.h
index b71e1af6..4405ed0d 100644
--- a/common/include/pcl/range_image/range_image.h
+++ b/common/include/pcl/range_image/range_image.h
@@ -41,10 +41,8 @@
 #include <pcl/point_cloud.h>
 #include <pcl/pcl_macros.h>
 #include <pcl/point_types.h>
-#include <pcl/PCLPointCloud2.h> // for PCLPointCloud2
 #include <pcl/common/angles.h> // for deg2rad
-#include <pcl/common/vector_average.h>
-#include <typeinfo>
+namespace pcl { struct PCLPointCloud2; }
 
 namespace pcl
 {
diff --git a/common/include/pcl/register_point_struct.h b/common/include/pcl/register_point_struct.h
index 27cd2d25..b3d36ff2 100644
--- a/common/include/pcl/register_point_struct.h
+++ b/common/include/pcl/register_point_struct.h
@@ -50,15 +50,12 @@
   #pragma warning (disable: 4244)
 #endif
 
-//https://bugreports.qt-project.org/browse/QTBUG-22829
-#ifndef Q_MOC_RUN
 #include <pcl/point_struct_traits.h> // for pcl::traits::POD, POINT_CLOUD_REGISTER_FIELD_(NAME, OFFSET, DATATYPE), POINT_CLOUD_REGISTER_POINT_FIELD_LIST
 #include <boost/mpl/assert.hpp>  // for BOOST_MPL_ASSERT_MSG
 #include <boost/preprocessor/seq/for_each.hpp>  // for BOOST_PP_SEQ_FOR_EACH
 #include <boost/preprocessor/seq/transform.hpp>  // for BOOST_PP_SEQ_TRANSFORM
 #include <boost/preprocessor/tuple/elem.hpp>  // for BOOST_PP_TUPLE_ELEM
 #include <boost/preprocessor/cat.hpp>  // for BOOST_PP_CAT
-#endif
 
 #include <cstdint>  // for std::uint32_t
 #include <type_traits>  // for std::enable_if_t, std::is_array, std::remove_const_t, std::remove_all_extents_t
diff --git a/common/include/pcl/types.h b/common/include/pcl/types.h
index 125f0d30..06cfb6a5 100644
--- a/common/include/pcl/types.h
+++ b/common/include/pcl/types.h
@@ -49,18 +49,10 @@
 
 #include <cstdint>
 
+#include <Eigen/Core>
+
 namespace pcl
 {
-  using uint8_t PCL_DEPRECATED(1, 12, "use std::uint8_t instead of pcl::uint8_t") = std::uint8_t;
-  using int8_t PCL_DEPRECATED(1, 12, "use std::int8_t instead of pcl::int8_t") = std::int8_t;
-  using uint16_t PCL_DEPRECATED(1, 12, "use std::uint16_t instead of pcl::uint16_t") = std::uint16_t;
-  using int16_t PCL_DEPRECATED(1, 12, "use std::uint16_t instead of pcl::int16_t") = std::int16_t;
-  using uint32_t PCL_DEPRECATED(1, 12, "use std::uint32_t instead of pcl::uint32_t") = std::uint32_t;
-  using int32_t PCL_DEPRECATED(1, 12, "use std::int32_t instead of pcl::int32_t") = std::int32_t;
-  using uint64_t PCL_DEPRECATED(1, 12, "use std::uint64_t instead of pcl::uint64_t") = std::uint64_t;
-  using int64_t PCL_DEPRECATED(1, 12, "use std::int64_t instead of pcl::int64_t") = std::int64_t;
-  using int_fast16_t PCL_DEPRECATED(1, 12, "use std::int_fast16_t instead of pcl::int_fast16_t") = std::int_fast16_t;
-
   namespace detail {
     /**
      * \brief int_type::type refers to an integral type that satisfies template parameters
@@ -139,5 +131,11 @@ namespace pcl
    * \brief Type used for indices in PCL
    */
   using Indices = IndicesAllocator<>;
+
+  /**
+   * \brief Type used for aligned vector of Eigen objects in PCL
+   */
+  template <typename T>
+  using AlignedVector = std::vector<T, Eigen::aligned_allocator<T>>;
 }  // namespace pcl
 
diff --git a/common/src/PCLPointCloud2.cpp b/common/src/PCLPointCloud2.cpp
index 462055c1..20d18d66 100644
--- a/common/src/PCLPointCloud2.cpp
+++ b/common/src/PCLPointCloud2.cpp
@@ -149,7 +149,7 @@ pcl::PCLPointCloud2::concatenate (pcl::PCLPointCloud2 &cloud1, const pcl::PCLPoi
   }
   const auto data1_size = cloud1.data.size ();
   cloud1.data.resize(data1_size + cloud2.data.size ());
-  for (std::size_t cp = 0; cp < size2; ++cp)
+  for (uindex_t cp = 0; cp < size2; ++cp)
   {
     for (const auto& field_data: valid_fields)
     {
diff --git a/common/src/bearing_angle_image.cpp b/common/src/bearing_angle_image.cpp
index 819d57c1..7c20c81e 100644
--- a/common/src/bearing_angle_image.cpp
+++ b/common/src/bearing_angle_image.cpp
@@ -40,7 +40,6 @@
   * \author: Qinghua Li (qinghua__li@163.com)
   */
 
-#include <pcl/common/eigen.h>
 #include <pcl/range_image/bearing_angle_image.h>
 
 namespace pcl
diff --git a/common/src/colors.cpp b/common/src/colors.cpp
index 0ecd5263..24987bce 100644
--- a/common/src/colors.cpp
+++ b/common/src/colors.cpp
@@ -38,8 +38,10 @@
 #include <pcl/point_types.h>
 #include <pcl/common/colors.h>
 
+#include <array>
+
 /// Glasbey lookup table
-static const unsigned char GLASBEY_LUT[] =
+static constexpr std::array<unsigned char, 256 * 3> GLASBEY_LUT =
 {
   77 , 175, 74 ,
   228, 26 , 28 ,
@@ -296,11 +298,11 @@ static const unsigned char GLASBEY_LUT[] =
   153, 61 , 225,
   237, 87 , 255,
   87 , 24 , 206,
-  117, 143, 207,
+  117, 143, 207
 };
 
 /// Viridis lookup table
-static const unsigned char VIRIDIS_LUT[] =
+static constexpr std::array<unsigned char, 256 * 3> VIRIDIS_LUT =
 {
   68 , 1  , 84 ,
   68 , 2  , 85 ,
@@ -557,27 +559,27 @@ static const unsigned char VIRIDIS_LUT[] =
   247, 230, 31 ,
   249, 231, 33 ,
   251, 231, 35 ,
-  254, 231, 36 ,
+  254, 231, 36
 };
 
 /// Number of colors in Glasbey lookup table
-static const std::size_t GLASBEY_LUT_SIZE = sizeof (GLASBEY_LUT) / (sizeof (GLASBEY_LUT[0]) * 3);
+static constexpr std::size_t GLASBEY_LUT_SIZE = GLASBEY_LUT.size() / 3;
 
 /// Number of colors in Viridis lookup table
-static const std::size_t VIRIDIS_LUT_SIZE = sizeof (VIRIDIS_LUT) / (sizeof (VIRIDIS_LUT[0]) * 3);
+static constexpr std::size_t VIRIDIS_LUT_SIZE = VIRIDIS_LUT.size() / 3;
 
-static const unsigned char* LUTS[] = { GLASBEY_LUT, VIRIDIS_LUT };
-static const std::size_t LUT_SIZES[] = { GLASBEY_LUT_SIZE, VIRIDIS_LUT_SIZE };
+static constexpr std::array<const std::array<unsigned char, 256 * 3>*, 2> LUTS = { &GLASBEY_LUT, &VIRIDIS_LUT };
+static constexpr std::array<std::size_t, 2> LUT_SIZES = { GLASBEY_LUT_SIZE, VIRIDIS_LUT_SIZE };
 
 template<typename pcl::ColorLUTName T> pcl::RGB
 pcl::ColorLUT<T>::at (std::size_t color_id)
 {
   assert (color_id < LUT_SIZES[T]);
   pcl::RGB color;
-  color.r = LUTS[T][color_id * 3 + 0];
-  color.g = LUTS[T][color_id * 3 + 1];
-  color.b = LUTS[T][color_id * 3 + 2];
-  return (color);
+  color.r = (*LUTS[T])[color_id * 3 + 0];
+  color.g = (*LUTS[T])[color_id * 3 + 1];
+  color.b = (*LUTS[T])[color_id * 3 + 2];
+  return color;
 }
 
 template<typename pcl::ColorLUTName T> std::size_t
@@ -589,7 +591,7 @@ pcl::ColorLUT<T>::size ()
 template<typename pcl::ColorLUTName T> const unsigned char*
 pcl::ColorLUT<T>::data ()
 {
-  return LUTS[T];
+  return LUTS[T]->data();
 }
 
 pcl::RGB
diff --git a/common/src/common.cpp b/common/src/common.cpp
index 5d5f0279..b3919cc2 100644
--- a/common/src/common.cpp
+++ b/common/src/common.cpp
@@ -39,6 +39,7 @@
 
 #include <limits>
 
+#include <pcl/PCLPointCloud2.h> // for PCLPointCloud2
 #include <pcl/common/common.h>
 #include <pcl/console/print.h>
 
@@ -59,7 +60,7 @@ pcl::getMinMax (const pcl::PCLPointCloud2 &cloud, int,
   }
   const auto field_idx = std::distance(cloud.fields.begin (), result);
 
-  for (unsigned int i = 0; i < cloud.fields[field_idx].count; ++i)
+  for (uindex_t i = 0; i < cloud.fields[field_idx].count; ++i)
   {
     float data;
     // TODO: replace float with the real data type
diff --git a/common/src/gaussian.cpp b/common/src/gaussian.cpp
index 2ba325d7..e5fef199 100644
--- a/common/src/gaussian.cpp
+++ b/common/src/gaussian.cpp
@@ -146,7 +146,7 @@ pcl::GaussianKernel::convolveRows (const pcl::PointCloud<float>& input,
     {
       output.width = input.width;
       output.height = input.height;
-      output.points.resize (input.height * input.width);
+      output.resize (input.height * input.width);
     }
     unaliased_input = &input;
   }
@@ -190,7 +190,7 @@ pcl::GaussianKernel::convolveCols (const pcl::PointCloud<float>& input,
     {
       output.width = input.width;
       output.height = input.height;
-      output.points.resize (input.height * input.width);
+      output.resize (input.height * input.width);
     }
     unaliased_input = &input;
   }
diff --git a/common/src/io.cpp b/common/src/io.cpp
index a5610d3a..76841aec 100644
--- a/common/src/io.cpp
+++ b/common/src/io.cpp
@@ -38,7 +38,6 @@
  *
  */
 
-#include <pcl/point_types.h>
 #include <pcl/common/io.h>
 
 //////////////////////////////////////////////////////////////////////////
@@ -182,7 +181,7 @@ pcl::concatenateFields (const pcl::PCLPointCloud2 &cloud1,
  
   // Iterate over each point and perform the appropriate memcpys
   int point_offset = 0;
-  for (std::size_t cp = 0; cp < cloud_out.width * cloud_out.height; ++cp)
+  for (uindex_t cp = 0; cp < cloud_out.width * cloud_out.height; ++cp)
   {
     memcpy (&cloud_out.data[point_offset], &cloud2.data[cp * cloud2.point_step], cloud2.point_step);
     int field_offset = cloud2.point_step;
@@ -214,115 +213,6 @@ pcl::concatenateFields (const pcl::PCLPointCloud2 &cloud1,
   return (true);
 }
 
-//////////////////////////////////////////////////////////////////////////
-bool
-pcl::concatenatePointCloud (const pcl::PCLPointCloud2 &cloud1,
-                            const pcl::PCLPointCloud2 &cloud2,
-                            pcl::PCLPointCloud2 &cloud_out)
-{
-  //if one input cloud has no points, but the other input does, just return the cloud with points
-  if (cloud1.width*cloud1.height == 0 && cloud2.width*cloud2.height > 0)
-  {
-    cloud_out = cloud2;
-    return (true);
-  }
-  if (cloud1.width*cloud1.height > 0 && cloud2.width*cloud2.height == 0)
-  {
-    cloud_out = cloud1;
-    return (true);
-  }
-
-  bool strip = false;
-  for (const auto &field : cloud1.fields)
-    if (field.name == "_")
-      strip = true;
-
-  for (const auto &field : cloud2.fields)
-    if (field.name == "_")
-      strip = true;
-
-  if (!strip && cloud1.fields.size () != cloud2.fields.size ())
-  {
-    PCL_ERROR ("[pcl::concatenatePointCloud] Number of fields in cloud1 (%u) != Number of fields in cloud2 (%u)\n", cloud1.fields.size (), cloud2.fields.size ());
-    return (false);
-  }
-  
-  // Copy cloud1 into cloud_out
-  cloud_out = cloud1;
-  std::size_t nrpts = cloud_out.data.size ();
-  // Height = 1 => no more organized
-  cloud_out.width    = cloud1.width * cloud1.height + cloud2.width * cloud2.height;
-  cloud_out.height   = 1;
-  if (!cloud1.is_dense || !cloud2.is_dense)
-    cloud_out.is_dense = false;
-  else
-    cloud_out.is_dense = true;
-
-  // We need to strip the extra padding fields
-  if (strip)
-  {
-    // Get the field sizes for the second cloud
-    std::vector<pcl::PCLPointField> fields2;
-    std::vector<int> fields2_sizes;
-    for (const auto &field : cloud2.fields)
-    {
-      if (field.name == "_")
-        continue;
-
-      fields2_sizes.push_back (field.count * 
-                               pcl::getFieldSize (field.datatype));
-      fields2.push_back (field);
-    }
-
-    cloud_out.data.resize (nrpts + (cloud2.width * cloud2.height) * cloud_out.point_step);
-
-    // Copy the second cloud
-    for (std::size_t cp = 0; cp < cloud2.width * cloud2.height; ++cp)
-    {
-      int i = 0;
-      for (std::size_t j = 0; j < fields2.size (); ++j)
-      {
-        if (cloud1.fields[i].name == "_")
-        {
-          ++i;
-          continue;
-        }
-
-        // We're fine with the special RGB vs RGBA use case
-        if ((cloud1.fields[i].name == "rgb" && fields2[j].name == "rgba") ||
-            (cloud1.fields[i].name == "rgba" && fields2[j].name == "rgb") ||
-            (cloud1.fields[i].name == fields2[j].name))
-        {
-          memcpy (reinterpret_cast<char*> (&cloud_out.data[nrpts + cp * cloud1.point_step + cloud1.fields[i].offset]), 
-                  reinterpret_cast<const char*> (&cloud2.data[cp * cloud2.point_step + cloud2.fields[j].offset]), 
-                  fields2_sizes[j]);
-          ++i;  // increment the field size i
-        }
-      }
-    }
-  }
-  else
-  {
-    for (std::size_t i = 0; i < cloud1.fields.size (); ++i)
-    {
-      // We're fine with the special RGB vs RGBA use case
-      if ((cloud1.fields[i].name == "rgb" && cloud2.fields[i].name == "rgba") ||
-          (cloud1.fields[i].name == "rgba" && cloud2.fields[i].name == "rgb"))
-        continue;
-      // Otherwise we need to make sure the names are the same
-      if (cloud1.fields[i].name != cloud2.fields[i].name)
-      {
-        PCL_ERROR ("[pcl::concatenatePointCloud] Name of field %d in cloud1, %s, does not match name in cloud2, %s\n", i, cloud1.fields[i].name.c_str (), cloud2.fields[i].name.c_str ());      
-        return (false);
-      }
-    }
-    
-    cloud_out.data.resize (nrpts + cloud2.data.size ());
-    memcpy (&cloud_out.data[nrpts], &cloud2.data[0], cloud2.data.size ());
-  }
-  return (true);
-}
-
 //////////////////////////////////////////////////////////////////////////
 bool
 pcl::getPointCloudAsEigen (const pcl::PCLPointCloud2 &in, Eigen::MatrixXf &out)
@@ -439,7 +329,7 @@ pcl::copyPointCloud (
 void 
 pcl::copyPointCloud (
     const pcl::PCLPointCloud2 &cloud_in,
-    const IndicesAllocator< Eigen::aligned_allocator<int> > &indices,
+    const IndicesAllocator< Eigen::aligned_allocator<index_t> > &indices,
     pcl::PCLPointCloud2 &cloud_out)
 {
   cloud_out.header       = cloud_in.header;
diff --git a/common/src/parse.cpp b/common/src/parse.cpp
index b6d67eeb..1d8d9d63 100644
--- a/common/src/parse.cpp
+++ b/common/src/parse.cpp
@@ -181,14 +181,20 @@ parse_argument (int argc, const char * const * argv, const char * str, T &val) n
 int
 pcl::console::parse_argument (int argc, const char * const * argv, const char * str, double &val)
 {
-  return parse_generic(strtod, argc, argv, str, val);
+  // added lambda wrapper for `strtod` to handle noexcept-type warning in GCC 7,
+  // refer to: https://stackoverflow.com/questions/46798456/handling-gccs-noexcept-type-warning
+  const auto strtod_l = [](const char *str, char **str_end){ return strtod(str, str_end); };
+  return parse_generic(strtod_l, argc, argv, str, val);
 }
 
 ////////////////////////////////////////////////////////////////////////////////
 int
 pcl::console::parse_argument (int argc, const char * const * argv, const char * str, float &val)
 {
-  return parse_generic(strtof, argc, argv, str, val);
+  // added lambda wrapper for `strtof` to handle noexcept-type warning in GCC 7,
+  // refer to: https://stackoverflow.com/questions/46798456/handling-gccs-noexcept-type-warning
+  const auto strtof_l = [](const char *str, char **str_end){ return strtof(str, str_end); };
+  return parse_generic(strtof_l, argc, argv, str, val);
 }
 
 ////////////////////////////////////////////////////////////////////////////////
diff --git a/common/src/point_types.cpp b/common/src/point_types.cpp
index 237a1ca8..e3ab796f 100644
--- a/common/src/point_types.cpp
+++ b/common/src/point_types.cpp
@@ -130,6 +130,13 @@ namespace pcl
     return (os);
   }
 
+  std::ostream& 
+  operator << (std::ostream& os, const PointXYZLAB& p)
+  {
+    os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.L << " , " <<  p.a << " , " << p.b << ")";
+    return (os);
+  }
+
   std::ostream& 
   operator << (std::ostream& os, const PointXYZHSV& p)
   {
@@ -190,8 +197,8 @@ namespace pcl
   operator << (std::ostream& os, const PointXYZRGBNormal& p)
   {
     os << "(" << p.x << "," << p.y << "," << p.z << " - "<< p.rgb << " - " << p.normal[0] << "," << p.normal[1] << "," << p.normal[2] << " - "
-      << static_cast<int>(p.r) << ", "
-      << static_cast<int>(p.g) << ", "
+      << static_cast<int>(p.r) << ","
+      << static_cast<int>(p.g) << ","
       << static_cast<int>(p.b) << " - "
       << p.curvature << ")";
     return (os);
diff --git a/common/src/print.cpp b/common/src/print.cpp
index 93b9e8fc..5e99d8fb 100644
--- a/common/src/print.cpp
+++ b/common/src/print.cpp
@@ -36,6 +36,7 @@
  */
 #include <pcl/console/print.h>
 #include <algorithm>
+#include <cstdarg> // for va_list, va_start, va_end
 #include <cstdlib>
 #include <cctype> // for toupper
 #include <map>
diff --git a/common/src/range_image.cpp b/common/src/range_image.cpp
index 7e4377f9..48b0a6a1 100644
--- a/common/src/range_image.cpp
+++ b/common/src/range_image.cpp
@@ -34,10 +34,9 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <iostream>
 #include <cmath>
+#include <pcl/PCLPointCloud2.h> // for PCLPointCloud2
 #include <pcl/common/time.h> // for MEASURE_FUNCTION_TIME
-#include <pcl/common/eigen.h>
 #include <pcl/range_image/range_image.h>
 
 namespace pcl 
@@ -351,8 +350,8 @@ RangeImage::getHalfImage (RangeImage& half_image) const
   half_image.width  = width/2;
   half_image.height = height/2;
   half_image.is_dense = is_dense;
-  half_image.points.clear ();
-  half_image.points.resize (half_image.width*half_image.height);
+  half_image.clear ();
+  half_image.resize (half_image.width*half_image.height);
   
   int src_start_x = 2*half_image.image_offset_x_ - image_offset_x_,
       src_start_y = 2*half_image.image_offset_y_ - image_offset_y_;
@@ -395,8 +394,8 @@ RangeImage::getSubImage (int sub_image_image_offset_x, int sub_image_image_offse
   sub_image.width = sub_image_width;
   sub_image.height = sub_image_height;
   sub_image.is_dense = is_dense;
-  sub_image.points.clear ();
-  sub_image.points.resize (sub_image.width*sub_image.height);
+  sub_image.clear ();
+  sub_image.resize (sub_image.width*sub_image.height);
   
   int src_start_x = combine_pixels*sub_image.image_offset_x_ - image_offset_x_,
       src_start_y = combine_pixels*sub_image.image_offset_y_ - image_offset_y_;
@@ -834,7 +833,7 @@ RangeImage::extractFarRanges (const pcl::PCLPointCloud2& point_cloud_data,
       vp_z_offset = point_cloud_data.fields[vp_z_idx].offset,
       distance_offset = point_cloud_data.fields[distance_idx].offset;
   
-  for (std::size_t point_idx = 0; point_idx < point_cloud_data.width*point_cloud_data.height; ++point_idx)
+  for (uindex_t point_idx = 0; point_idx < point_cloud_data.width*point_cloud_data.height; ++point_idx)
   {
     float x = *reinterpret_cast<const float*> (data+x_offset), 
           y = *reinterpret_cast<const float*> (data+y_offset), 
@@ -850,7 +849,7 @@ RangeImage::extractFarRanges (const pcl::PCLPointCloud2& point_cloud_data,
       PointWithViewpoint point;
       point.x=distance; point.y=y; point.z=z;
       point.vp_x=vp_x; point.vp_y=vp_y; point.vp_z=vp_z;
-      far_ranges.points.push_back (point);
+      far_ranges.push_back (point);
     }
   }
   far_ranges.width= far_ranges.size ();  far_ranges.height = 1;
diff --git a/cuda/CMakeLists.txt b/cuda/CMakeLists.txt
index 72cbd38b..c327f95e 100644
--- a/cuda/CMakeLists.txt
+++ b/cuda/CMakeLists.txt
@@ -11,7 +11,9 @@ endif()
 if(CMAKE_COMPILER_IS_GNUCXX)
   string(REPLACE "-Wold-style-cast" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
   string(REPLACE "-Wno-invalid-offsetof" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-conversion -Wno-unused-parameter -Wno-unused-variable -Wno-unused-function -Wno-unused-but-set-variable")
+  string(APPEND CMAKE_CXX_FLAGS " -Wno-conversion -Wno-unused-parameter -Wno-unused-variable -Wno-unused-function -Wno-unused-but-set-variable")
+  # allow deprecation warnings in Eigen(3.3.7)/Core, see here: https://gitlab.kitware.com/vtk/vtk/-/issues/17661
+  string(APPEND CMAKE_CXX_FLAGS " -Wno-error=cpp")
 endif()
 
 collect_subproject_directory_names("${CMAKE_CURRENT_SOURCE_DIR}" "CMakeLists.txt" PCL_CUDA_MODULES_NAMES PCL_CUDA_MODULES_DIRS)
diff --git a/cuda/apps/src/kinect_normals_cuda.cpp b/cuda/apps/src/kinect_normals_cuda.cpp
index 07c0011f..53adab03 100644
--- a/cuda/apps/src/kinect_normals_cuda.cpp
+++ b/cuda/apps/src/kinect_normals_cuda.cpp
@@ -102,7 +102,7 @@ class NormalEstimation
     {
       pcl::PointCloud<pcl::PointXYZRGB>::Ptr output (new pcl::PointCloud<pcl::PointXYZRGB>);
       PointCloudAOS<Host> data_host;
-      data_host.points.resize (cloud->points.size());
+      data_host.resize (cloud->points.size());
       for (std::size_t i = 0; i < cloud->points.size (); ++i)
       {
         PointXYZRGB pt;
diff --git a/cuda/apps/src/kinect_ransac.cpp b/cuda/apps/src/kinect_ransac.cpp
index b7835279..c4eeae5f 100644
--- a/cuda/apps/src/kinect_ransac.cpp
+++ b/cuda/apps/src/kinect_ransac.cpp
@@ -68,7 +68,7 @@ class SimpleKinectTool
       pcl::ScopeTime ttt ("all");
       pcl::PointCloud<pcl::PointXYZRGB>::Ptr output (new pcl::PointCloud<pcl::PointXYZRGB>);
       PointCloudAOS<Host> data_host;
-      data_host.points.resize (cloud->points.size());
+      data_host.resize (cloud->points.size());
       for (std::size_t i = 0; i < cloud->points.size (); ++i)
       {
         PointXYZRGB pt;
diff --git a/cuda/apps/src/kinect_segmentation_cuda.cpp b/cuda/apps/src/kinect_segmentation_cuda.cpp
index 3e2c6f5b..c6b6510b 100644
--- a/cuda/apps/src/kinect_segmentation_cuda.cpp
+++ b/cuda/apps/src/kinect_segmentation_cuda.cpp
@@ -149,7 +149,7 @@ class Segmentation
     {
       pcl::PointCloud<pcl::PointXYZRGB>::Ptr output (new pcl::PointCloud<pcl::PointXYZRGB>);
       PointCloudAOS<Host> data_host;
-      data_host.points.resize (cloud->points.size());
+      data_host.resize (cloud->points.size());
       for (std::size_t i = 0; i < cloud->points.size (); ++i)
       {
         PointXYZRGB pt;
diff --git a/cuda/apps/src/kinect_segmentation_planes_cuda.cpp b/cuda/apps/src/kinect_segmentation_planes_cuda.cpp
index a1db2afa..cb7591b6 100644
--- a/cuda/apps/src/kinect_segmentation_planes_cuda.cpp
+++ b/cuda/apps/src/kinect_segmentation_planes_cuda.cpp
@@ -123,7 +123,7 @@ class Segmentation
     {
       pcl::PointCloud<pcl::PointXYZRGB>::Ptr output (new pcl::PointCloud<pcl::PointXYZRGB>);
       PointCloudAOS<Host> data_host;
-      data_host.points.resize (cloud->points.size());
+      data_host.resize (cloud->points.size());
       for (std::size_t i = 0; i < cloud->points.size (); ++i)
       {
         PointXYZRGB pt;
diff --git a/cuda/common/include/pcl/cuda/point_cloud.h b/cuda/common/include/pcl/cuda/point_cloud.h
index c152e0f7..4435f61f 100644
--- a/cuda/common/include/pcl/cuda/point_cloud.h
+++ b/cuda/common/include/pcl/cuda/point_cloud.h
@@ -151,8 +151,8 @@ namespace pcl
         {
           points   = rhs.points;
           // TODO: Test speed on operator () = vs resize+copy
-          //points.resize (rhs.points.size ());
-          //thrust::copy (rhs.points.begin (), rhs.points.end (), points.begin ());
+          //points.resize (rhs.size ());
+          //thrust::copy (rhs.begin (), rhs.end (), points.begin ());
           width    = rhs.width;
           height   = rhs.height;
           is_dense = rhs.is_dense;
diff --git a/cuda/filters/include/pcl/cuda/filters/passthrough.h b/cuda/filters/include/pcl/cuda/filters/passthrough.h
index b5405b76..51d1a6d4 100644
--- a/cuda/filters/include/pcl/cuda/filters/passthrough.h
+++ b/cuda/filters/include/pcl/cuda/filters/passthrough.h
@@ -127,14 +127,14 @@ namespace pcl_cuda
       applyFilter (PointCloud &output)
       {
         // Allocate enough space
-        output.points.resize (input_->points.size ());
+        output.resize (input_->points.size ());
         // Copy data
-        Device<PointXYZRGB>::type::iterator nr_points = thrust::copy_if (input_->points.begin (), input_->points.end (), output.points.begin (), isFiniteAOS ());
-        //Device<float3>::type::iterator nr_points = thrust::copy_if (input_->points.begin (), input_->points.end (), output.points.begin (), isFiniteAOS ());
-        output.points.resize (nr_points - output.points.begin ());
+        Device<PointXYZRGB>::type::iterator nr_points = thrust::copy_if (input_->points.begin (), input_->points.end (), output.begin (), isFiniteAOS ());
+        //Device<float3>::type::iterator nr_points = thrust::copy_if (input_->points.begin (), input_->points.end (), output.begin (), isFiniteAOS ());
+        output.resize (nr_points - output.begin ());
 
         //std::cerr << "[applyFilterAOS]: ";
-        //std::cerr << input_->points.size () << " " << output.points.size () << std::endl;
+        //std::cerr << input_->points.size () << " " << output.size () << std::endl;
       }
   };
  
diff --git a/cuda/filters/include/pcl/cuda/filters/voxel_grid.h b/cuda/filters/include/pcl/cuda/filters/voxel_grid.h
index 219a1168..39c207ba 100644
--- a/cuda/filters/include/pcl/cuda/filters/voxel_grid.h
+++ b/cuda/filters/include/pcl/cuda/filters/voxel_grid.h
@@ -92,13 +92,13 @@ namespace pcl_cuda
       applyFilter (PointCloud &output)
       {
         // Allocate enough space
-        output.points.resize (input_->points.size ());
+        output.resize (input_->points.size ());
         // Copy data
-        Device<PointXYZRGB>::type::iterator nr_points = thrust::copy_if (input_->points.begin (), input_->points.end (), output.points.begin (), isFiniteAOS ());
-        output.points.resize (nr_points - output.points.begin ());
+        Device<PointXYZRGB>::type::iterator nr_points = thrust::copy_if (input_->points.begin (), input_->points.end (), output.begin (), isFiniteAOS ());
+        output.resize (nr_points - output.begin ());
 
         //std::cerr << "[applyFilterAOS]: ";
-        //std::cerr << input_->points.size () << " " << output.points.size () << std::endl;
+        //std::cerr << input_->points.size () << " " << output.size () << std::endl;
       }
   };
  
diff --git a/cuda/io/CMakeLists.txt b/cuda/io/CMakeLists.txt
index b792231b..f60a472e 100644
--- a/cuda/io/CMakeLists.txt
+++ b/cuda/io/CMakeLists.txt
@@ -2,16 +2,17 @@ set(SUBSYS_NAME cuda_io)
 set(SUBSYS_PATH cuda/io)
 set(SUBSYS_DESC "Point cloud CUDA IO library")
 set(SUBSYS_DEPS cuda_common io common)
+set(SUBSYS_EXT_DEPS openni)
 
 # ---[ Point Cloud Library - pcl/cuda/io
 
 set(build TRUE)
 PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" OFF)
 mark_as_advanced("BUILD_${SUBSYS_NAME}")
-PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS})
+PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} EXT_DEPS ${SUBSYS_EXT_DEPS})
 PCL_SET_SUBSYS_INCLUDE_DIR("${SUBSYS_NAME}" "${SUBSYS_PATH}")
 
-if(NOT (build AND HAVE_OPENNI))
+if(NOT build)
   return()
 endif()
 
@@ -37,9 +38,7 @@ set(LIB_NAME "pcl_${SUBSYS_NAME}")
 include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include")
 PCL_CUDA_ADD_LIBRARY(${LIB_NAME} COMPONENT ${SUBSYS_NAME} SOURCES ${srcs} ${incs})
 
-set(EXT_DEPS "")
-#set(EXT_DEPS CUDA)
-PCL_MAKE_PKGCONFIG(${LIB_NAME} COMPONENT ${SUBSYS_NAME} DESC ${SUBSYS_DESC} PCL_DEPS ${SUBSYS_DEPS} EXT_DEPS ${EXT_DEPS})
+PCL_MAKE_PKGCONFIG(${LIB_NAME} COMPONENT ${SUBSYS_NAME} DESC ${SUBSYS_DESC} PCL_DEPS ${SUBSYS_DEPS} EXT_DEPS ${SUBSYS_EXT_DEPS})
 
 # Install include files
 PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_PATH}" ${incs})
diff --git a/cuda/io/src/cloud_from_pcl.cu b/cuda/io/src/cloud_from_pcl.cu
index 2ef517c2..c4c8fdc4 100644
--- a/cuda/io/src/cloud_from_pcl.cu
+++ b/cuda/io/src/cloud_from_pcl.cu
@@ -48,8 +48,8 @@ namespace cuda
 void
 fromPCL (const pcl::PointCloud<pcl::PointXYZRGB> &input, PointCloudAOS<Host> &output)
 {
-//  output.points.resize (input.points.size());
-//  for (std::size_t i = 0; i < input.points.size (); ++i)
+//  output.resize (input.size());
+//  for (std::size_t i = 0; i < input.size (); ++i)
 //  {
 //    output[i].x = input[i].x;
 //    output[i].y = input[i].y;
@@ -57,7 +57,7 @@ fromPCL (const pcl::PointCloud<pcl::PointXYZRGB> &input, PointCloudAOS<Host> &ou
 //    // Pack RGB into a float
 //    output[i].rgb = *(float*)(&input[i].rgb);
 //  }
-//  thrust::copy (output.points.begin(), output.points.end (), input.points.begin());
+//  thrust::copy (output.begin(), output.end (), input.begin());
 //  output.width    = input.width;
 //  output.height   = input.height;
 //  output.is_dense = input.is_dense;
diff --git a/cuda/io/src/cloud_to_pcl.cpp b/cuda/io/src/cloud_to_pcl.cpp
index 71f4536a..7d555fab 100644
--- a/cuda/io/src/cloud_to_pcl.cpp
+++ b/cuda/io/src/cloud_to_pcl.cpp
@@ -50,7 +50,7 @@ toPCL (const PointCloudAOS<Host> &input,
                  const thrust::host_vector<float4> &normals,
                  pcl::PointCloud<pcl::PointXYZRGBNormal> &output)
 {
-  output.points.resize (input.points.size ());
+  output.resize (input.points.size ());
   for (std::size_t i = 0; i < input.points.size (); ++i)
   {
     output[i].x = input.points[i].x;
@@ -88,7 +88,7 @@ void
 toPCL (const PointCloudAOS<Host> &input, 
                  pcl::PointCloud<pcl::PointXYZRGB> &output)
 {
-  output.points.resize (input.points.size ());
+  output.resize (input.points.size ());
   for (std::size_t i = 0; i < input.points.size (); ++i)
   {
     output[i].x = input.points[i].x;
@@ -102,7 +102,7 @@ toPCL (const PointCloudAOS<Host> &input,
   output.height   = input.height;
   output.is_dense = input.is_dense;
 
-/*  for (std::size_t i = 0; i < output.points.size (); ++i)
+/*  for (std::size_t i = 0; i < output.size (); ++i)
   std::cerr << 
     output[i].x << " " <<
     output[i].y << " " <<
diff --git a/cuda/io/src/debayering.cu b/cuda/io/src/debayering.cu
index 9c0e55d9..b9f23e95 100644
--- a/cuda/io/src/debayering.cu
+++ b/cuda/io/src/debayering.cu
@@ -114,8 +114,8 @@ namespace pcl
     OpenNIRGB DebayerBilinear<Storage>::operator () (int index) const
     {
     	// get position
-    	int xIdx = index % width;
-    	int yIdx = index / width;
+    	unsigned int xIdx = index % width;
+    	unsigned int yIdx = index / width;
     	
     	OpenNIRGB result;
       
diff --git a/cuda/nn/organized_neighbor_search.hpp b/cuda/nn/organized_neighbor_search.hpp
index 22bba967..a586adf9 100644
--- a/cuda/nn/organized_neighbor_search.hpp
+++ b/cuda/nn/organized_neighbor_search.hpp
@@ -43,7 +43,7 @@ namespace pcl
     {
       if (input_->height == 1)
       {
-        PCL_ERROR ("[pcl::%s::radiusSearch] Input dataset is not organized!", getName ().c_str ());
+        PCL_ERROR ("[pcl::%s::radiusSearch] Input dataset is not organized!\n", getName ().c_str ());
         return 0;
       }
 
@@ -171,7 +171,7 @@ namespace pcl
 
       if (input_->height == 1)
       {
-        PCL_ERROR ("[pcl::%s::nearestKSearch] Input dataset is not organized!", getName ().c_str ());
+        PCL_ERROR ("[pcl::%s::nearestKSearch] Input dataset is not organized!\n", getName ().c_str ());
         return 0;
       }
 
diff --git a/cuda/sample_consensus/src/sac_model.cu b/cuda/sample_consensus/src/sac_model.cu
index adeee909..461e66f5 100644
--- a/cuda/sample_consensus/src/sac_model.cu
+++ b/cuda/sample_consensus/src/sac_model.cu
@@ -57,7 +57,7 @@ namespace pcl
 
     template <template <typename> class Storage>
     __inline__ __host__
-    void create_scatter_stencil (int new_w, int new_h, int skip, int width, int height, typename Storage<int>::type &stencil)
+    void create_scatter_stencil (unsigned int new_w, unsigned int new_h, int skip, int width, int height, typename Storage<int>::type &stencil)
     { 
       for (unsigned int i = 0; i < new_w * new_h; ++i)
       { 
diff --git a/doc/advanced/content/c_cache.rst b/doc/advanced/content/c_cache.rst
index 21da47ba..8d2826cc 100644
--- a/doc/advanced/content/c_cache.rst
+++ b/doc/advanced/content/c_cache.rst
@@ -35,14 +35,14 @@ Install ``colorgcc`` on an Ubuntu system with ::
 
 To enable colorgcc, perform the following steps:
 
-.. code-block:: cmake
+.. code-block:: shell
 
   cp /etc/colorgcc/colorgccrc $HOME/.colorgccrc
 
 
 * edit the $HOME/.colorgccrc file, search for the following lines:
 
-.. code-block:: cmake
+.. code-block:: text
 
     g++: /usr/bin/g++
     gcc: /usr/bin/gcc
@@ -54,7 +54,7 @@ To enable colorgcc, perform the following steps:
     
 and replace them with:
 
-.. code-block:: cmake
+.. code-block:: text
 
     g++: ccache /usr/bin/g++
     gcc: ccache /usr/bin/gcc
@@ -66,7 +66,7 @@ and replace them with:
 
 * create a $HOME/bin or $HOME/sbin directory, and create the following softlinks in it
 
-.. code-block:: cmake
+.. code-block:: shell
 
     ln -s /usr/bin/colorgcc c++
     ln -s /usr/bin/colorgcc cc
diff --git a/doc/advanced/content/index.rst b/doc/advanced/content/index.rst
index cb2c4beb..93d4a1a1 100644
--- a/doc/advanced/content/index.rst
+++ b/doc/advanced/content/index.rst
@@ -97,7 +97,7 @@ development that everyone should follow:
   An in-depth discussion about the PCL 2.x API can be found here.
 
 Committing changes to the git master
------------------------------------
+------------------------------------
 In order to oversee the commit messages more easier and that the changelist looks homogenous please keep the following format:
 
 "* <fixed|bugfix|changed|new> X in @<classname>@ (#<bug number>)" 
diff --git a/doc/advanced/content/pcl_style_guide.rst b/doc/advanced/content/pcl_style_guide.rst
index aafbe486..34bbe40d 100644
--- a/doc/advanced/content/pcl_style_guide.rst
+++ b/doc/advanced/content/pcl_style_guide.rst
@@ -45,7 +45,7 @@ same directory, in any other case the include statement is made with
  .. code-block:: cpp
 
   #include <pcl/module_name/file_name.h>
-  #incluce <pcl/module_name/impl/file_name.hpp>
+  #include <pcl/module_name/impl/file_name.hpp>
 
 1.4. Defines & Macros
 ^^^^^^^^^^^^^^^^^^^^^
diff --git a/doc/doxygen/pcl.doxy b/doc/doxygen/pcl.doxy
index 29a0f91c..94a39d69 100644
--- a/doc/doxygen/pcl.doxy
+++ b/doc/doxygen/pcl.doxy
@@ -44,7 +44,8 @@ Please visit http://www.pointclouds.org for more information.
   booktitle = {{IEEE International Conference on Robotics and Automation (ICRA)}},
   month     = {May 9-13},
   year      = {2011},
-  address   = {Shanghai, China}
+  address   = {Shanghai, China},
+  publisher = {IEEE}
 }
 </pre>
 <a href="http://www.pointclouds.org/assets/pdf/pcl_icra2011.pdf">Download PDF here</a>.
diff --git a/doc/tutorials/content/adding_custom_ptype.rst b/doc/tutorials/content/adding_custom_ptype.rst
index d123d953..7acb5cdc 100644
--- a/doc/tutorials/content/adding_custom_ptype.rst
+++ b/doc/tutorials/content/adding_custom_ptype.rst
@@ -849,7 +849,7 @@ data (SSE padded), together with a test float.
    main (int argc, char** argv)
    {
      pcl::PointCloud<MyPointType> cloud;
-     cloud.points.resize (2);
+     cloud.resize (2);
      cloud.width = 2;
      cloud.height = 1;
 
diff --git a/doc/tutorials/content/bspline_fitting.rst b/doc/tutorials/content/bspline_fitting.rst
index 518261ed..968fd607 100644
--- a/doc/tutorials/content/bspline_fitting.rst
+++ b/doc/tutorials/content/bspline_fitting.rst
@@ -23,7 +23,7 @@ In this video, the algorithm is applied to the frontal scan of the stanford bunn
 
 .. raw:: html
 
-  <iframe title="Trimmed B-spline surface fitting" width="480" height="390" src="http://www.youtube.com/embed/trH2kWELvyw?rel=0" frameborder="0" allowfullscreen></iframe>
+  <iframe title="Trimmed B-spline surface fitting" width="480" height="390" src="https://www.youtube.com/embed/trH2kWELvyw?rel=0" frameborder="0" allowfullscreen></iframe>
 
 
 Theoretical background
@@ -59,7 +59,7 @@ You can find the input file at *pcl/test/bunny.pcd*.
 .. literalinclude:: sources/bspline_fitting/bspline_fitting.cpp
    :language: cpp
    :linenos:
-   :lines: 1-220
+   :lines: 1-219
 
 
 The explanation
diff --git a/doc/tutorials/content/compiling_pcl_posix.rst b/doc/tutorials/content/compiling_pcl_posix.rst
index f9c735b9..3265435e 100644
--- a/doc/tutorials/content/compiling_pcl_posix.rst
+++ b/doc/tutorials/content/compiling_pcl_posix.rst
@@ -18,9 +18,9 @@ Stable
 
 For systems for which we do not offer precompiled binaries, you need to compile Point Cloud Library (PCL) from source. Here are the steps that you need to take:
 Go to `Github <https://github.com/PointCloudLibrary/pcl/releases>`_ and download the version number of your choice.
-Uncompress the tar-bzip archive, e.g. (replace 1.7.2 with the correct version number)::
+Uncompress the tar-gzip archive, e.g. (replace 1.7.2 with the correct version number)::
 
-  tar xvfj pcl-pcl-1.7.2.tar.gz
+  tar xvf pcl-pcl-1.7.2.tar.gz
 
 Change the directory to the pcl-pcl-1.7.2 (replace 1.7.2 with the correct version number) directory, and create a build directory in there::
 
@@ -84,7 +84,7 @@ Mandatory
 The following code libraries are **required** for the compilation and usage of the PCL libraries shown below:
 
 .. note::
-pcl_* denotes all PCL libraries, meaning that the particular dependency is a strict requirement for the usage of anything in PCL.
+   pcl_* denotes all PCL libraries, meaning that the particular dependency is a strict requirement for the usage of anything in PCL.
 
 +---------------------------------------------------------------+-----------------+-------------------------+-------------------+
 | Logo                                                          | Library         | Minimum version         | Mandatory         |
diff --git a/doc/tutorials/content/conditional_euclidean_clustering.rst b/doc/tutorials/content/conditional_euclidean_clustering.rst
index ee207734..cfc6b9b4 100644
--- a/doc/tutorials/content/conditional_euclidean_clustering.rst
+++ b/doc/tutorials/content/conditional_euclidean_clustering.rst
@@ -44,7 +44,7 @@ Since the Conditional Euclidean Clustering class is for more advanced users, I w
  - ``pcl::console::TicToc`` is used for easy output of timing results.
  - :ref:`voxelgrid` is being used (lines 66-73) to downsample the cloud and give a more equalized point density.
  - :ref:`normal_estimation` is being used (lines 75-83)  to estimate normals which will be appended to the point information;
-   The Conditional Euclidean Clustering class will be templated with ``pcl::PoitnXYZINormal``, containing x, y, z, intensity, normal and curvature information to use in the condition function.
+   The Conditional Euclidean Clustering class will be templated with ``pcl::PointXYZINormal``, containing x, y, z, intensity, normal and curvature information to use in the condition function.
 
 Lines 85-95 set up the Conditional Euclidean Clustering class for use:
 
@@ -64,7 +64,7 @@ A more elaborate description of the different lines of code:
  - Clusters that make up less than 0.1% of the cloud's total points are considered too small.
  - Clusters that make up more than 20% of the cloud's total points are considered too large.
  - The resulting clusters are stored in the ``pcl::IndicesClusters`` format, which is an array of indices-arrays, indexing points of the input point cloud.
- - Too small clusters or too large clusters are not passed to the main output but can instead be retrieved in separate ``pcl::IndicesClusters`` data containers, but only is the class was initialized with TRUE.
+ - Too small clusters or too large clusters are not passed to the main output but can instead be retrieved in separate ``pcl::IndicesClusters`` data containers, but only if the class was initialized with TRUE.
 
 Lines 12-49 show some examples of condition functions:
 
diff --git a/doc/tutorials/content/convex_hull_2d.rst b/doc/tutorials/content/convex_hull_2d.rst
index 3fa12db8..cda20c83 100644
--- a/doc/tutorials/content/convex_hull_2d.rst
+++ b/doc/tutorials/content/convex_hull_2d.rst
@@ -12,7 +12,7 @@ dataset `table_scene_mug_stereo_textured.pcd
 
 .. raw:: html
   
-  <iframe title="Acquiring the convex hull of a planar surface" width="480" height="390" src="http://www.youtube.com/embed/J9CjWDgPDTM?rel=0" frameborder="0" allowfullscreen></iframe>
+  <iframe title="Acquiring the convex hull of a planar surface" width="480" height="390" src="https://www.youtube.com/embed/J9CjWDgPDTM?rel=0" frameborder="0" allowfullscreen></iframe>
 
 The code
 --------
diff --git a/doc/tutorials/content/cylinder_segmentation.rst b/doc/tutorials/content/cylinder_segmentation.rst
index a83da226..a618d7e7 100644
--- a/doc/tutorials/content/cylinder_segmentation.rst
+++ b/doc/tutorials/content/cylinder_segmentation.rst
@@ -14,7 +14,7 @@ operations are applied to the input dataset (in order):
 
 .. raw:: html
 
-   <iframe title="Cylinder model segmentation" width="480" height="390" src="http://www.youtube.com/embed/SjbEDEGAeTk?rel=0" frameborder="0" allowfullscreen></iframe>
+   <iframe title="Cylinder model segmentation" width="480" height="390" src="https://www.youtube.com/embed/SjbEDEGAeTk?rel=0" frameborder="0" allowfullscreen></iframe>
 
 .. note:: 
    The cylindrical model is not perfect due to the presence of noise in the data.
diff --git a/doc/tutorials/content/ensenso_cameras.rst b/doc/tutorials/content/ensenso_cameras.rst
index c993b787..a69d7590 100644
--- a/doc/tutorials/content/ensenso_cameras.rst
+++ b/doc/tutorials/content/ensenso_cameras.rst
@@ -47,6 +47,7 @@ If you have performed and stored an extrinsic calibration it will be temporary r
 If you are using an Ensenso X device you have to calibrate the device before trying to run the PCL driver. If you don't you will get an error like this:
 
 .. code-block:: cpp
+
   Initialising nxLib
   Opening Ensenso stereo camera id = 0
   openDevice: NxLib error ExecutionFailed (17) occurred while accessing item /Execute.
diff --git a/doc/tutorials/content/extract_indices.rst b/doc/tutorials/content/extract_indices.rst
index 4026601b..ced702cc 100644
--- a/doc/tutorials/content/extract_indices.rst
+++ b/doc/tutorials/content/extract_indices.rst
@@ -11,7 +11,7 @@ the :ref:`planar_segmentation` tutorial for more information.
 
 .. raw:: html
 
-   <iframe title="Extracting indices from a PointCloud" width="480" height="390" src="http://www.youtube.com/embed/ZTK7NR1Xx4c?rel=0" frameborder="0" allowfullscreen></iframe>
+   <iframe title="Extracting indices from a PointCloud" width="480" height="390" src="https://www.youtube.com/embed/ZTK7NR1Xx4c?rel=0" frameborder="0" allowfullscreen></iframe>
 
 The code
 --------
diff --git a/doc/tutorials/content/function_filter.rst b/doc/tutorials/content/function_filter.rst
new file mode 100644
index 00000000..59ed2335
--- /dev/null
+++ b/doc/tutorials/content/function_filter.rst
@@ -0,0 +1,78 @@
+.. _function_filter:
+
+Removing outliers using a custom non-destructive condition
+----------------------------------------------------------
+
+This document demonstrates how to use the FunctionFilter class to remove points from a PointCloud that do not satisfy a custom criteria. This is a cleaner
+and faster appraoch compared to ConditionalRemoval filter or a `custom Condition class <https://cpp-optimizations.netlify.app/pcl_filter/>`_.
+
+.. note::
+   Advanced users can use the FunctorFilter class that can provide a small but measurable speedup when used with a `lambda <https://en.cppreference.com/w/cpp/language/lambda>`_.
+
+The code
+--------
+
+First, create a file, let's say, sphere_removal.cpp in you favorite editor, and place the following inside it:
+
+.. literalinclude:: sources/function_filter/sphere_removal.cpp
+   :language: cpp
+   :linenos:
+
+The explanation
+---------------
+
+Now, let's break down the code piece by piece.
+
+In the following lines, we define the PointCloud structures, fill in the input cloud, and display its content to screen.
+
+.. literalinclude:: sources/function_filter/sphere_removal.cpp
+   :language: cpp
+   :lines: 10-21
+
+Then, we create the condition which a given point must satisfy so that it remains in our PointCloud. To do this we create a `std::function` which accepts a PointCloud by const reference and an index, and returns true only if the point lies inside a sphere. This is then used to build the filter
+
+.. literalinclude:: sources/function_filter/sphere_removal.cpp
+   :language: cpp
+   :lines: 23-34
+
+This last bit of code just applies the filter to our original PointCloud, and removes all of the points that do not satisfy the conditions we specified. Then it outputs all of the points remaining in the PointCloud.
+
+.. literalinclude:: sources/function_filter/sphere_removal.cpp
+   :language: cpp
+   :lines: 36-42
+
+Compiling and running the program
+---------------------------------
+
+Add the following lines to your CMakeLists.txt file:
+
+.. literalinclude:: sources/function_filter/CMakeLists.txt
+   :language: cmake
+   :linenos:
+
+After you have compiled the executable, you can run it. Simply do::
+
+  $ ./sphere_removal
+
+You will see something similar to::
+
+  Cloud before filtering:
+      -1.23392 1.81505 -0.968005
+      -0.00934529 1.36497 0.158734
+      0.488435 1.96851 -0.0534078
+      1.27135 1.16404 -1.00462
+      -0.249089 -0.0815883 1.13229
+      0.448447 1.48914 1.78378
+      1.14143 1.77363 1.68965
+      1.08544 -1.01664 -1.13041
+      1.1199 0.9951 -1.13308
+      1.44268 -1.44434 -0.391739
+  Cloud after filtering:
+      -1.23392 1.81505 -0.968005
+      -0.00934529 1.36497 0.158734
+      0.488435 1.96851 -0.0534078
+      1.27135 1.16404 -1.00462
+      1.14143 1.77363 1.68965
+      1.08544 -1.01664 -1.13041
+      1.1199 0.9951 -1.13308
+      1.44268 -1.44434 -0.391739
diff --git a/doc/tutorials/content/global_hypothesis_verification.rst b/doc/tutorials/content/global_hypothesis_verification.rst
index aee55182..e0a41121 100644
--- a/doc/tutorials/content/global_hypothesis_verification.rst
+++ b/doc/tutorials/content/global_hypothesis_verification.rst
@@ -110,21 +110,21 @@ We create a ``instances`` list to store the "coarse" transformations :
 
 .. literalinclude:: sources/global_hypothesis_verification/global_hypothesis_verification.cpp
    :language: c++
-   :lines: 389-399
+   :lines: 387-397
 
 
 then, we run ICP on the ``instances`` wrt. the ``scene`` to obtain the ``registered_instances``: 
 
 .. literalinclude:: sources/global_hypothesis_verification/global_hypothesis_verification.cpp
    :language: c++
-   :lines: 401-431
+   :lines: 399-429
 
 Hypotheses Verification
 ***********************
 
 .. literalinclude:: sources/global_hypothesis_verification/global_hypothesis_verification.cpp
    :language: c++
-   :lines: 433-466
+   :lines: 431-465
 
 ``GlobalHypothesesVerification`` takes as input a list of ``registered_instances`` and a ``scene`` so we can ``verify()`` them
 to get a ``hypotheses_mask``: this is a `bool` array where ``hypotheses_mask[i]`` is ``TRUE`` if ``registered_instances[i]`` is a
@@ -142,7 +142,7 @@ Each ``registered_instances[i]`` will be displayed with two optional colors: ``s
 
 .. literalinclude:: sources/global_hypothesis_verification/global_hypothesis_verification.cpp
    :language: c++
-   :lines: 468-525
+   :lines: 467-524
 
 
 Compiling and running the program
diff --git a/doc/tutorials/content/gpu_install.rst b/doc/tutorials/content/gpu_install.rst
index 83963163..5b58a5ce 100644
--- a/doc/tutorials/content/gpu_install.rst
+++ b/doc/tutorials/content/gpu_install.rst
@@ -9,7 +9,7 @@ This tutorial is for Ubuntu, other Linux distrubutions can follow a similar proc
 Windows is currently  **not** officially supported for the GPU methods.
 
 Checking CUDA Version
----------------
+---------------------
 
 In order to run the code you will need a system with an Nvidia GPU, having CUDA Toolkit v9.2+ installed. 
 We will not be covering CUDA toolkit installation in this tutorial as there already exists many great tutorials for the same.
@@ -20,7 +20,7 @@ You can check your CUDA toolkit version using the following command::
  
  
 Checking C++ Version
----------------
+--------------------
 
 The GPU methods in PCL require a min version of GCC 7 or Clang 6 onwards (min version unknown). 
 This will not be a problem if you are running Ubuntu 18.04 or later. However on Ubuntu 16.04, you will need to install GCC 7 or Clang 6 (lower versions not tested) manually because the versions available by default are: GCC 5 and Clang 3.8
@@ -46,10 +46,10 @@ $ sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-7 60 --slave
 $ sudo update-alternatives --config gcc
 
 Installing Eigen
---------------- 
+----------------
 
 You will also need Eigen v3.3.7+, since the previous versions are incompatible with the latest CUDA versions. 
-If you are on Ubuntu 29+, then there is no issue since Eigen 3.3.7 is shipped by default. 
+If you are on Ubuntu 20 or newer, then there is no issue since Eigen 3.3.7 is shipped by default. 
 On older versions Eigen v3.3.7 will need to be installed manually::
 
 $ wget -qO- https://gitlab.com/libeigen/eigen/-/archive/3.3.7/eigen-3.3.7.tar.gz | tar xz 
diff --git a/doc/tutorials/content/gpu_people.rst b/doc/tutorials/content/gpu_people.rst
index 67398e8d..e9c9cdf0 100644
--- a/doc/tutorials/content/gpu_people.rst
+++ b/doc/tutorials/content/gpu_people.rst
@@ -9,7 +9,7 @@ has been presented on ICRA2012 and IROS2012 and an official reference for a jour
 
   .. raw:: html
 
-     <iframe width="560" height="315" src="http://www.youtube.com/embed/Wd4OM8wOO1E?rel=0" frameborder="0" allowfullscreen></iframe>
+     <iframe width="560" height="315" src="https://www.youtube.com/embed/Wd4OM8wOO1E?rel=0" frameborder="0" allowfullscreen></iframe>
 
 This shows how to detect people with an Primesense device, the full version 
 working on oni and pcd files can be found in the git master.
@@ -39,7 +39,7 @@ Now, let's break down the code piece by piece. Starting from the main routine.
 
 .. literalinclude:: sources/gpu/people_detect/src/people_detect.cpp
    :language: cpp
-   :lines: 317-367
+   :lines: 319-369
 
 First the GPU device is set, by default this is the first GPU found in the bus, but if you have multiple GPU's in
 your system, this allows you to select a specific one.
@@ -47,7 +47,7 @@ Then a OpenNI Capture is made, see the OpenNI Grabber tutorial for more info on
 
 .. literalinclude:: sources/gpu/people_detect/src/people_detect.cpp
    :language: cpp
-   :lines: 329-338
+   :lines: 331-340
 
 The implementation is based on a similar approach as Shotton et al. and thus needs off-line learned random
 decision forests for labeling. The current implementation allows up to 4 decision trees to be loaded into
@@ -55,19 +55,19 @@ the forest. This is done by giving it the names of the text files to load.
 
 .. literalinclude:: sources/gpu/people_detect/src/people_detect.cpp
    :language: cpp
-   :lines: 340-341
+   :lines: 342-343
 
 An additional parameter allows you to configure the number of trees to be loaded. 
 
 .. literalinclude:: sources/gpu/people_detect/src/people_detect.cpp
    :language: cpp
-   :lines: 350-352
+   :lines: 352-354
 
 Then the RDF object is created, loading the trees upon creation.
 
 .. literalinclude:: sources/gpu/people_detect/src/people_detect.cpp
    :language: cpp
-   :lines: 354-359
+   :lines: 356-361
 
 Now we create the application object, give it the pointer to the RDF object and start the loop.
 Now we'll have a look at the main loop.
@@ -75,7 +75,7 @@ Now we'll have a look at the main loop.
 
 .. literalinclude:: sources/gpu/people_detect/src/people_detect.cpp
    :language: cpp
-   :lines: 238-286
+   :lines: 237-288
 
 This routine first connects a callback routine to the grabber and waits for valid data to arrive.
 Each time the data arrives it will call the process function of the people detector, this is a
@@ -87,13 +87,13 @@ The visualizeAndWrite method will illustrate one of the available methods of the
 
 .. literalinclude:: sources/gpu/people_detect/src/people_detect.cpp
    :language: cpp
-   :lines: 141-178
+   :lines: 140-177
 
-Line 144 calls the RDF getLabels method which returns the labels on the device, these however
+Line 143 calls the RDF getLabels method which returns the labels on the device, these however
 are a discrete enum of the labels and are visually hard to recognize, so these are converted to
-colors that illustrate each body part in line 145. 
+colors that illustrate each body part in line 144. 
 At this point the results are still stored in the device memory and need to be copied to the CPU
-host memory, this is done in line 151. Afterwards the images are shown and stored to disk.
+host memory, this is done in line 150. Afterwards the images are shown and stored to disk.
 
 Compiling and running the program
 ---------------------------------
diff --git a/doc/tutorials/content/greedy_projection.rst b/doc/tutorials/content/greedy_projection.rst
index 484e96fa..ddd3482e 100644
--- a/doc/tutorials/content/greedy_projection.rst
+++ b/doc/tutorials/content/greedy_projection.rst
@@ -9,7 +9,7 @@ local neighborhoods. An example of the method's output can be seen here:
 
 .. raw:: html
 
-  <iframe title="Surface Triangulation and Point Cloud Classification" width="480" height="390" src="http://www.youtube.com/embed/VALTnZCyWc0?rel=0" frameborder="0" allowfullscreen></iframe>
+  <iframe title="Surface Triangulation and Point Cloud Classification" width="480" height="390" src="https://www.youtube.com/embed/VALTnZCyWc0?rel=0" frameborder="0" allowfullscreen></iframe>
 
 Background: algorithm and parameters
 ------------------------------------
diff --git a/doc/tutorials/content/how_features_work.rst b/doc/tutorials/content/how_features_work.rst
index 0989543e..8eef3a32 100644
--- a/doc/tutorials/content/how_features_work.rst
+++ b/doc/tutorials/content/how_features_work.rst
@@ -54,13 +54,13 @@ the table below for a reference on each of the terms used.
 +=============+================================================+
 | Foo         | a class named `Foo`                            |
 +-------------+------------------------------------------------+
-| FooPtr      | a shared pointer to a class `Foo`,       |
+| FooPtr      | a shared pointer to a class `Foo`,             |
 |             |                                                | 
-|             | e.g., `shared_ptr<Foo>`                 |
+|             | e.g., `shared_ptr<Foo>`                        |
 +-------------+------------------------------------------------+
-| FooConstPtr | a const shared pointer to a class `Foo`, |
+| FooConstPtr | a const shared pointer to a class `Foo`,       |
 |             |                                                |
-|             | e.g., `const shared_ptr<const Foo>`     |
+|             | e.g., `const shared_ptr<const Foo>`            |
 +-------------+------------------------------------------------+
 
 How to pass the input
diff --git a/doc/tutorials/content/index.rst b/doc/tutorials/content/index.rst
index f286330a..a922b33c 100644
--- a/doc/tutorials/content/index.rst
+++ b/doc/tutorials/content/index.rst
@@ -2,7 +2,7 @@
 
 
 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,
diff --git a/doc/tutorials/content/matrix_transform.rst b/doc/tutorials/content/matrix_transform.rst
index 763f296d..a7017603 100644
--- a/doc/tutorials/content/matrix_transform.rst
+++ b/doc/tutorials/content/matrix_transform.rst
@@ -4,7 +4,7 @@ Using a matrix to transform a point cloud
 -----------------------------------------
 
 In this tutorial we will learn how to transform a point cloud using a 4x4 matrix.
-We will apply a rotation and a translation to a loaded point cloud and display then
+We will apply a rotation and a translation to a loaded point cloud and display the
 result.
 
 This program is able to load one PCD or PLY file; apply a matrix transformation on it
diff --git a/doc/tutorials/content/min_cut_segmentation.rst b/doc/tutorials/content/min_cut_segmentation.rst
index f20774c0..3fb1dd88 100644
--- a/doc/tutorials/content/min_cut_segmentation.rst
+++ b/doc/tutorials/content/min_cut_segmentation.rst
@@ -69,62 +69,62 @@ These lines are simply loading the cloud from the .pcd file.
 
 .. literalinclude:: sources/min_cut_segmentation/min_cut_segmentation.cpp
    :language: cpp
-   :lines: 18-23
+   :lines: 18-19
 
-This few lines are not necessary. Their only purpose is to show that ``pcl::MinCutSegmentation`` class can work with indices.
+The purpose of these lines is to show that ``pcl::MinCutSegmentation`` class can work with indices. Here, only the valid points are chosen for segmentation.
 
 .. literalinclude:: sources/min_cut_segmentation/min_cut_segmentation.cpp
    :language: cpp
-   :lines: 25-25
+   :lines: 21-21
 
 Here is the line where the instantiation of the ``pcl::MinCutSegmentation`` class takes place.
 It is the tamplate class that has only one parameter - PointT - which says what type of points will be used.
 
 .. literalinclude:: sources/min_cut_segmentation/min_cut_segmentation.cpp
    :language: cpp
-   :lines: 26-27
+   :lines: 22-23
 
 These lines provide the algorithm with the cloud that must be segmented and the indices.
 
 .. literalinclude:: sources/min_cut_segmentation/min_cut_segmentation.cpp
    :language: cpp
-   :lines: 29-35
+   :lines: 25-31
 
 As mentioned before, algorithm requires point that is known to be the objects center. These lines provide it.
 
 .. literalinclude:: sources/min_cut_segmentation/min_cut_segmentation.cpp
    :language: cpp
-   :lines: 37-38
+   :lines: 33-34
 
 These lines set :math:`\sigma` and objects radius required for smooth cost calculation.
 
 .. literalinclude:: sources/min_cut_segmentation/min_cut_segmentation.cpp
    :language: cpp
-   :lines: 39-39
+   :lines: 35-35
 
 This line tells how much neighbours to find when constructing the graph. The more neighbours is set, the more number of edges it will contain.
 
 .. literalinclude:: sources/min_cut_segmentation/min_cut_segmentation.cpp
    :language: cpp
-   :lines: 40-40
+   :lines: 36-36
 
 Here is the line where foreground penalty is set.
 
 .. literalinclude:: sources/min_cut_segmentation/min_cut_segmentation.cpp
    :language: cpp
-   :lines: 42-43
+   :lines: 38-39
 
 These lines are responsible for launching the algorithm. After the segmentation clusters will contain the result.
 
 .. literalinclude:: sources/min_cut_segmentation/min_cut_segmentation.cpp
    :language: cpp
-   :lines: 45-45
+   :lines: 41-41
 
 You can easily access the flow value that was computed during the graph cut. This is exactly what happening here.
 
 .. literalinclude:: sources/min_cut_segmentation/min_cut_segmentation.cpp
    :language: cpp
-   :lines: 47-52
+   :lines: 43-48
 
 These lines simply create the instance of ``CloudViewer`` class for result visualization.
 
diff --git a/doc/tutorials/content/mobile_streaming.rst b/doc/tutorials/content/mobile_streaming.rst
index 0932494f..52bc755a 100644
--- a/doc/tutorials/content/mobile_streaming.rst
+++ b/doc/tutorials/content/mobile_streaming.rst
@@ -166,7 +166,7 @@ that lie outside of the predefined bounding box or contain NaN values.
     {
       const std::size_t nr_points = cloud->size ();
 
-      cloud_buffers.points.resize (nr_points*3);
+      cloud_buffers.resize (nr_points*3);
       cloud_buffers.rgb.resize (nr_points*3);
 
       const pcl::PointXYZ  bounds_min (-0.9, -0.8, 1.0);
@@ -204,7 +204,7 @@ that lie outside of the predefined bounding box or contain NaN values.
         j++;
       }
 
-      cloud_buffers.points.resize (j * 3);
+      cloud_buffers.resize (j * 3);
       cloud_buffers.rgb.resize (j * 3);
     }
 
diff --git a/doc/tutorials/content/normal_estimation.rst b/doc/tutorials/content/normal_estimation.rst
index ed9cbdce..ec1f9e76 100644
--- a/doc/tutorials/content/normal_estimation.rst
+++ b/doc/tutorials/content/normal_estimation.rst
@@ -26,7 +26,7 @@ directly compute the surface normals at each point in the cloud.
 
 .. raw:: html
 
-  <iframe width="425" height="349" src="http://www.youtube.com/embed/x1FSssJrfik" frameborder="0" allowfullscreen></iframe>
+  <iframe width="425" height="349" src="https://www.youtube.com/embed/x1FSssJrfik" frameborder="0" allowfullscreen></iframe>
 
 Theoretical primer
 ------------------
diff --git a/doc/tutorials/content/octree.rst b/doc/tutorials/content/octree.rst
index f38adfdc..fbafcb1b 100644
--- a/doc/tutorials/content/octree.rst
+++ b/doc/tutorials/content/octree.rst
@@ -22,7 +22,7 @@ The explanation
 
 Now, let's explain the code in detail.
 
-We fist define and instantiate a shared PointCloud structure and fill it with random points.
+We first define and instantiate a shared PointCloud structure and fill it with random points.
 
 .. literalinclude:: sources/octree_search/octree_search.cpp
    :language: cpp
@@ -38,7 +38,7 @@ Then we assign a pointer to the PointCloud and add all points to the octree.
    :language: cpp
    :lines: 27-32
 
-Once the PointCloud is associated with an octree, we can perform search operations. The fist search method used here is "Neighbors within Voxel Search". It assigns the search point to the corresponding 
+Once the PointCloud is associated with an octree, we can perform search operations. The first search method used here is "Neighbors within Voxel Search". It assigns the search point to the corresponding 
 leaf node voxel and returns a vector of point indices. These indices relate to points which fall within the same voxel. The distance between 
 the search point and the search result depend therefore on the resolution parameter of the octree.
 
diff --git a/doc/tutorials/content/octree_change.rst b/doc/tutorials/content/octree_change.rst
index 536627c9..dc553683 100644
--- a/doc/tutorials/content/octree_change.rst
+++ b/doc/tutorials/content/octree_change.rst
@@ -23,7 +23,7 @@ The explanation
 
 Now, let's discuss the code in detail.
 
-We fist instantiate the OctreePointCloudChangeDetector class and define its voxel resolution. 
+We first instantiate the OctreePointCloudChangeDetector class and define its voxel resolution. 
 
 .. literalinclude:: sources/octree_change_detection/octree_change_detection.cpp
    :language: cpp
diff --git a/doc/tutorials/content/openni_grabber.rst b/doc/tutorials/content/openni_grabber.rst
index f4509b1f..885f1247 100644
--- a/doc/tutorials/content/openni_grabber.rst
+++ b/doc/tutorials/content/openni_grabber.rst
@@ -34,7 +34,7 @@ the OpenNI Grabber.
 
 .. raw:: html
   
-  <iframe title="PCL OpenNI Viewer example" width="480" height="390" src="http://www.youtube.com/embed/x3SaWQkPsPI?rel=0" frameborder="0" allowfullscreen></iframe>
+  <iframe title="PCL OpenNI Viewer example" width="480" height="390" src="https://www.youtube.com/embed/x3SaWQkPsPI?rel=0" frameborder="0" allowfullscreen></iframe>
 
 So let's look at the code. From *visualization/tools/openni_viewer_simple.cpp*
 
diff --git a/doc/tutorials/content/pcd_file_format.rst b/doc/tutorials/content/pcd_file_format.rst
index 210b37be..688da23b 100644
--- a/doc/tutorials/content/pcd_file_format.rst
+++ b/doc/tutorials/content/pcd_file_format.rst
@@ -151,7 +151,7 @@ As of version 0.7, the PCD header contains the following entries:
 
 
 * **DATA** - specifies the data type that the point cloud data is stored in. As
-  of version 0.7, two data types are supported: *ascii* and *binary*. See the
+  of version 0.7, three data types are supported: *ascii*, *binary*, and *binary_compressed*. See the
   next section for more details.
 
 
@@ -178,7 +178,7 @@ As of version 0.7, the PCD header contains the following entries:
 Data storage types
 ------------------
 
-As of version 0.7, the **.PCD** file format uses two different modes for storing data:
+As of version 0.7, the **.PCD** file format uses three different modes for storing data:
 
 * in **ASCII** form, with each point on a new line::
 
@@ -198,6 +198,8 @@ As of version 0.7, the **.PCD** file format uses two different modes for storing
   `pcl::PointCloud.points` array/vector. On Linux systems, we use `mmap`/`munmap`
   operations for the fastest possible read/write access to the data.
 
+* in **binary_compressed** form. The body (everything after the header) starts with a 32 bit unsigned binary number which specifies the size in bytes of the data in *compressed* form. Next is another 32 bit unsigned binary number which specifies the size in bytes of the data in *uncompressed* form. Then follows the compressed data. The compression and decompression is done using Marc Lehmann's LZF algorithm. It is mediocre in terms of size reduction, but very fast. For typical point clouds, the compressed data has 30 to 60 percent of the original size. Before compressing, the data is reordered to improve compression, from the standard array-of-structures layout to a structure-of-arrays layout. So for example a cloud with three points and fields x, y, z would be reordered from xyzxyzxyz to xxxyyyzzz.
+
 
 Storing point cloud data in both a simple ascii form with each point on a line,
 space or tab separated, without any other characters on it, as well as in a
diff --git a/doc/tutorials/content/pcl_painter2D.rst b/doc/tutorials/content/pcl_painter2D.rst
index 5cd41b84..28c2e70b 100644
--- a/doc/tutorials/content/pcl_painter2D.rst
+++ b/doc/tutorials/content/pcl_painter2D.rst
@@ -74,4 +74,4 @@ The following video shows the the output of the demo.
 
 .. raw:: html
     
-    <iframe width="420" height="315" src="http://www.youtube.com/embed/0kPwTds7HSk" frameborder="0" allowfullscreen></iframe>
+    <iframe width="420" height="315" src="https://www.youtube.com/embed/0kPwTds7HSk" frameborder="0" allowfullscreen></iframe>
diff --git a/doc/tutorials/content/pcl_plotter.rst b/doc/tutorials/content/pcl_plotter.rst
index 79d33435..97cdd49e 100644
--- a/doc/tutorials/content/pcl_plotter.rst
+++ b/doc/tutorials/content/pcl_plotter.rst
@@ -245,6 +245,6 @@ The following video shows the the output of the demo.
 
 .. raw:: html
     
-    <iframe width="480" height="270" src="http://www.youtube.com/embed/2Xgd67nkwzs" frameborder="0" allowfullscreen></iframe>
+    <iframe width="480" height="270" src="https://www.youtube.com/embed/2Xgd67nkwzs" frameborder="0" allowfullscreen></iframe>
 
     
diff --git a/doc/tutorials/content/planar_segmentation.rst b/doc/tutorials/content/planar_segmentation.rst
index be1b1bef..c5b5b17a 100644
--- a/doc/tutorials/content/planar_segmentation.rst
+++ b/doc/tutorials/content/planar_segmentation.rst
@@ -10,7 +10,7 @@ the **filtering** section.
 
 .. raw:: html
 
-   <iframe title="Planar model segmentation" width="480" height="390" src="http://www.youtube.com/embed/ZTK7NR1Xx4c?rel=0" frameborder="0" allowfullscreen></iframe>
+   <iframe title="Planar model segmentation" width="480" height="390" src="https://www.youtube.com/embed/ZTK7NR1Xx4c?rel=0" frameborder="0" allowfullscreen></iframe>
 
 The code
 --------
@@ -36,7 +36,7 @@ Lines:
 
 .. important::
 
-  Please visit http://docs.pointclouds.org/trunk/a02954.html
+  Please visit https://pointclouds.org/documentation/group__sample__consensus.html
   for more information on various other implemented Sample Consensus models and
   robust estimators.
 
diff --git a/doc/tutorials/content/random_sample_consensus.rst b/doc/tutorials/content/random_sample_consensus.rst
index d627f8b2..ed5dad49 100644
--- a/doc/tutorials/content/random_sample_consensus.rst
+++ b/doc/tutorials/content/random_sample_consensus.rst
@@ -10,7 +10,7 @@ Theoretical Primer
 
 The abbreviation of "RANdom SAmple Consensus" is RANSAC, and it is an iterative method that is used to estimate parameters of a mathematical model from a set of data containing outliers.  This algorithm was published by Fischler and Bolles in 1981.  The RANSAC algorithm assumes that all of the data we are looking at is comprised of both inliers and outliers.  Inliers can be explained by a model with a particular set of parameter values, while outliers do not fit that model in any circumstance.  Another necessary assumption is that a procedure which can optimally estimate the parameters of the chosen model from the data is available.
 
-From [Wikipedia]_:
+From [WikipediaRANSAC]_:
 
   *The input to the RANSAC algorithm is a set of observed data values, a parameterized model which can explain or be fitted to the observations, and some confidence parameters.*
 
@@ -38,7 +38,7 @@ From [Wikipedia]_:
    :align: right
    :height: 200px
 
-The pictures to the left and right (From [Wikipedia]_) show a simple application of the RANSAC algorithm on a 2-dimensional set of data. The image on our left is a visual representation of a data set containing both inliers and outliers.  The image on our right shows all of the outliers in red, and shows inliers in blue.  The blue line is the result of the work done by RANSAC.  In this case the model that we are trying to fit to the data is a line, and it looks like it's a fairly good fit to our data.
+The pictures to the left and right (From [WikipediaRANSAC]_) show a simple application of the RANSAC algorithm on a 2-dimensional set of data. The image on our left is a visual representation of a data set containing both inliers and outliers.  The image on our right shows all of the outliers in red, and shows inliers in blue.  The blue line is the result of the work done by RANSAC.  In this case the model that we are trying to fit to the data is a line, and it looks like it's a fairly good fit to our data.
 
 The code
 --------
@@ -123,4 +123,4 @@ It will show you the result of applying RandomSampleConsensus to this data set w
    :align: center
    :height: 400px
 
-.. [Wikipedia] http://en.wikipedia.org/wiki/RANSAC
+.. [WikipediaRANSAC] http://en.wikipedia.org/wiki/RANSAC
diff --git a/doc/tutorials/content/region_growing_rgb_segmentation.rst b/doc/tutorials/content/region_growing_rgb_segmentation.rst
index a32ccf6d..bbb81c79 100644
--- a/doc/tutorials/content/region_growing_rgb_segmentation.rst
+++ b/doc/tutorials/content/region_growing_rgb_segmentation.rst
@@ -39,7 +39,7 @@ They are simply loading the cloud from the .pcd file. Note that points must have
 
 .. literalinclude:: sources/region_growing_rgb_segmentation/region_growing_rgb_segmentation.cpp
    :language: cpp
-   :lines: 34-34
+   :lines: 30-30
 
 This line is responsible for ``pcl::RegionGrowingRGB`` instantiation. This class has two parameters:
 
@@ -49,40 +49,40 @@ This line is responsible for ``pcl::RegionGrowingRGB`` instantiation. This class
 
 .. literalinclude:: sources/region_growing_rgb_segmentation/region_growing_rgb_segmentation.cpp
    :language: cpp
-   :lines: 35-37
+   :lines: 31-33
 
 These lines provide the instance with the input cloud, indices and search method.
 
 .. literalinclude:: sources/region_growing_rgb_segmentation/region_growing_rgb_segmentation.cpp
    :language: cpp
-   :lines: 38-38
+   :lines: 34-34
 
 Here the distance threshold is set. It is used to determine whether the point is neighbouring or not. If the point is located at a distance less than
 the given threshold, then it is considered to be neighbouring. It is used for clusters neighbours search.
 
 .. literalinclude:: sources/region_growing_rgb_segmentation/region_growing_rgb_segmentation.cpp
    :language: cpp
-   :lines: 39-39
+   :lines: 35-35
 
 This line sets the color threshold. Just as angle threshold is used for testing points normals in ``pcl::RegionGrowing``
 to determine if the point belongs to cluster, this value is used for testing points colors.
 
 .. literalinclude:: sources/region_growing_rgb_segmentation/region_growing_rgb_segmentation.cpp
    :language: cpp
-   :lines: 40-40
+   :lines: 36-36
 
 Here the color threshold for clusters is set. This value is similar to the previous, but is used when the merging process takes place.
 
 .. literalinclude:: sources/region_growing_rgb_segmentation/region_growing_rgb_segmentation.cpp
    :language: cpp
-   :lines: 41-41
+   :lines: 37-37
 
 This value is similar to that which was used in the :ref:`region_growing_segmentation` tutorial. In addition to that, it is used for merging process mentioned in the beginning.
 If cluster has less points than was set through ``setMinClusterSize`` method, then it will be merged with the nearest neighbour.
 
 .. literalinclude:: sources/region_growing_rgb_segmentation/region_growing_rgb_segmentation.cpp
    :language: cpp
-   :lines: 43-44
+   :lines: 39-40
 
 Here is the place where the algorithm is launched. It will return the array of clusters when the segmentation process will be over.
 
diff --git a/doc/tutorials/content/region_growing_segmentation.rst b/doc/tutorials/content/region_growing_segmentation.rst
index 65193dc0..91c8fab3 100644
--- a/doc/tutorials/content/region_growing_segmentation.rst
+++ b/doc/tutorials/content/region_growing_segmentation.rst
@@ -116,14 +116,13 @@ To learn more about how it is done you should take a look at the :ref:`normal_es
 
 .. literalinclude:: sources/region_growing_segmentation/region_growing_segmentation.cpp
    :language: cpp
-   :lines: 30-35
+   :lines: 30-31
 
-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 instruct that you only segment those points that are listed in the indices array instead of the whole point cloud.
+Insofar as ``pcl::RegionGrowing`` is derived from ``pcl::PCLBase``, 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. Here, only the valid points are chosen for segmentation.
 
 .. literalinclude:: sources/region_growing_segmentation/region_growing_segmentation.cpp
    :language: cpp
-   :lines: 37-39
+   :lines: 33-35
 
 You have finally reached the part where ``pcl::RegionGrowing`` is instantiated. It is a template class that has two parameters:
 
@@ -136,14 +135,14 @@ The default values for minimum and maximum are 1 and 'as much as possible' respe
 
 .. literalinclude:: sources/region_growing_segmentation/region_growing_segmentation.cpp
    :language: cpp
-   :lines: 40-44
+   :lines: 36-40
 
 The algorithm needs K nearest search in its internal structure, so here is the place where a search method is provided
 and number of neighbours is set. After that it receives the cloud that must be segmented, point indices and normals.
 
 .. literalinclude:: sources/region_growing_segmentation/region_growing_segmentation.cpp
    :language: cpp
-   :lines: 45-46
+   :lines: 41-42
 
 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.
@@ -154,20 +153,20 @@ And if this value is less than the curvature threshold then the algorithm will c
 
 .. literalinclude:: sources/region_growing_segmentation/region_growing_segmentation.cpp
    :language: cpp
-   :lines: 48-49
+   :lines: 44-45
 
 This method simply launches the segmentation algorithm. After its work it will return clusters array.
 
 .. literalinclude:: sources/region_growing_segmentation/region_growing_segmentation.cpp
    :language: cpp
-   :lines: 51-63
+   :lines: 47-59
 
 These lines are simple enough, so they won't be commented. They are intended for those who are not familiar with how to work with ``pcl::PointIndices``
 and how to access its elements.
 
 .. literalinclude:: sources/region_growing_segmentation/region_growing_segmentation.cpp
    :language: cpp
-   :lines: 65-73
+   :lines: 61-69
 
 The ``pcl::RegionGrowing`` class provides a method that returns the colored cloud where each cluster has its own color.
 So in this part of code the ``pcl::visualization::CloudViewer`` is instantiated for viewing the result of the segmentation - the same colored cloud.
diff --git a/doc/tutorials/content/resampling.rst b/doc/tutorials/content/resampling.rst
index 66455a5a..f5da5fb3 100644
--- a/doc/tutorials/content/resampling.rst
+++ b/doc/tutorials/content/resampling.rst
@@ -9,7 +9,7 @@ the video below:
 
 .. raw:: html
 
-   <iframe title="Smoothing and normal estimation based on polynomial reconstruction" width="480" height="390" src="http://www.youtube.com/embed/FqHroDuo_I8?rel=0" frameborder="0" allowfullscreen></iframe>
+   <iframe title="Smoothing and normal estimation based on polynomial reconstruction" width="480" height="390" src="https://www.youtube.com/embed/FqHroDuo_I8?rel=0" frameborder="0" allowfullscreen></iframe>
 
 
 Some of the data irregularities (caused by small distance measurement errors)
@@ -24,7 +24,7 @@ be smoothed.
 
 .. image:: images/resampling_1.jpg
 
-On the left side of the figure above, we see the effect or estimating surface
+On the left side of the figure above, we see the effect of estimating surface
 normals in a dataset comprised of two registered point clouds together. Due to
 alignment errors, the resultant normals are noisy. On the right side we see the
 effects of surface normal estimation in the same dataset after it has been
@@ -40,7 +40,7 @@ defined on a robustly computed reference plane.
 
 .. raw:: html
 
-   <iframe title="Removing noisy data through resampling" width="480" height="390" src="http://www.youtube.com/embed/N5AgC0KEcw0?rel=0" frameborder="0" allowfullscreen></iframe>
+   <iframe title="Removing noisy data through resampling" width="480" height="390" src="https://www.youtube.com/embed/N5AgC0KEcw0?rel=0" frameborder="0" allowfullscreen></iframe>
 
 The code
 --------
diff --git a/doc/tutorials/content/sources/CMakeLists.txt b/doc/tutorials/content/sources/CMakeLists.txt
index 3adb9765..49b80f8b 100644
--- a/doc/tutorials/content/sources/CMakeLists.txt
+++ b/doc/tutorials/content/sources/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project("PCL_Tutorials")
 
diff --git a/doc/tutorials/content/sources/alignment_prerejective/CMakeLists.txt b/doc/tutorials/content/sources/alignment_prerejective/CMakeLists.txt
index 5ed7e82e..3239259b 100644
--- a/doc/tutorials/content/sources/alignment_prerejective/CMakeLists.txt
+++ b/doc/tutorials/content/sources/alignment_prerejective/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(alignment_prerejective)
 
diff --git a/doc/tutorials/content/sources/alignment_prerejective/alignment_prerejective.cpp b/doc/tutorials/content/sources/alignment_prerejective/alignment_prerejective.cpp
index 9a0fe104..6509177c 100644
--- a/doc/tutorials/content/sources/alignment_prerejective/alignment_prerejective.cpp
+++ b/doc/tutorials/content/sources/alignment_prerejective/alignment_prerejective.cpp
@@ -1,4 +1,5 @@
 #include <Eigen/Core>
+
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
 #include <pcl/common/time.h>
@@ -10,7 +11,6 @@
 #include <pcl/io/pcd_io.h>
 #include <pcl/registration/icp.h>
 #include <pcl/registration/sample_consensus_prerejective.h>
-#include <pcl/segmentation/sac_segmentation.h>
 #include <pcl/visualization/pcl_visualizer.h>
 
 // Types
diff --git a/doc/tutorials/content/sources/bare_earth/CMakeLists.txt b/doc/tutorials/content/sources/bare_earth/CMakeLists.txt
index 2652140a..4cf8a63c 100644
--- a/doc/tutorials/content/sources/bare_earth/CMakeLists.txt
+++ b/doc/tutorials/content/sources/bare_earth/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(bare_earth)
 
diff --git a/doc/tutorials/content/sources/bare_earth/bare_earth.cpp b/doc/tutorials/content/sources/bare_earth/bare_earth.cpp
index c09f49b0..c9774079 100644
--- a/doc/tutorials/content/sources/bare_earth/bare_earth.cpp
+++ b/doc/tutorials/content/sources/bare_earth/bare_earth.cpp
@@ -5,7 +5,7 @@
 #include <pcl/segmentation/progressive_morphological_filter.h>
 
 int
-main (int argc, char** argv)
+main ()
 {
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
diff --git a/doc/tutorials/content/sources/bspline_fitting/CMakeLists.txt b/doc/tutorials/content/sources/bspline_fitting/CMakeLists.txt
index d64245b4..7742fc76 100644
--- a/doc/tutorials/content/sources/bspline_fitting/CMakeLists.txt
+++ b/doc/tutorials/content/sources/bspline_fitting/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(bspline_fitting)
 
diff --git a/doc/tutorials/content/sources/cloud_viewer/CMakeLists.txt b/doc/tutorials/content/sources/cloud_viewer/CMakeLists.txt
index 284f06be..43bff93b 100644
--- a/doc/tutorials/content/sources/cloud_viewer/CMakeLists.txt
+++ b/doc/tutorials/content/sources/cloud_viewer/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(cloud_viewer)
 
diff --git a/doc/tutorials/content/sources/cluster_extraction/CMakeLists.txt b/doc/tutorials/content/sources/cluster_extraction/CMakeLists.txt
index da3071f4..5a186fb9 100644
--- a/doc/tutorials/content/sources/cluster_extraction/CMakeLists.txt
+++ b/doc/tutorials/content/sources/cluster_extraction/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(cluster_extraction)
 
diff --git a/doc/tutorials/content/sources/cluster_extraction/cluster_extraction.cpp b/doc/tutorials/content/sources/cluster_extraction/cluster_extraction.cpp
index 174f7c7d..22385562 100644
--- a/doc/tutorials/content/sources/cluster_extraction/cluster_extraction.cpp
+++ b/doc/tutorials/content/sources/cluster_extraction/cluster_extraction.cpp
@@ -4,7 +4,7 @@
 #include <pcl/filters/extract_indices.h>
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/features/normal_3d.h>
-#include <pcl/kdtree/kdtree.h>
+#include <pcl/search/kdtree.h>
 #include <pcl/sample_consensus/method_types.h>
 #include <pcl/sample_consensus/model_types.h>
 #include <pcl/segmentation/sac_segmentation.h>
@@ -12,7 +12,7 @@
 
 
 int 
-main (int argc, char** argv)
+main ()
 {
   // Read in the cloud data
   pcl::PCDReader reader;
@@ -40,7 +40,7 @@ main (int argc, char** argv)
   seg.setMaxIterations (100);
   seg.setDistanceThreshold (0.02);
 
-  int i=0, nr_points = (int) cloud_filtered->size ();
+  int nr_points = (int) cloud_filtered->size ();
   while (cloud_filtered->size () > 0.3 * nr_points)
   {
     // Segment the largest planar component from the remaining cloud
@@ -85,8 +85,8 @@ main (int argc, char** argv)
   for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
   {
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
-    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
-      cloud_cluster->push_back ((*cloud_filtered)[*pit]); //*
+    for (const auto& idx : it->indices)
+      cloud_cluster->push_back ((*cloud_filtered)[idx]); //*
     cloud_cluster->width = cloud_cluster->size ();
     cloud_cluster->height = 1;
     cloud_cluster->is_dense = true;
diff --git a/doc/tutorials/content/sources/concatenate_clouds/CMakeLists.txt b/doc/tutorials/content/sources/concatenate_clouds/CMakeLists.txt
index 8f55a284..01d85087 100644
--- a/doc/tutorials/content/sources/concatenate_clouds/CMakeLists.txt
+++ b/doc/tutorials/content/sources/concatenate_clouds/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(concatenate_clouds)
 
diff --git a/doc/tutorials/content/sources/concatenate_clouds/concatenate_clouds.cpp b/doc/tutorials/content/sources/concatenate_clouds/concatenate_clouds.cpp
index b817f6e3..ceb3dd36 100644
--- a/doc/tutorials/content/sources/concatenate_clouds/concatenate_clouds.cpp
+++ b/doc/tutorials/content/sources/concatenate_clouds/concatenate_clouds.cpp
@@ -1,6 +1,6 @@
 #include <iostream>
-#include <pcl/io/pcd_io.h>
 #include <pcl/point_types.h>
+#include <pcl/common/io.h> // for concatenateFields
 
 int
   main (int argc, char** argv)
@@ -17,15 +17,15 @@ int
   // Fill in the cloud data
   cloud_a.width  = 5;
   cloud_a.height = cloud_b.height = n_cloud_b.height = 1;
-    cloud_a.points.resize (cloud_a.width * cloud_a.height);
+    cloud_a.resize (cloud_a.width * cloud_a.height);
   if (strcmp(argv[1], "-p") == 0)
   {
     cloud_b.width  = 3;
-    cloud_b.points.resize (cloud_b.width * cloud_b.height);
+    cloud_b.resize (cloud_b.width * cloud_b.height);
   }
   else{
     n_cloud_b.width = 5;
-    n_cloud_b.points.resize (n_cloud_b.width * n_cloud_b.height);
+    n_cloud_b.resize (n_cloud_b.width * n_cloud_b.height);
   }
 
   for (std::size_t i = 0; i < cloud_a.size (); ++i)
diff --git a/doc/tutorials/content/sources/concatenate_fields/CMakeLists.txt b/doc/tutorials/content/sources/concatenate_fields/CMakeLists.txt
index 970adacf..adb6cff6 100644
--- a/doc/tutorials/content/sources/concatenate_fields/CMakeLists.txt
+++ b/doc/tutorials/content/sources/concatenate_fields/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(concatenate_fields)
 
diff --git a/doc/tutorials/content/sources/concatenate_fields/concatenate_fields.cpp b/doc/tutorials/content/sources/concatenate_fields/concatenate_fields.cpp
index 581fd05c..74a3a4ab 100644
--- a/doc/tutorials/content/sources/concatenate_fields/concatenate_fields.cpp
+++ b/doc/tutorials/content/sources/concatenate_fields/concatenate_fields.cpp
@@ -1,9 +1,9 @@
 #include <iostream>
-#include <pcl/io/pcd_io.h>
+#include <pcl/common/io.h> // for concatenateFields
 #include <pcl/point_types.h>
 
 int
-  main (int argc, char** argv)
+  main ()
 {
   pcl::PointCloud<pcl::PointXYZ> cloud_a;
   pcl::PointCloud<pcl::Normal> cloud_b;
@@ -12,8 +12,8 @@ int
   // Fill in the cloud data
   cloud_a.width  = cloud_b.width  = 5;
   cloud_a.height = cloud_b.height = 1;
-  cloud_a.points.resize (cloud_a.width * cloud_a.height);
-  cloud_b.points.resize (cloud_b.width * cloud_b.height);
+  cloud_a.resize (cloud_a.width * cloud_a.height);
+  cloud_b.resize (cloud_b.width * cloud_b.height);
 
   for (std::size_t i = 0; i < cloud_a.size (); ++i)
   {
diff --git a/doc/tutorials/content/sources/concatenate_points/CMakeLists.txt b/doc/tutorials/content/sources/concatenate_points/CMakeLists.txt
index 2044f0e3..80368b57 100644
--- a/doc/tutorials/content/sources/concatenate_points/CMakeLists.txt
+++ b/doc/tutorials/content/sources/concatenate_points/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(concatenate_points)
 
diff --git a/doc/tutorials/content/sources/concatenate_points/concatenate_points.cpp b/doc/tutorials/content/sources/concatenate_points/concatenate_points.cpp
index 942a1f18..e0b7a106 100644
--- a/doc/tutorials/content/sources/concatenate_points/concatenate_points.cpp
+++ b/doc/tutorials/content/sources/concatenate_points/concatenate_points.cpp
@@ -1,9 +1,9 @@
 #include <iostream>
-#include <pcl/io/pcd_io.h>
+#include <pcl/point_cloud.h> // for PointCloud
 #include <pcl/point_types.h>
 
 int
-  main (int argc, char** argv)
+  main ()
 {
   pcl::PointCloud<pcl::PointXYZ> cloud_a, cloud_b, cloud_c;
 
@@ -11,8 +11,8 @@ int
   cloud_a.width  = 5;
   cloud_b.width  = 3;
   cloud_a.height = cloud_b.height = 1;
-  cloud_a.points.resize (cloud_a.width * cloud_a.height);
-  cloud_b.points.resize (cloud_b.width * cloud_b.height);
+  cloud_a.resize (cloud_a.width * cloud_a.height);
+  cloud_b.resize (cloud_b.width * cloud_b.height);
 
   for (std::size_t i = 0; i < cloud_a.size (); ++i)
   {
diff --git a/doc/tutorials/content/sources/concave_hull_2d/CMakeLists.txt b/doc/tutorials/content/sources/concave_hull_2d/CMakeLists.txt
index 37af0cb2..eff91289 100644
--- a/doc/tutorials/content/sources/concave_hull_2d/CMakeLists.txt
+++ b/doc/tutorials/content/sources/concave_hull_2d/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(concave_hull_2d)
 
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 828e904f..8e9ee148 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
@@ -9,7 +9,7 @@
 #include <pcl/surface/concave_hull.h>
 
 int
-main (int argc, char** argv)
+main ()
 {
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), 
                                       cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), 
@@ -17,7 +17,7 @@ main (int argc, char** argv)
   pcl::PCDReader reader;
 
   reader.read ("table_scene_mug_stereo_textured.pcd", *cloud);
-  // Build a filter to remove spurious NaNs
+  // Build a filter to remove spurious NaNs and scene background
   pcl::PassThrough<pcl::PointXYZ> pass;
   pass.setInputCloud (cloud);
   pass.setFilterFieldName ("z");
diff --git a/doc/tutorials/content/sources/conditional_euclidean_clustering/CMakeLists.txt b/doc/tutorials/content/sources/conditional_euclidean_clustering/CMakeLists.txt
index 870d0de8..630217c8 100644
--- a/doc/tutorials/content/sources/conditional_euclidean_clustering/CMakeLists.txt
+++ b/doc/tutorials/content/sources/conditional_euclidean_clustering/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(conditional_euclidean_clustering)
 
diff --git a/doc/tutorials/content/sources/conditional_euclidean_clustering/conditional_euclidean_clustering.cpp b/doc/tutorials/content/sources/conditional_euclidean_clustering/conditional_euclidean_clustering.cpp
index 6e7b7bc3..eb4eb13e 100644
--- a/doc/tutorials/content/sources/conditional_euclidean_clustering/conditional_euclidean_clustering.cpp
+++ b/doc/tutorials/content/sources/conditional_euclidean_clustering/conditional_euclidean_clustering.cpp
@@ -10,7 +10,7 @@ typedef pcl::PointXYZI PointTypeIO;
 typedef pcl::PointXYZINormal PointTypeFull;
 
 bool
-enforceIntensitySimilarity (const PointTypeFull& point_a, const PointTypeFull& point_b, float squared_distance)
+enforceIntensitySimilarity (const PointTypeFull& point_a, const PointTypeFull& point_b, float /*squared_distance*/)
 {
   if (std::abs (point_a.intensity - point_b.intensity) < 5.0f)
     return (true);
@@ -19,12 +19,12 @@ enforceIntensitySimilarity (const PointTypeFull& point_a, const PointTypeFull& p
 }
 
 bool
-enforceCurvatureOrIntensitySimilarity (const PointTypeFull& point_a, const PointTypeFull& point_b, float squared_distance)
+enforceNormalOrIntensitySimilarity (const PointTypeFull& point_a, const PointTypeFull& point_b, float /*squared_distance*/)
 {
   Eigen::Map<const Eigen::Vector3f> point_a_normal = point_a.getNormalVector3fMap (), point_b_normal = point_b.getNormalVector3fMap ();
   if (std::abs (point_a.intensity - point_b.intensity) < 5.0f)
     return (true);
-  if (std::abs (point_a_normal.dot (point_b_normal)) < 0.05)
+  if (std::abs (point_a_normal.dot (point_b_normal)) > std::cos (30.0f / 180.0f * static_cast<float> (M_PI)))
     return (true);
   return (false);
 }
@@ -49,7 +49,7 @@ customRegionGrowing (const PointTypeFull& point_a, const PointTypeFull& point_b,
 }
 
 int
-main (int argc, char** argv)
+main ()
 {
   // Data containers used
   pcl::PointCloud<PointTypeIO>::Ptr cloud_in (new pcl::PointCloud<PointTypeIO>), cloud_out (new pcl::PointCloud<PointTypeIO>);
@@ -95,17 +95,17 @@ main (int argc, char** argv)
   std::cerr << ">> Done: " << tt.toc () << " ms\n";
 
   // Using the intensity channel for lazy visualization of the output
-  for (int i = 0; i < small_clusters->size (); ++i)
-    for (int j = 0; j < (*small_clusters)[i].indices.size (); ++j)
-      (*cloud_out)[(*small_clusters)[i].indices[j]].intensity = -2.0;
-  for (int i = 0; i < large_clusters->size (); ++i)
-    for (int j = 0; j < (*large_clusters)[i].indices.size (); ++j)
-      (*cloud_out)[(*large_clusters)[i].indices[j]].intensity = +10.0;
-  for (int i = 0; i < clusters->size (); ++i)
+  for (const auto& small_cluster : (*small_clusters))
+    for (const auto& j : small_cluster.indices)
+      (*cloud_out)[j].intensity = -2.0;
+  for (const auto& large_cluster : (*large_clusters))
+    for (const auto& j : large_cluster.indices)
+      (*cloud_out)[j].intensity = +10.0;
+  for (const auto& cluster : (*clusters))
   {
     int label = rand () % 8;
-    for (int j = 0; j < (*clusters)[i].indices.size (); ++j)
-      (*cloud_out)[(*clusters)[i].indices[j]].intensity = label;
+    for (const auto& j : cluster.indices)
+      (*cloud_out)[j].intensity = label;
   }
 
   // Save the output point cloud
diff --git a/doc/tutorials/content/sources/conditional_removal/CMakeLists.txt b/doc/tutorials/content/sources/conditional_removal/CMakeLists.txt
index f1f78e0c..a861eb3b 100644
--- a/doc/tutorials/content/sources/conditional_removal/CMakeLists.txt
+++ b/doc/tutorials/content/sources/conditional_removal/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(conditional_removal)
 
diff --git a/doc/tutorials/content/sources/conditional_removal/conditional_removal.cpp b/doc/tutorials/content/sources/conditional_removal/conditional_removal.cpp
index 7eb6dc35..9b2a81c6 100644
--- a/doc/tutorials/content/sources/conditional_removal/conditional_removal.cpp
+++ b/doc/tutorials/content/sources/conditional_removal/conditional_removal.cpp
@@ -3,7 +3,7 @@
 #include <pcl/filters/conditional_removal.h>
 
 int
- main (int argc, char** argv)
+ main ()
 {
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
diff --git a/doc/tutorials/content/sources/convex_hull_2d/CMakeLists.txt b/doc/tutorials/content/sources/convex_hull_2d/CMakeLists.txt
index cf149d85..df4059b3 100644
--- a/doc/tutorials/content/sources/convex_hull_2d/CMakeLists.txt
+++ b/doc/tutorials/content/sources/convex_hull_2d/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(convex_hull_2d)
 
diff --git a/doc/tutorials/content/sources/convex_hull_2d/convex_hull_2d.cpp b/doc/tutorials/content/sources/convex_hull_2d/convex_hull_2d.cpp
index ca9af729..e12c7d19 100644
--- a/doc/tutorials/content/sources/convex_hull_2d/convex_hull_2d.cpp
+++ b/doc/tutorials/content/sources/convex_hull_2d/convex_hull_2d.cpp
@@ -9,13 +9,13 @@
 #include <pcl/surface/convex_hull.h>
 
 int
- main (int argc, char** argv)
+ main ()
 {
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_projected (new pcl::PointCloud<pcl::PointXYZ>);
   pcl::PCDReader reader;
   reader.read ("table_scene_mug_stereo_textured.pcd", *cloud);
 
-  // Build a filter to remove spurious NaNs
+  // Build a filter to remove spurious NaNs and scene background
   pcl::PassThrough<pcl::PointXYZ> pass;
   pass.setInputCloud (cloud);
   pass.setFilterFieldName ("z");
diff --git a/doc/tutorials/content/sources/cylinder_segmentation/CMakeLists.txt b/doc/tutorials/content/sources/cylinder_segmentation/CMakeLists.txt
index 4ef87ab1..4c3bf827 100644
--- a/doc/tutorials/content/sources/cylinder_segmentation/CMakeLists.txt
+++ b/doc/tutorials/content/sources/cylinder_segmentation/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(cylinder_segmentation)
 
diff --git a/doc/tutorials/content/sources/cylinder_segmentation/cylinder_segmentation.cpp b/doc/tutorials/content/sources/cylinder_segmentation/cylinder_segmentation.cpp
index 703d8763..326ebc3a 100644
--- a/doc/tutorials/content/sources/cylinder_segmentation/cylinder_segmentation.cpp
+++ b/doc/tutorials/content/sources/cylinder_segmentation/cylinder_segmentation.cpp
@@ -11,7 +11,7 @@
 typedef pcl::PointXYZ PointT;
 
 int
-main (int argc, char** argv)
+main ()
 {
   // All the objects needed
   pcl::PCDReader reader;
@@ -36,7 +36,7 @@ main (int argc, char** argv)
   reader.read ("table_scene_mug_stereo_textured.pcd", *cloud);
   std::cerr << "PointCloud has: " << cloud->size () << " data points." << std::endl;
 
-  // Build a passthrough filter to remove spurious NaNs
+  // Build a passthrough filter to remove spurious NaNs and scene background
   pass.setInputCloud (cloud);
   pass.setFilterFieldName ("z");
   pass.setFilterLimits (0, 1.5);
diff --git a/doc/tutorials/content/sources/davidsdk/CMakeLists.txt b/doc/tutorials/content/sources/davidsdk/CMakeLists.txt
index 2336dcb2..247cec65 100644
--- a/doc/tutorials/content/sources/davidsdk/CMakeLists.txt
+++ b/doc/tutorials/content/sources/davidsdk/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(pcl-davidSDK_images_viewer)
 
diff --git a/doc/tutorials/content/sources/don_segmentation/CMakeLists.txt b/doc/tutorials/content/sources/don_segmentation/CMakeLists.txt
index e2f1310b..3686b809 100644
--- a/doc/tutorials/content/sources/don_segmentation/CMakeLists.txt
+++ b/doc/tutorials/content/sources/don_segmentation/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(don_segmentation)
 
diff --git a/doc/tutorials/content/sources/don_segmentation/don_segmentation.cpp b/doc/tutorials/content/sources/don_segmentation/don_segmentation.cpp
index 7e6b4972..7c9bc869 100644
--- a/doc/tutorials/content/sources/don_segmentation/don_segmentation.cpp
+++ b/doc/tutorials/content/sources/don_segmentation/don_segmentation.cpp
@@ -171,9 +171,9 @@ main (int argc, char *argv[])
   for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it, j++)
   {
     pcl::PointCloud<PointNormal>::Ptr cloud_cluster_don (new pcl::PointCloud<PointNormal>);
-    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
+    for (const auto& idx : it->indices)
     {
-      cloud_cluster_don->points.push_back ((*doncloud)[*pit]);
+      cloud_cluster_don->points.push_back ((*doncloud)[idx]);
     }
 
     cloud_cluster_don->width = cloud_cluster_don->size ();
diff --git a/doc/tutorials/content/sources/ensenso_cameras/CMakeLists.txt b/doc/tutorials/content/sources/ensenso_cameras/CMakeLists.txt
index e510ea86..c07d6f87 100644
--- a/doc/tutorials/content/sources/ensenso_cameras/CMakeLists.txt
+++ b/doc/tutorials/content/sources/ensenso_cameras/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(pcl-ensenso_cloud_images_viewer)
 
diff --git a/doc/tutorials/content/sources/extract_indices/CMakeLists.txt b/doc/tutorials/content/sources/extract_indices/CMakeLists.txt
index 6a406a91..b9df2f73 100644
--- a/doc/tutorials/content/sources/extract_indices/CMakeLists.txt
+++ b/doc/tutorials/content/sources/extract_indices/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(extract_indices)
 
diff --git a/doc/tutorials/content/sources/extract_indices/extract_indices.cpp b/doc/tutorials/content/sources/extract_indices/extract_indices.cpp
index 993f31b4..ee4bc6be 100644
--- a/doc/tutorials/content/sources/extract_indices/extract_indices.cpp
+++ b/doc/tutorials/content/sources/extract_indices/extract_indices.cpp
@@ -9,7 +9,7 @@
 #include <pcl/filters/extract_indices.h>
 
 int
-main (int argc, char** argv)
+main ()
 {
   pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2), cloud_filtered_blob (new pcl::PCLPointCloud2);
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
diff --git a/doc/tutorials/content/sources/function_filter/CMakeLists.txt b/doc/tutorials/content/sources/function_filter/CMakeLists.txt
new file mode 100644
index 00000000..0e8e3d0f
--- /dev/null
+++ b/doc/tutorials/content/sources/function_filter/CMakeLists.txt
@@ -0,0 +1,9 @@
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
+
+project(function_filter)
+
+find_package(PCL 1.11.1.99 REQUIRED)
+
+add_executable (sphere_removal sphere_removal.cpp)
+target_link_libraries (sphere_removal ${PCL_LIBRARIES})
+add_definitions(${PCL_DEFINITIONS})
diff --git a/doc/tutorials/content/sources/function_filter/sphere_removal.cpp b/doc/tutorials/content/sources/function_filter/sphere_removal.cpp
new file mode 100644
index 00000000..1362c80d
--- /dev/null
+++ b/doc/tutorials/content/sources/function_filter/sphere_removal.cpp
@@ -0,0 +1,45 @@
+#include <pcl/common/generate.h>
+#include <pcl/filters/experimental/functor_filter.h>
+#include <pcl/point_types.h>
+
+#include <iostream>
+
+int
+main()
+{
+  using XYZCloud = pcl::PointCloud<pcl::PointXYZ>;
+  const auto cloud = pcl::make_shared<XYZCloud>();
+  const auto filtered_cloud = pcl::make_shared<XYZCloud>();
+
+  // Create a random generator to fill in the cloud
+  pcl::common::CloudGenerator<pcl::PointXYZ, pcl::common::UniformGenerator<float>>
+      generator{{-2.0, 2, 1234}};
+  generator.fill(10, 1, *cloud);
+
+  std::cerr << "Cloud before filtering: " << std::endl;
+  for (const auto& pt : *cloud)
+    std::cerr << "    " << pt.x << " " << pt.y << " " << pt.z << std::endl;
+
+  // Setup a condition to reject points inside a filter
+  const Eigen::Vector3f center{0, 0, 2};
+  const float radius = 2;
+
+  pcl::experimental::FilterFunction<pcl::PointXYZ> filter;
+  filter = [=](const XYZCloud& cloud, pcl::index_t idx) {
+    return ((cloud[idx].getVector3fMap() - center).norm() >= radius);
+  };
+
+  // build the filter
+  pcl::experimental::FunctionFilter<pcl::PointXYZ> func_filter(filter);
+  func_filter.setInputCloud(cloud);
+
+  // apply filter
+  func_filter.filter(*filtered_cloud);
+
+  // display pointcloud after filtering
+  std::cerr << "Cloud after filtering: " << std::endl;
+  for (const auto& pt : *filtered_cloud)
+    std::cerr << "    " << pt.x << " " << pt.y << " " << pt.z << std::endl;
+
+  return (0);
+}
diff --git a/doc/tutorials/content/sources/global_hypothesis_verification/global_hypothesis_verification.cpp b/doc/tutorials/content/sources/global_hypothesis_verification/global_hypothesis_verification.cpp
index 26225bf7..293a9b10 100644
--- a/doc/tutorials/content/sources/global_hypothesis_verification/global_hypothesis_verification.cpp
+++ b/doc/tutorials/content/sources/global_hypothesis_verification/global_hypothesis_verification.cpp
@@ -451,7 +451,7 @@ main (int argc,
   GoHv.verify ();
   GoHv.getMask (hypotheses_mask);  // i-element TRUE if hvModels[i] verifies hypotheses
 
-  for (int i = 0; i < hypotheses_mask.size (); i++)
+  for (std::size_t i = 0; i < hypotheses_mask.size (); i++)
   {
     if (hypotheses_mask[i])
     {
diff --git a/doc/tutorials/content/sources/gpu/people_detect/CMakeLists.txt b/doc/tutorials/content/sources/gpu/people_detect/CMakeLists.txt
index 9062ce87..2b9a2447 100644
--- a/doc/tutorials/content/sources/gpu/people_detect/CMakeLists.txt
+++ b/doc/tutorials/content/sources/gpu/people_detect/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(people_detect)
 
diff --git a/doc/tutorials/content/sources/gpu/people_detect/src/people_detect.cpp b/doc/tutorials/content/sources/gpu/people_detect/src/people_detect.cpp
index 068c7ecf..025ed22a 100644
--- a/doc/tutorials/content/sources/gpu/people_detect/src/people_detect.cpp
+++ b/doc/tutorials/content/sources/gpu/people_detect/src/people_detect.cpp
@@ -124,13 +124,13 @@ class PeoplePCDApp
       depth_view_.setPosition (650, 0);
 
       cmap_device_.create(ROWS, COLS);
-      cmap_host_.points.resize(COLS * ROWS);
+      cmap_host_.resize(COLS * ROWS);
       depth_device_.create(ROWS, COLS);
       image_device_.create(ROWS, COLS);
 
-      depth_host_.points.resize(COLS * ROWS);
+      depth_host_.resize(COLS * ROWS);
 
-      rgba_host_.points.resize(COLS * ROWS);
+      rgba_host_.resize(COLS * ROWS);
       rgb_host_.resize(COLS * ROWS * 3);
 
       people::uploadColorMap(color_map_);
@@ -146,7 +146,7 @@ class PeoplePCDApp
       int c;
       cmap_host_.width = cmap_device_.cols();
       cmap_host_.height = cmap_device_.rows();
-      cmap_host_.points.resize(cmap_host_.width * cmap_host_.height);
+      cmap_host_.resize(cmap_host_.width * cmap_host_.height);
       cmap_device_.download(cmap_host_.points, c);
 
       final_view_.showRGBImage<pcl::RGB>(cmap_host_);
@@ -156,7 +156,7 @@ class PeoplePCDApp
       {
         depth_host_.width = people_detector_.depth_device1_.cols();
         depth_host_.height = people_detector_.depth_device1_.rows();
-        depth_host_.points.resize(depth_host_.width * depth_host_.height);
+        depth_host_.resize(depth_host_.width * depth_host_.height);
         people_detector_.depth_device1_.download(depth_host_.points, c);
       }
 
@@ -203,7 +203,7 @@ class PeoplePCDApp
         const unsigned short *data = depth_wrapper->getDepthMetaData().Data();
         depth_device_.upload(data, s, h, w);
 
-        depth_host_.points.resize(w *h);
+        depth_host_.resize(w *h);
         depth_host_.width = w;
         depth_host_.height = h;
         std::copy(data, data + w * h, &depth_host_[0]);
@@ -218,7 +218,7 @@ class PeoplePCDApp
         image_wrapper->fillRGB(w, h, (unsigned char*)&rgb_host_[0]);
 
         // convert to rgba, TODO image_wrapper should be updated to support rgba directly
-        rgba_host_.points.resize(w * h);
+        rgba_host_.resize(w * h);
         rgba_host_.width = w;
         rgba_host_.height = h;
         for(int i = 0; i < rgba_host_.size(); ++i)
diff --git a/doc/tutorials/content/sources/greedy_projection/CMakeLists.txt b/doc/tutorials/content/sources/greedy_projection/CMakeLists.txt
index d2689fb1..838a7fa0 100644
--- a/doc/tutorials/content/sources/greedy_projection/CMakeLists.txt
+++ b/doc/tutorials/content/sources/greedy_projection/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(greedy_projection)
 
diff --git a/doc/tutorials/content/sources/greedy_projection/greedy_projection.cpp b/doc/tutorials/content/sources/greedy_projection/greedy_projection.cpp
index f783ff00..a0403c61 100644
--- a/doc/tutorials/content/sources/greedy_projection/greedy_projection.cpp
+++ b/doc/tutorials/content/sources/greedy_projection/greedy_projection.cpp
@@ -1,11 +1,11 @@
 #include <pcl/point_types.h>
 #include <pcl/io/pcd_io.h>
-#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/search/kdtree.h> // for KdTree
 #include <pcl/features/normal_3d.h>
 #include <pcl/surface/gp3.h>
 
 int
-main (int argc, char** argv)
+main ()
 {
   // Load input file into a PointCloud<T> with an appropriate type
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
diff --git a/doc/tutorials/content/sources/ground_based_rgbd_people_detection/CMakeLists.txt b/doc/tutorials/content/sources/ground_based_rgbd_people_detection/CMakeLists.txt
index 175f6452..277aa610 100644
--- a/doc/tutorials/content/sources/ground_based_rgbd_people_detection/CMakeLists.txt
+++ b/doc/tutorials/content/sources/ground_based_rgbd_people_detection/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 project(ground_based_rgbd_people_detector)
 find_package(PCL 1.7 REQUIRED)
 
diff --git a/doc/tutorials/content/sources/iccv2011/CMakeLists.txt b/doc/tutorials/content/sources/iccv2011/CMakeLists.txt
index a800af08..0d9251dd 100644
--- a/doc/tutorials/content/sources/iccv2011/CMakeLists.txt
+++ b/doc/tutorials/content/sources/iccv2011/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(iccv11)
 
diff --git a/doc/tutorials/content/sources/iccv2011/include/feature_estimation.h b/doc/tutorials/content/sources/iccv2011/include/feature_estimation.h
index 04b9ed3e..92d7d84c 100644
--- a/doc/tutorials/content/sources/iccv2011/include/feature_estimation.h
+++ b/doc/tutorials/content/sources/iccv2011/include/feature_estimation.h
@@ -46,7 +46,7 @@ estimateSurfaceNormals (const PointCloudPtr & input, float radius)
  * Return: A pointer to a point cloud of keypoints
  */
 PointCloudPtr
-detectKeypoints (const PointCloudPtr & points, const SurfaceNormalsPtr & normals,
+detectKeypoints (const PointCloudPtr & points, const SurfaceNormalsPtr & /*normals*/,
                  float min_scale, int nr_octaves, int nr_scales_per_octave, float min_contrast)
 {
   pcl::SIFTKeypoint<PointT, pcl::PointWithScale> sift_detect;
diff --git a/doc/tutorials/content/sources/iccv2011/include/registration.h b/doc/tutorials/content/sources/iccv2011/include/registration.h
index b209ed8e..c5258d05 100644
--- a/doc/tutorials/content/sources/iccv2011/include/registration.h
+++ b/doc/tutorials/content/sources/iccv2011/include/registration.h
@@ -20,7 +20,7 @@
  *     The minimum distance between any two random samples
  *   max_correspondence_distance
  *     The 
- *   nr_interations
+ *   nr_iterations
  *     The number of RANSAC iterations to perform
  * Return: A transformation matrix that will roughly align the points in source to the points in target
  */
@@ -53,7 +53,7 @@ computeInitialAlignment (const PointCloudPtr & source_points, const LocalDescrip
  *     The "source" points, i.e., the points that must be transformed to align with the target point cloud
  *   target_points
  *     The "target" points, i.e., the points to which the source point cloud will be aligned
- *   intial_alignment
+ *   initial_alignment
  *     An initial estimate of the transformation matrix that aligns the source points to the target points
  *   max_correspondence_distance
  *     A threshold on the distance between any two corresponding points.  Any corresponding points that are further 
diff --git a/doc/tutorials/content/sources/iccv2011/src/build_all_object_models.cpp b/doc/tutorials/content/sources/iccv2011/src/build_all_object_models.cpp
index e660b2cb..f82bbf2b 100644
--- a/doc/tutorials/content/sources/iccv2011/src/build_all_object_models.cpp
+++ b/doc/tutorials/content/sources/iccv2011/src/build_all_object_models.cpp
@@ -7,6 +7,7 @@
 #include <pcl/io/pcd_io.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <boost/filesystem.hpp>
+#include <boost/algorithm/string.hpp> // for split, is_any_of
 namespace bf = boost::filesystem;
 
 inline void
@@ -90,7 +91,11 @@ main (int argc, char ** argv)
 
   //Parse filter parameters
   std::string filter_parameters_file;
-  pcl::console::parse_argument (argc, argv, "--filter", filter_parameters_file) > 0;
+  if (pcl::console::parse_argument (argc, argv, "--filter", filter_parameters_file) < 0)
+  {
+    pcl::console::print_error ("Missing option --filter\n");
+    return (1);
+  }
   params_stream.open (filter_parameters_file.c_str ());
   if (params_stream.is_open())
   {
@@ -109,7 +114,11 @@ main (int argc, char ** argv)
 
   // Parse segmentation parameters
   std::string segmentation_parameters_file;
-  pcl::console::parse_argument (argc, argv, "--segment", segmentation_parameters_file) > 0;
+  if (pcl::console::parse_argument (argc, argv, "--segment", segmentation_parameters_file) < 0)
+  {
+    pcl::console::print_error ("Missing option --segment\n");
+    return (1);
+  }
   params_stream.open (segmentation_parameters_file.c_str ());
   if (params_stream.is_open())
   {
@@ -129,7 +138,11 @@ main (int argc, char ** argv)
 
   // Parse feature estimation parameters
   std::string feature_estimation_parameters_file;
-  pcl::console::parse_argument (argc, argv, "--feature", feature_estimation_parameters_file) > 0;
+  if (pcl::console::parse_argument (argc, argv, "--feature", feature_estimation_parameters_file) < 0)
+  {
+    pcl::console::print_error ("Missing option --feature\n");
+    return (1);
+  }
   params_stream.open (feature_estimation_parameters_file.c_str ());
   if (params_stream.is_open())
   {
@@ -150,7 +163,11 @@ main (int argc, char ** argv)
 
   // Parse the registration parameters
   std::string registration_parameters_file;
-  pcl::console::parse_argument (argc, argv, "--registration", registration_parameters_file) > 0;
+  if (pcl::console::parse_argument (argc, argv, "--registration", registration_parameters_file) < 0)
+  {
+    pcl::console::print_error ("Missing option --registration\n");
+    return (1);
+  }
   params_stream.open (registration_parameters_file.c_str ());
   if (params_stream.is_open())
   {
diff --git a/doc/tutorials/content/sources/iccv2011/src/build_object_model.cpp b/doc/tutorials/content/sources/iccv2011/src/build_object_model.cpp
index ea946530..deaf2760 100644
--- a/doc/tutorials/content/sources/iccv2011/src/build_object_model.cpp
+++ b/doc/tutorials/content/sources/iccv2011/src/build_object_model.cpp
@@ -66,7 +66,11 @@ main (int argc, char ** argv)
 
   //Parse filter parameters
   std::string filter_parameters_file;
-  pcl::console::parse_argument (argc, argv, "--filter", filter_parameters_file) > 0;    
+  if (pcl::console::parse_argument (argc, argv, "--filter", filter_parameters_file) < 0)
+  {
+    pcl::console::print_error ("Missing option --filter\n");
+    return (1);
+  }
   params_stream.open (filter_parameters_file.c_str ());
   if (params_stream.is_open())
   {
@@ -85,7 +89,11 @@ main (int argc, char ** argv)
   
   // Parse segmentation parameters
   std::string segmentation_parameters_file;
-  pcl::console::parse_argument (argc, argv, "--segment", segmentation_parameters_file) > 0;    
+  if (pcl::console::parse_argument (argc, argv, "--segment", segmentation_parameters_file) < 0)
+  {
+    pcl::console::print_error ("Missing option --segment\n");
+    return (1);
+  }
   params_stream.open (segmentation_parameters_file.c_str ());
   if (params_stream.is_open())
   {
@@ -105,7 +113,11 @@ main (int argc, char ** argv)
 
   // Parse feature estimation parameters
   std::string feature_estimation_parameters_file;
-  pcl::console::parse_argument (argc, argv, "--feature", feature_estimation_parameters_file) > 0;    
+  if (pcl::console::parse_argument (argc, argv, "--feature", feature_estimation_parameters_file) < 0)
+  {
+    pcl::console::print_error ("Missing option --feature\n");
+    return (1);
+  }
   params_stream.open (feature_estimation_parameters_file.c_str ());
   if (params_stream.is_open())
   {
@@ -126,7 +138,11 @@ main (int argc, char ** argv)
 
   // Parse the registration parameters
   std::string registration_parameters_file;
-  pcl::console::parse_argument (argc, argv, "--registration", registration_parameters_file) > 0;    
+  if (pcl::console::parse_argument (argc, argv, "--registration", registration_parameters_file) < 0)
+  {
+    pcl::console::print_error ("Missing option --registration\n");
+    return (1);
+  }
   params_stream.open (registration_parameters_file.c_str ());
   if (params_stream.is_open())
   {
diff --git a/doc/tutorials/content/sources/iccv2011/src/correspondence_viewer.cpp b/doc/tutorials/content/sources/iccv2011/src/correspondence_viewer.cpp
index 40297ccc..3ba7056d 100644
--- a/doc/tutorials/content/sources/iccv2011/src/correspondence_viewer.cpp
+++ b/doc/tutorials/content/sources/iccv2011/src/correspondence_viewer.cpp
@@ -71,7 +71,7 @@ void
 visualize_correspondences (const PointCloudPtr points1, const PointCloudPtr keypoints1,
                            const PointCloudPtr points2, const PointCloudPtr keypoints2,
                            const std::vector<int> &correspondences,
-                           const std::vector<float> &correspondence_scores, int max_to_display)
+                           const std::vector<float> &correspondence_scores, std::size_t max_to_display)
 { 
   // We want to visualize two clouds side-by-side, so do to this, we'll make copies of the clouds and transform them
   // by shifting one to the left and the other to the right.  Then we'll draw lines between the corresponding points
diff --git a/doc/tutorials/content/sources/iccv2011/src/openni_capture.cpp b/doc/tutorials/content/sources/iccv2011/src/openni_capture.cpp
index 1f1c5073..7266fde4 100644
--- a/doc/tutorials/content/sources/iccv2011/src/openni_capture.cpp
+++ b/doc/tutorials/content/sources/iccv2011/src/openni_capture.cpp
@@ -7,8 +7,8 @@
 
 OpenNICapture::OpenNICapture (const std::string& device_id)
   : grabber_ (device_id)
-  , most_recent_frame_ ()
   , frame_counter_ (0)
+  , most_recent_frame_ ()
   , use_trigger_ (false)
   , trigger_ (false)
 {
diff --git a/doc/tutorials/content/sources/iccv2011/src/test_feature_estimation.cpp b/doc/tutorials/content/sources/iccv2011/src/test_feature_estimation.cpp
index 0d98d57f..d3ceb678 100644
--- a/doc/tutorials/content/sources/iccv2011/src/test_feature_estimation.cpp
+++ b/doc/tutorials/content/sources/iccv2011/src/test_feature_estimation.cpp
@@ -6,6 +6,7 @@
 #include <pcl/io/pcd_io.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/visualization/histogram_visualizer.h>
+#include <boost/algorithm/string/split.hpp> // for split
 
 int 
 main (int argc, char ** argv)
diff --git a/doc/tutorials/content/sources/iccv2011/src/test_object_recognition.cpp b/doc/tutorials/content/sources/iccv2011/src/test_object_recognition.cpp
index d9ebb473..bd363d2b 100644
--- a/doc/tutorials/content/sources/iccv2011/src/test_object_recognition.cpp
+++ b/doc/tutorials/content/sources/iccv2011/src/test_object_recognition.cpp
@@ -86,7 +86,11 @@ main (int argc, char ** argv)
   
   //Parse filter parameters
   std::string filter_parameters_file;
-  pcl::console::parse_argument (argc, argv, "--filter", filter_parameters_file) > 0;    
+  if (pcl::console::parse_argument (argc, argv, "--filter", filter_parameters_file) < 0)
+  {
+    pcl::console::print_error ("Missing option --filter\n");
+    return (1);
+  }
   input_stream.open (filter_parameters_file.c_str ());
   if (input_stream.is_open())
   {
@@ -105,7 +109,11 @@ main (int argc, char ** argv)
   
   // Parse segmentation parameters
   std::string segmentation_parameters_file;
-  pcl::console::parse_argument (argc, argv, "--segment", segmentation_parameters_file) > 0;    
+  if (pcl::console::parse_argument (argc, argv, "--segment", segmentation_parameters_file) < 0)
+  {
+    pcl::console::print_error ("Missing option --segment\n");
+    return (1);
+  }
   input_stream.open (segmentation_parameters_file.c_str ());
   if (input_stream.is_open())
   {
@@ -125,7 +133,11 @@ main (int argc, char ** argv)
 
   // Parse feature estimation parameters
   std::string feature_estimation_parameters_file;
-  pcl::console::parse_argument (argc, argv, "--feature", feature_estimation_parameters_file) > 0;    
+  if (pcl::console::parse_argument (argc, argv, "--feature", feature_estimation_parameters_file) < 0)
+  {
+    pcl::console::print_error ("Missing option --feature\n");
+    return (1);
+  }
   input_stream.open (feature_estimation_parameters_file.c_str ());
   if (input_stream.is_open())
   {
@@ -146,7 +158,11 @@ main (int argc, char ** argv)
 
   // Parse the registration parameters
   std::string registration_parameters_file;
-  pcl::console::parse_argument (argc, argv, "--registration", registration_parameters_file) > 0;    
+  if (pcl::console::parse_argument (argc, argv, "--registration", registration_parameters_file) < 0)
+  {
+    pcl::console::print_error ("Missing option --registration\n");
+    return (1);
+  }
   input_stream.open (registration_parameters_file.c_str ());
   if (input_stream.is_open())
   {
diff --git a/doc/tutorials/content/sources/iccv2011/src/test_registration.cpp b/doc/tutorials/content/sources/iccv2011/src/test_registration.cpp
index 33fb8c38..59348ce4 100644
--- a/doc/tutorials/content/sources/iccv2011/src/test_registration.cpp
+++ b/doc/tutorials/content/sources/iccv2011/src/test_registration.cpp
@@ -5,7 +5,7 @@
 #include <pcl/console/parse.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/visualization/pcl_visualizer.h>
-
+#include <boost/algorithm/string/split.hpp> // for split
 #include "load_clouds.h"
 
 int 
diff --git a/doc/tutorials/content/sources/iccv2011/src/test_surface.cpp b/doc/tutorials/content/sources/iccv2011/src/test_surface.cpp
index b704f655..d22ca6ab 100644
--- a/doc/tutorials/content/sources/iccv2011/src/test_surface.cpp
+++ b/doc/tutorials/content/sources/iccv2011/src/test_surface.cpp
@@ -4,6 +4,7 @@
 #include <pcl/console/parse.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/visualization/pcl_visualizer.h>
+#include <boost/algorithm/string/split.hpp> // for split
 
 int 
 main (int argc, char ** argv)
diff --git a/doc/tutorials/content/sources/iccv2011/src/tutorial.cpp b/doc/tutorials/content/sources/iccv2011/src/tutorial.cpp
index 3ae0aa65..f5f8a774 100644
--- a/doc/tutorials/content/sources/iccv2011/src/tutorial.cpp
+++ b/doc/tutorials/content/sources/iccv2011/src/tutorial.cpp
@@ -26,6 +26,7 @@
 #include <pcl/features/pfhrgb.h>
 #include <pcl/features/3dsc.h>
 #include <pcl/features/shot_omp.h>
+#include <pcl/filters/extract_indices.h> // for ExtractIndices
 #include <pcl/kdtree/kdtree_flann.h>
 #include <pcl/kdtree/impl/kdtree_flann.hpp>
 #include <pcl/registration/transformation_estimation_svd.h>
@@ -309,7 +310,7 @@ void ICCVTutorial<FeatureType>::filterCorrespondences ()
   std::cout << "correspondence rejection..." << std::flush;
   std::vector<std::pair<unsigned, unsigned> > correspondences;
   for (unsigned cIdx = 0; cIdx < source2target_.size (); ++cIdx)
-    if (target2source_[source2target_[cIdx]] == cIdx)
+    if (static_cast<unsigned int>(target2source_[source2target_[cIdx]]) == cIdx)
       correspondences.push_back(std::make_pair(cIdx, source2target_[cIdx]));
 
   correspondences_->resize (correspondences.size());
@@ -397,7 +398,7 @@ void ICCVTutorial<FeatureType>::run()
 }
 
 template<typename FeatureType>
-void ICCVTutorial<FeatureType>::keyboard_callback (const pcl::visualization::KeyboardEvent& event, void* cookie)
+void ICCVTutorial<FeatureType>::keyboard_callback (const pcl::visualization::KeyboardEvent& event, void* /*cookie*/)
 {
   if (event.keyUp())
   {
diff --git a/doc/tutorials/content/sources/implicit_shape_model/CMakeLists.txt b/doc/tutorials/content/sources/implicit_shape_model/CMakeLists.txt
index 9f85b0a1..940e8ef1 100644
--- a/doc/tutorials/content/sources/implicit_shape_model/CMakeLists.txt
+++ b/doc/tutorials/content/sources/implicit_shape_model/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(implicit_shape_model)
 
diff --git a/doc/tutorials/content/sources/interactive_icp/interactive_icp.cpp b/doc/tutorials/content/sources/interactive_icp/interactive_icp.cpp
index d0351ae0..dfb6cbfb 100644
--- a/doc/tutorials/content/sources/interactive_icp/interactive_icp.cpp
+++ b/doc/tutorials/content/sources/interactive_icp/interactive_icp.cpp
@@ -25,7 +25,7 @@ print4x4Matrix (const Eigen::Matrix4d & matrix)
 
 void
 keyboardEventOccurred (const pcl::visualization::KeyboardEvent& event,
-                       void* nothing)
+                       void*)
 {
   if (event.getKeySym () == "space" && event.keyDown ())
     next_iteration = true;
diff --git a/doc/tutorials/content/sources/iros2011/CMakeLists.txt b/doc/tutorials/content/sources/iros2011/CMakeLists.txt
index 6698ad68..c7bc9d58 100644
--- a/doc/tutorials/content/sources/iros2011/CMakeLists.txt
+++ b/doc/tutorials/content/sources/iros2011/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(iros11)
 
diff --git a/doc/tutorials/content/sources/iros2011/include/object_recognition.h b/doc/tutorials/content/sources/iros2011/include/object_recognition.h
index e403efc5..ed9080c2 100644
--- a/doc/tutorials/content/sources/iros2011/include/object_recognition.h
+++ b/doc/tutorials/content/sources/iros2011/include/object_recognition.h
@@ -59,19 +59,19 @@ public:
   {}
 
   void 
-  populateDatabase (const std::vector<std::string> & filenames)
+  populateDatabase (const std::vector<std::string> & /*filenames*/)
   {
   } 
 
   const ObjectModel & 
-  recognizeObject (const PointCloudPtr & query_cloud)
+  recognizeObject (const PointCloudPtr & /*query_cloud*/)
   {
     int best_match = 0;
     return (models_[best_match]);
   }
 
   PointCloudPtr
-  recognizeAndAlignPoints (const PointCloudPtr & query_cloud)
+  recognizeAndAlignPoints (const PointCloudPtr & /*query_cloud*/)
   {
     PointCloudPtr output;
     return (output);
diff --git a/doc/tutorials/content/sources/iros2011/include/registration.h b/doc/tutorials/content/sources/iros2011/include/registration.h
index bbe5d002..4d4e08ab 100644
--- a/doc/tutorials/content/sources/iros2011/include/registration.h
+++ b/doc/tutorials/content/sources/iros2011/include/registration.h
@@ -19,7 +19,7 @@
  *     The minimum distance between any two random samples
  *   max_correspondence_distance
  *     The 
- *   nr_interations
+ *   nr_iterations
  *     The number of RANSAC iterations to perform
  * Return: A transformation matrix that will roughly align the points in source to the points in target
  */
@@ -39,7 +39,7 @@ computeInitialAlignment (const PointCloudPtr & source_points, const LocalDescrip
  *     The "source" points, i.e., the points that must be transformed to align with the target point cloud
  *   target_points
  *     The "target" points, i.e., the points to which the source point cloud will be aligned
- *   intial_alignment
+ *   initial_alignment
  *     An initial estimate of the transformation matrix that aligns the source points to the target points
  *   max_correspondence_distance
  *     A threshold on the distance between any two corresponding points.  Any corresponding points that are further 
diff --git a/doc/tutorials/content/sources/iros2011/include/solution/feature_estimation.h b/doc/tutorials/content/sources/iros2011/include/solution/feature_estimation.h
index 96d11d30..6249aaa2 100644
--- a/doc/tutorials/content/sources/iros2011/include/solution/feature_estimation.h
+++ b/doc/tutorials/content/sources/iros2011/include/solution/feature_estimation.h
@@ -47,7 +47,7 @@ estimateSurfaceNormals (const PointCloudPtr & input, float radius)
  * Return: A pointer to a point cloud of keypoints
  */
 PointCloudPtr
-detectKeypoints (const PointCloudPtr & points, const SurfaceNormalsPtr & normals,
+detectKeypoints (const PointCloudPtr & points, const SurfaceNormalsPtr & /*normals*/,
                  float min_scale, int nr_octaves, int nr_scales_per_octave, float min_contrast)
 {
   pcl::SIFTKeypoint<PointT, pcl::PointWithScale> sift_detect;
diff --git a/doc/tutorials/content/sources/iros2011/include/solution/registration.h b/doc/tutorials/content/sources/iros2011/include/solution/registration.h
index 45c48d48..ab12f9d9 100644
--- a/doc/tutorials/content/sources/iros2011/include/solution/registration.h
+++ b/doc/tutorials/content/sources/iros2011/include/solution/registration.h
@@ -20,7 +20,7 @@
  *     The minimum distance between any two random samples
  *   max_correspondence_distance
  *     The 
- *   nr_interations
+ *   nr_iterations
  *     The number of RANSAC iterations to perform
  * Return: A transformation matrix that will roughly align the points in source to the points in target
  */
@@ -53,7 +53,7 @@ computeInitialAlignment (const PointCloudPtr & source_points, const LocalDescrip
  *     The "source" points, i.e., the points that must be transformed to align with the target point cloud
  *   target_points
  *     The "target" points, i.e., the points to which the source point cloud will be aligned
- *   intial_alignment
+ *   initial_alignment
  *     An initial estimate of the transformation matrix that aligns the source points to the target points
  *   max_correspondence_distance
  *     A threshold on the distance between any two corresponding points.  Any corresponding points that are further 
diff --git a/doc/tutorials/content/sources/iros2011/include/surface.h b/doc/tutorials/content/sources/iros2011/include/surface.h
index 7749faed..189d3d80 100644
--- a/doc/tutorials/content/sources/iros2011/include/surface.h
+++ b/doc/tutorials/content/sources/iros2011/include/surface.h
@@ -21,21 +21,21 @@ public:
 using MeshPtr = std::shared_ptr<Mesh>;
 
 PointCloudPtr
-smoothPointCloud (const PointCloudPtr & input, float radius, int polynomial_order)
+smoothPointCloud (const PointCloudPtr & /*input*/, float /*radius*/, int /*polynomial_order*/)
 {
   PointCloudPtr output (new PointCloud);
   return (output);
 }
 
 SurfaceElementsPtr
-computeSurfaceElements (const PointCloudPtr & input, float radius, int polynomial_order)
+computeSurfaceElements (const PointCloudPtr & /*input*/, float /*radius*/, int /*polynomial_order*/)
 {
   SurfaceElementsPtr surfels (new SurfaceElements);
   return (surfels);
 }
 
 MeshPtr
-computeConvexHull (const PointCloudPtr & input)
+computeConvexHull (const PointCloudPtr & /*input*/)
 {
   MeshPtr output (new Mesh);
   return (output);
@@ -43,15 +43,15 @@ computeConvexHull (const PointCloudPtr & input)
 
 
 MeshPtr
-computeConcaveHull (const PointCloudPtr & input, float alpha)
+computeConcaveHull (const PointCloudPtr & /*input*/, float /*alpha*/)
 {
   MeshPtr output (new Mesh);
   return (output);
 }
 
 pcl::PolygonMesh::Ptr
-greedyTriangulation (const SurfaceElementsPtr & surfels, float radius, float mu, int max_nearest_neighbors, 
-                     float max_surface_angle, float min_angle, float max_angle)
+greedyTriangulation (const SurfaceElementsPtr & /*surfels*/, float /*radius*/, float /*mu*/, int /*max_nearest_neighbors*/, 
+                     float /*max_surface_angle*/, float /*min_angle*/, float /*max_angle*/)
 
 {
   pcl::PolygonMesh::Ptr output (new pcl::PolygonMesh);
@@ -60,7 +60,7 @@ greedyTriangulation (const SurfaceElementsPtr & surfels, float radius, float mu,
 
 
 pcl::PolygonMesh::Ptr
-marchingCubesTriangulation (const SurfaceElementsPtr & surfels, float leaf_size, float iso_level)
+marchingCubesTriangulation (const SurfaceElementsPtr & /*surfels*/, float /*leaf_size*/, float /*iso_level*/)
 {
   pcl::PolygonMesh::Ptr output (new pcl::PolygonMesh);
   return (output);
diff --git a/doc/tutorials/content/sources/iros2011/src/build_all_object_models.cpp b/doc/tutorials/content/sources/iros2011/src/build_all_object_models.cpp
index cb77cdf8..81736542 100644
--- a/doc/tutorials/content/sources/iros2011/src/build_all_object_models.cpp
+++ b/doc/tutorials/content/sources/iros2011/src/build_all_object_models.cpp
@@ -7,6 +7,7 @@
 #include <pcl/io/pcd_io.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <boost/filesystem.hpp>
+#include <boost/algorithm/string.hpp> // for split, is_any_of
 namespace bf = boost::filesystem;
 
 inline void
@@ -90,7 +91,11 @@ main (int argc, char ** argv)
 
   //Parse filter parameters
   std::string filter_parameters_file;
-  pcl::console::parse_argument (argc, argv, "--filter", filter_parameters_file) > 0;
+  if (pcl::console::parse_argument (argc, argv, "--filter", filter_parameters_file) < 0)
+  {
+    pcl::console::print_error ("Missing option --filter\n");
+    return (1);
+  }
   params_stream.open (filter_parameters_file.c_str ());
   if (params_stream.is_open())
   {
@@ -109,7 +114,11 @@ main (int argc, char ** argv)
 
   // Parse segmentation parameters
   std::string segmentation_parameters_file;
-  pcl::console::parse_argument (argc, argv, "--segment", segmentation_parameters_file) > 0;
+  if (pcl::console::parse_argument (argc, argv, "--segment", segmentation_parameters_file) < 0)
+  {
+    pcl::console::print_error ("Missing option --segment\n");
+    return (1);
+  }
   params_stream.open (segmentation_parameters_file.c_str ());
   if (params_stream.is_open())
   {
@@ -129,7 +138,11 @@ main (int argc, char ** argv)
 
   // Parse feature estimation parameters
   std::string feature_estimation_parameters_file;
-  pcl::console::parse_argument (argc, argv, "--feature", feature_estimation_parameters_file) > 0;
+  if (pcl::console::parse_argument (argc, argv, "--feature", feature_estimation_parameters_file) < 0)
+  {
+    pcl::console::print_error ("Missing option --feature\n");
+    return (1);
+  }
   params_stream.open (feature_estimation_parameters_file.c_str ());
   if (params_stream.is_open())
   {
@@ -150,7 +163,11 @@ main (int argc, char ** argv)
 
   // Parse the registration parameters
   std::string registration_parameters_file;
-  pcl::console::parse_argument (argc, argv, "--registration", registration_parameters_file) > 0;
+  if (pcl::console::parse_argument (argc, argv, "--registration", registration_parameters_file) < 0)
+  {
+    pcl::console::print_error ("Missing option --registration\n");
+    return (1);
+  }
   params_stream.open (registration_parameters_file.c_str ());
   if (params_stream.is_open())
   {
diff --git a/doc/tutorials/content/sources/iros2011/src/build_object_model.cpp b/doc/tutorials/content/sources/iros2011/src/build_object_model.cpp
index ea946530..deaf2760 100644
--- a/doc/tutorials/content/sources/iros2011/src/build_object_model.cpp
+++ b/doc/tutorials/content/sources/iros2011/src/build_object_model.cpp
@@ -66,7 +66,11 @@ main (int argc, char ** argv)
 
   //Parse filter parameters
   std::string filter_parameters_file;
-  pcl::console::parse_argument (argc, argv, "--filter", filter_parameters_file) > 0;    
+  if (pcl::console::parse_argument (argc, argv, "--filter", filter_parameters_file) < 0)
+  {
+    pcl::console::print_error ("Missing option --filter\n");
+    return (1);
+  }
   params_stream.open (filter_parameters_file.c_str ());
   if (params_stream.is_open())
   {
@@ -85,7 +89,11 @@ main (int argc, char ** argv)
   
   // Parse segmentation parameters
   std::string segmentation_parameters_file;
-  pcl::console::parse_argument (argc, argv, "--segment", segmentation_parameters_file) > 0;    
+  if (pcl::console::parse_argument (argc, argv, "--segment", segmentation_parameters_file) < 0)
+  {
+    pcl::console::print_error ("Missing option --segment\n");
+    return (1);
+  }
   params_stream.open (segmentation_parameters_file.c_str ());
   if (params_stream.is_open())
   {
@@ -105,7 +113,11 @@ main (int argc, char ** argv)
 
   // Parse feature estimation parameters
   std::string feature_estimation_parameters_file;
-  pcl::console::parse_argument (argc, argv, "--feature", feature_estimation_parameters_file) > 0;    
+  if (pcl::console::parse_argument (argc, argv, "--feature", feature_estimation_parameters_file) < 0)
+  {
+    pcl::console::print_error ("Missing option --feature\n");
+    return (1);
+  }
   params_stream.open (feature_estimation_parameters_file.c_str ());
   if (params_stream.is_open())
   {
@@ -126,7 +138,11 @@ main (int argc, char ** argv)
 
   // Parse the registration parameters
   std::string registration_parameters_file;
-  pcl::console::parse_argument (argc, argv, "--registration", registration_parameters_file) > 0;    
+  if (pcl::console::parse_argument (argc, argv, "--registration", registration_parameters_file) < 0)
+  {
+    pcl::console::print_error ("Missing option --registration\n");
+    return (1);
+  }
   params_stream.open (registration_parameters_file.c_str ());
   if (params_stream.is_open())
   {
diff --git a/doc/tutorials/content/sources/iros2011/src/correspondence_viewer.cpp b/doc/tutorials/content/sources/iros2011/src/correspondence_viewer.cpp
index 40297ccc..3ba7056d 100644
--- a/doc/tutorials/content/sources/iros2011/src/correspondence_viewer.cpp
+++ b/doc/tutorials/content/sources/iros2011/src/correspondence_viewer.cpp
@@ -71,7 +71,7 @@ void
 visualize_correspondences (const PointCloudPtr points1, const PointCloudPtr keypoints1,
                            const PointCloudPtr points2, const PointCloudPtr keypoints2,
                            const std::vector<int> &correspondences,
-                           const std::vector<float> &correspondence_scores, int max_to_display)
+                           const std::vector<float> &correspondence_scores, std::size_t max_to_display)
 { 
   // We want to visualize two clouds side-by-side, so do to this, we'll make copies of the clouds and transform them
   // by shifting one to the left and the other to the right.  Then we'll draw lines between the corresponding points
diff --git a/doc/tutorials/content/sources/iros2011/src/openni_capture.cpp b/doc/tutorials/content/sources/iros2011/src/openni_capture.cpp
index 29ae70d5..36c623c9 100644
--- a/doc/tutorials/content/sources/iros2011/src/openni_capture.cpp
+++ b/doc/tutorials/content/sources/iros2011/src/openni_capture.cpp
@@ -7,11 +7,11 @@
 
 OpenNICapture::OpenNICapture (const std::string& device_id)
   : grabber_ (device_id)
-  , most_recent_frame_ ()
+  , preview_ ()
   , frame_counter_ (0)
+  , most_recent_frame_ ()
   , use_trigger_ (false)
   , trigger_ (false)
-  , preview_ ()
 {
   // Register a callback function to our OpenNI grabber...
   std::function<void (const PointCloudConstPtr&)> frame_cb = [this] (const PointCloudConstPtr& cloud) { onNewFrame (cloud); };
diff --git a/doc/tutorials/content/sources/iros2011/src/test_feature_estimation.cpp b/doc/tutorials/content/sources/iros2011/src/test_feature_estimation.cpp
index 0df4ad0e..3e27e60c 100644
--- a/doc/tutorials/content/sources/iros2011/src/test_feature_estimation.cpp
+++ b/doc/tutorials/content/sources/iros2011/src/test_feature_estimation.cpp
@@ -6,6 +6,7 @@
 #include <pcl/io/pcd_io.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/visualization/histogram_visualizer.h>
+#include <boost/algorithm/string/split.hpp> // for split
 
 int 
 main (int argc, char ** argv)
diff --git a/doc/tutorials/content/sources/iros2011/src/test_object_recognition.cpp b/doc/tutorials/content/sources/iros2011/src/test_object_recognition.cpp
index 251463d5..79f24a2c 100644
--- a/doc/tutorials/content/sources/iros2011/src/test_object_recognition.cpp
+++ b/doc/tutorials/content/sources/iros2011/src/test_object_recognition.cpp
@@ -86,7 +86,11 @@ main (int argc, char ** argv)
   
   //Parse filter parameters
   std::string filter_parameters_file;
-  pcl::console::parse_argument (argc, argv, "--filter", filter_parameters_file) > 0;    
+  if (pcl::console::parse_argument (argc, argv, "--filter", filter_parameters_file) < 0)
+  {
+    pcl::console::print_error ("Missing option --filter\n");
+    return (1);
+  }
   input_stream.open (filter_parameters_file.c_str ());
   if (input_stream.is_open())
   {
@@ -105,7 +109,11 @@ main (int argc, char ** argv)
   
   // Parse segmentation parameters
   std::string segmentation_parameters_file;
-  pcl::console::parse_argument (argc, argv, "--segment", segmentation_parameters_file) > 0;    
+  if (pcl::console::parse_argument (argc, argv, "--segment", segmentation_parameters_file) < 0)
+  {
+    pcl::console::print_error ("Missing option --segment\n");
+    return (1);
+  }
   input_stream.open (segmentation_parameters_file.c_str ());
   if (input_stream.is_open())
   {
@@ -125,7 +133,11 @@ main (int argc, char ** argv)
 
   // Parse feature estimation parameters
   std::string feature_estimation_parameters_file;
-  pcl::console::parse_argument (argc, argv, "--feature", feature_estimation_parameters_file) > 0;    
+  if (pcl::console::parse_argument (argc, argv, "--feature", feature_estimation_parameters_file) < 0)
+  {
+    pcl::console::print_error ("Missing option --feature\n");
+    return (1);
+  }
   input_stream.open (feature_estimation_parameters_file.c_str ());
   if (input_stream.is_open())
   {
@@ -146,7 +158,11 @@ main (int argc, char ** argv)
 
   // Parse the registration parameters
   std::string registration_parameters_file;
-  pcl::console::parse_argument (argc, argv, "--registration", registration_parameters_file) > 0;    
+  if (pcl::console::parse_argument (argc, argv, "--registration", registration_parameters_file) < 0)
+  {
+    pcl::console::print_error ("Missing option --registration\n");
+    return (1);
+  }
   input_stream.open (registration_parameters_file.c_str ());
   if (input_stream.is_open())
   {
diff --git a/doc/tutorials/content/sources/iros2011/src/test_registration.cpp b/doc/tutorials/content/sources/iros2011/src/test_registration.cpp
index a1c8c92a..2fce3439 100644
--- a/doc/tutorials/content/sources/iros2011/src/test_registration.cpp
+++ b/doc/tutorials/content/sources/iros2011/src/test_registration.cpp
@@ -5,7 +5,7 @@
 #include <pcl/console/parse.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/visualization/pcl_visualizer.h>
-
+#include <boost/algorithm/string/split.hpp> // for split
 #include "load_clouds.h"
 
 int 
diff --git a/doc/tutorials/content/sources/iros2011/src/test_surface.cpp b/doc/tutorials/content/sources/iros2011/src/test_surface.cpp
index b704f655..d22ca6ab 100644
--- a/doc/tutorials/content/sources/iros2011/src/test_surface.cpp
+++ b/doc/tutorials/content/sources/iros2011/src/test_surface.cpp
@@ -4,6 +4,7 @@
 #include <pcl/console/parse.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/visualization/pcl_visualizer.h>
+#include <boost/algorithm/string/split.hpp> // for split
 
 int 
 main (int argc, char ** argv)
diff --git a/doc/tutorials/content/sources/iterative_closest_point/CMakeLists.txt b/doc/tutorials/content/sources/iterative_closest_point/CMakeLists.txt
index 97533be9..447c725b 100644
--- a/doc/tutorials/content/sources/iterative_closest_point/CMakeLists.txt
+++ b/doc/tutorials/content/sources/iterative_closest_point/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(iterative_closest_point)
 
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 06cffa96..d45ab78c 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
@@ -4,7 +4,7 @@
 #include <pcl/registration/icp.h>
 
 int
- main (int argc, char** argv)
+ main ()
 {
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>(5,1));
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);
diff --git a/doc/tutorials/content/sources/kdtree_search/CMakeLists.txt b/doc/tutorials/content/sources/kdtree_search/CMakeLists.txt
index 18747864..4ed20c7d 100644
--- a/doc/tutorials/content/sources/kdtree_search/CMakeLists.txt
+++ b/doc/tutorials/content/sources/kdtree_search/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(kdtree_search)
 
diff --git a/doc/tutorials/content/sources/kdtree_search/kdtree_search.cpp b/doc/tutorials/content/sources/kdtree_search/kdtree_search.cpp
index e704c9cc..c53e3f5f 100644
--- a/doc/tutorials/content/sources/kdtree_search/kdtree_search.cpp
+++ b/doc/tutorials/content/sources/kdtree_search/kdtree_search.cpp
@@ -6,7 +6,7 @@
 #include <ctime>
 
 int
-main (int argc, char** argv)
+main ()
 {
   srand (time (NULL));
 
@@ -38,21 +38,21 @@ main (int argc, char** argv)
 
   int K = 10;
 
-  std::vector<int> pointIdxNKNSearch(K);
-  std::vector<float> pointNKNSquaredDistance(K);
+  std::vector<int> pointIdxKNNSearch(K);
+  std::vector<float> pointKNNSquaredDistance(K);
 
   std::cout << "K nearest neighbor search at (" << searchPoint.x 
             << " " << searchPoint.y 
             << " " << searchPoint.z
             << ") with K=" << K << std::endl;
 
-  if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 )
+  if ( kdtree.nearestKSearch (searchPoint, K, pointIdxKNNSearch, pointKNNSquaredDistance) > 0 )
   {
-    for (std::size_t i = 0; i < pointIdxNKNSearch.size (); ++i)
-      std::cout << "    "  <<   (*cloud)[ pointIdxNKNSearch[i] ].x 
-                << " " << (*cloud)[ pointIdxNKNSearch[i] ].y 
-                << " " << (*cloud)[ pointIdxNKNSearch[i] ].z 
-                << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
+    for (std::size_t i = 0; i < pointIdxKNNSearch.size (); ++i)
+      std::cout << "    "  <<   (*cloud)[ pointIdxKNNSearch[i] ].x 
+                << " " << (*cloud)[ pointIdxKNNSearch[i] ].y 
+                << " " << (*cloud)[ pointIdxKNNSearch[i] ].z 
+                << " (squared distance: " << pointKNNSquaredDistance[i] << ")" << std::endl;
   }
 
   // Neighbors within radius search
diff --git a/doc/tutorials/content/sources/min_cut_segmentation/CMakeLists.txt b/doc/tutorials/content/sources/min_cut_segmentation/CMakeLists.txt
index 541dfb27..2759751a 100644
--- a/doc/tutorials/content/sources/min_cut_segmentation/CMakeLists.txt
+++ b/doc/tutorials/content/sources/min_cut_segmentation/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(min_cut_segmentation)
 
diff --git a/doc/tutorials/content/sources/min_cut_segmentation/min_cut_segmentation.cpp b/doc/tutorials/content/sources/min_cut_segmentation/min_cut_segmentation.cpp
index 31ce6979..5e4076ae 100644
--- a/doc/tutorials/content/sources/min_cut_segmentation/min_cut_segmentation.cpp
+++ b/doc/tutorials/content/sources/min_cut_segmentation/min_cut_segmentation.cpp
@@ -3,10 +3,10 @@
 #include <pcl/io/pcd_io.h>
 #include <pcl/point_types.h>
 #include <pcl/visualization/cloud_viewer.h>
-#include <pcl/filters/passthrough.h>
+#include <pcl/filters/filter_indices.h> // for pcl::removeNaNFromPointCloud
 #include <pcl/segmentation/min_cut_segmentation.h>
 
-int main (int argc, char** argv)
+int main ()
 {
   pcl::PointCloud <pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud <pcl::PointXYZ>);
   if ( pcl::io::loadPCDFile <pcl::PointXYZ> ("min_cut_segmentation_tutorial.pcd", *cloud) == -1 )
@@ -16,11 +16,7 @@ int main (int argc, char** argv)
   }
 
   pcl::IndicesPtr indices (new std::vector <int>);
-  pcl::PassThrough<pcl::PointXYZ> pass;
-  pass.setInputCloud (cloud);
-  pass.setFilterFieldName ("z");
-  pass.setFilterLimits (0.0, 1.0);
-  pass.filter (*indices);
+  pcl::removeNaNFromPointCloud(*cloud, *indices);
 
   pcl::MinCutSegmentation<pcl::PointXYZ> seg;
   seg.setInputCloud (cloud);
@@ -52,4 +48,4 @@ int main (int argc, char** argv)
   }
 
   return (0);
-}
\ No newline at end of file
+}
diff --git a/doc/tutorials/content/sources/model_outlier_removal/CMakeLists.txt b/doc/tutorials/content/sources/model_outlier_removal/CMakeLists.txt
index de01df66..0dd90eeb 100644
--- a/doc/tutorials/content/sources/model_outlier_removal/CMakeLists.txt
+++ b/doc/tutorials/content/sources/model_outlier_removal/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(model_outlier_removal)
 
diff --git a/doc/tutorials/content/sources/model_outlier_removal/model_outlier_removal.cpp b/doc/tutorials/content/sources/model_outlier_removal/model_outlier_removal.cpp
index b7de1fed..80735fad 100644
--- a/doc/tutorials/content/sources/model_outlier_removal/model_outlier_removal.cpp
+++ b/doc/tutorials/content/sources/model_outlier_removal/model_outlier_removal.cpp
@@ -9,8 +9,8 @@ main ()
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_sphere_filtered (new pcl::PointCloud<pcl::PointXYZ>);
 
   // 1. Generate cloud data
-  int noise_size = 5;
-  int sphere_data_size = 10;
+  std::size_t noise_size = 5;
+  std::size_t sphere_data_size = 10;
   cloud->width = noise_size + sphere_data_size;
   cloud->height = 1;
   cloud->points.resize (cloud->width * cloud->height);
@@ -24,7 +24,7 @@ main ()
   // 1.2 Add sphere:
   double rand_x1 = 1;
   double rand_x2 = 1;
-  for (std::size_t i = noise_size; i < noise_size + sphere_data_size; ++i)
+  for (std::size_t i = noise_size; i < (noise_size + sphere_data_size); ++i)
   {
     // See: http://mathworld.wolfram.com/SpherePointPicking.html
     while (pow (rand_x1, 2) + pow (rand_x2, 2) >= 1)
diff --git a/doc/tutorials/content/sources/moment_of_inertia/CMakeLists.txt b/doc/tutorials/content/sources/moment_of_inertia/CMakeLists.txt
index f64ea547..6891ea61 100644
--- a/doc/tutorials/content/sources/moment_of_inertia/CMakeLists.txt
+++ b/doc/tutorials/content/sources/moment_of_inertia/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(moment_of_inertia)
 
diff --git a/doc/tutorials/content/sources/narf_descriptor_visualization/narf_descriptor_visualization.cpp b/doc/tutorials/content/sources/narf_descriptor_visualization/narf_descriptor_visualization.cpp
index 25200c3d..3df0b1d3 100644
--- a/doc/tutorials/content/sources/narf_descriptor_visualization/narf_descriptor_visualization.cpp
+++ b/doc/tutorials/content/sources/narf_descriptor_visualization/narf_descriptor_visualization.cpp
@@ -120,7 +120,7 @@ main (int argc, char** argv)
   std::cout << "Now extracting NARFs in every image point.\n";
   std::vector<std::vector<pcl::Narf*> > narfs;
   narfs.resize (range_image.size ());
-  int last_percentage=-1;
+  unsigned int last_percentage=0;
   for (unsigned int y=0; y<range_image.height; ++y)
   {
     for (unsigned int x=0; x<range_image.width; ++x)
@@ -188,7 +188,7 @@ main (int argc, char** argv)
     float surface_patch_world_size = narf.getSurfacePatchWorldSize ();
     surface_patch_widget.showFloatImage (narf.getSurfacePatch (), surface_patch_pixel_size, surface_patch_pixel_size,
                                          -0.5f*surface_patch_world_size, 0.5f*surface_patch_world_size, true);
-    float surface_patch_rotation = narf.getSurfacePatchRotation ();
+    /*float surface_patch_rotation = narf.getSurfacePatchRotation ();
     float patch_middle = 0.5f* (float (surface_patch_pixel_size-1));
     float angle_step_size = pcl::deg2rad (360.0f)/narf.getDescriptorSize ();
     float cell_size = surface_patch_world_size/float (surface_patch_pixel_size),
@@ -208,7 +208,7 @@ main (int argc, char** argv)
     {
       //surface_patch_widget.markLine (radius-0.5, radius-0.5, radius-0.5f + 2.0f*radius*sinf (rotations[i]),
                                                 //radius-0.5f - 2.0f*radius*std::cos (rotations[i]), pcl::visualization::Vector3ub (255,0,0));
-    }
+    }*/
     
     descriptor_widget.showFloatImage (narf.getDescriptor (), narf.getDescriptorSize (), 1, -0.1f, 0.3f, true);
 
diff --git a/doc/tutorials/content/sources/narf_feature_extraction/narf_feature_extraction.cpp b/doc/tutorials/content/sources/narf_feature_extraction/narf_feature_extraction.cpp
index d2488a5b..ba450f4e 100644
--- a/doc/tutorials/content/sources/narf_feature_extraction/narf_feature_extraction.cpp
+++ b/doc/tutorials/content/sources/narf_feature_extraction/narf_feature_extraction.cpp
@@ -121,7 +121,7 @@ main (int argc, char** argv)
       for (float y=-0.5f; y<=0.5f; y+=0.01f)
       {
         PointType point;  point.x = x;  point.y = y;  point.z = 2.0f - y;
-        point_cloud.points.push_back (point);
+        point_cloud.push_back (point);
       }
     }
     point_cloud.width = point_cloud.size ();  point_cloud.height = 1;
diff --git a/doc/tutorials/content/sources/narf_keypoint_extraction/narf_keypoint_extraction.cpp b/doc/tutorials/content/sources/narf_keypoint_extraction/narf_keypoint_extraction.cpp
index 79a8a291..42e55957 100644
--- a/doc/tutorials/content/sources/narf_keypoint_extraction/narf_keypoint_extraction.cpp
+++ b/doc/tutorials/content/sources/narf_keypoint_extraction/narf_keypoint_extraction.cpp
@@ -115,7 +115,7 @@ main (int argc, char** argv)
       for (float y=-0.5f; y<=0.5f; y+=0.01f)
       {
         PointType point;  point.x = x;  point.y = y;  point.z = 2.0f - y;
-        point_cloud.points.push_back (point);
+        point_cloud.push_back (point);
       }
     }
     point_cloud.width = point_cloud.size ();  point_cloud.height = 1;
diff --git a/doc/tutorials/content/sources/normal_distributions_transform/CMakeLists.txt b/doc/tutorials/content/sources/normal_distributions_transform/CMakeLists.txt
index 076c56ae..6f98b3dc 100644
--- a/doc/tutorials/content/sources/normal_distributions_transform/CMakeLists.txt
+++ b/doc/tutorials/content/sources/normal_distributions_transform/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(normal_distributions_transform)
 
diff --git a/doc/tutorials/content/sources/normal_distributions_transform/normal_distributions_transform.cpp b/doc/tutorials/content/sources/normal_distributions_transform/normal_distributions_transform.cpp
index 24938193..b5a525d2 100644
--- a/doc/tutorials/content/sources/normal_distributions_transform/normal_distributions_transform.cpp
+++ b/doc/tutorials/content/sources/normal_distributions_transform/normal_distributions_transform.cpp
@@ -12,7 +12,7 @@
 using namespace std::chrono_literals;
 
 int
-main (int argc, char** argv)
+main ()
 {
   // Loading first scan of room.
   pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);
diff --git a/doc/tutorials/content/sources/octree_change_detection/CMakeLists.txt b/doc/tutorials/content/sources/octree_change_detection/CMakeLists.txt
index dae72ab2..a7bf4bb6 100644
--- a/doc/tutorials/content/sources/octree_change_detection/CMakeLists.txt
+++ b/doc/tutorials/content/sources/octree_change_detection/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(octree_change_detection)
 
diff --git a/doc/tutorials/content/sources/octree_change_detection/octree_change_detection.cpp b/doc/tutorials/content/sources/octree_change_detection/octree_change_detection.cpp
index 16aeef1c..6f48ba3a 100644
--- a/doc/tutorials/content/sources/octree_change_detection/octree_change_detection.cpp
+++ b/doc/tutorials/content/sources/octree_change_detection/octree_change_detection.cpp
@@ -6,7 +6,7 @@
 #include <ctime>
 
 int
-main (int argc, char** argv)
+main ()
 {
   srand ((unsigned int) time (NULL));
 
diff --git a/doc/tutorials/content/sources/octree_search/CMakeLists.txt b/doc/tutorials/content/sources/octree_search/CMakeLists.txt
index 26d1661c..80e7c785 100644
--- a/doc/tutorials/content/sources/octree_search/CMakeLists.txt
+++ b/doc/tutorials/content/sources/octree_search/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(octree_search)
 
diff --git a/doc/tutorials/content/sources/octree_search/octree_search.cpp b/doc/tutorials/content/sources/octree_search/octree_search.cpp
index a37daad2..6b49d22c 100644
--- a/doc/tutorials/content/sources/octree_search/octree_search.cpp
+++ b/doc/tutorials/content/sources/octree_search/octree_search.cpp
@@ -6,7 +6,7 @@
 #include <ctime>
 
 int
-main (int argc, char** argv)
+main ()
 {
   srand ((unsigned int) time (NULL));
 
diff --git a/doc/tutorials/content/sources/openni_grabber/CMakeLists.txt b/doc/tutorials/content/sources/openni_grabber/CMakeLists.txt
index 0a67ac5c..c0ff398c 100644
--- a/doc/tutorials/content/sources/openni_grabber/CMakeLists.txt
+++ b/doc/tutorials/content/sources/openni_grabber/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(openni_grabber)
 
diff --git a/doc/tutorials/content/sources/openni_narf_keypoint_extraction/openni_narf_keypoint_extraction.cpp b/doc/tutorials/content/sources/openni_narf_keypoint_extraction/openni_narf_keypoint_extraction.cpp
index 145b04d4..8e4d130c 100644
--- a/doc/tutorials/content/sources/openni_narf_keypoint_extraction/openni_narf_keypoint_extraction.cpp
+++ b/doc/tutorials/content/sources/openni_narf_keypoint_extraction/openni_narf_keypoint_extraction.cpp
@@ -4,7 +4,7 @@
 #include <iostream>
 #include <pcl/io/openni_grabber.h>
 #include <pcl/range_image/range_image_planar.h>
-#include <pcl/common/common_headers.h>
+#include <pcl/common/time.h> // for pcl::getTime
 #include <pcl/visualization/range_image_visualizer.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/features/range_image_border_extractor.h>
@@ -156,12 +156,12 @@ int main (int argc, char** argv)
       int width = depth_image_ptr->getWidth (), height = depth_image_ptr->getHeight ();
       float center_x = width/2, center_y = height/2;
       float focal_length_x = depth_image_ptr->getFocalLength (), focal_length_y = focal_length_x;
-      float original_angular_resolution = asinf (0.5f*float (width)/float (focal_length_x)) / (0.5f*float (width));
+      // float original_angular_resolution = asinf (0.5f*float (width)/float (focal_length_x)) / (0.5f*float (width));
       float desired_angular_resolution = angular_resolution;
       range_image_planar.setDepthImage (depth_map, width, height, center_x, center_y,
                                         focal_length_x, focal_length_y, desired_angular_resolution);
       depth_image_mutex.unlock ();
-      got_new_range_image = !range_image_planar.points.empty ();
+      got_new_range_image = !range_image_planar.empty ();
     }
     
     if (!got_new_range_image)
diff --git a/doc/tutorials/content/sources/openni_range_image_visualization/openni_range_image_visualization.cpp b/doc/tutorials/content/sources/openni_range_image_visualization/openni_range_image_visualization.cpp
index af7c411e..dabc5432 100644
--- a/doc/tutorials/content/sources/openni_range_image_visualization/openni_range_image_visualization.cpp
+++ b/doc/tutorials/content/sources/openni_range_image_visualization/openni_range_image_visualization.cpp
@@ -4,7 +4,7 @@
 #include <iostream>
 #include <pcl/io/openni_grabber.h>
 #include <pcl/range_image/range_image_planar.h>
-#include <pcl/common/common_headers.h>
+#include <pcl/common/angles.h> // for pcl::deg2rad
 #include <pcl/visualization/range_image_visualizer.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/console/parse.h>
@@ -121,12 +121,12 @@ int main (int argc, char** argv)
       int width = depth_image_ptr->getWidth (), height = depth_image_ptr->getHeight ();
       float center_x = width/2, center_y = height/2;
       float focal_length_x = depth_image_ptr->getFocalLength (), focal_length_y = focal_length_x;
-      float original_angular_resolution = asinf (0.5f*float (width)/float (focal_length_x)) / (0.5f*float (width));
+      // float original_angular_resolution = asinf (0.5f*float (width)/float (focal_length_x)) / (0.5f*float (width));
       float desired_angular_resolution = angular_resolution;
       range_image_planar.setDepthImage (depth_map, width, height, center_x, center_y,
                                         focal_length_x, focal_length_y, desired_angular_resolution);
       depth_image_mutex.unlock ();
-      got_new_range_image = !range_image_planar.points.empty ();
+      got_new_range_image = !range_image_planar.empty ();
     }
     
     if (!got_new_range_image)
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 b087d0d8..6f841165 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
@@ -143,11 +143,11 @@ void showCloudsRight(const PointCloudWithNormals::Ptr cloud_target, const PointC
 
   PointCloudColorHandlerGenericField<PointNormalT> tgt_color_handler (cloud_target, "curvature");
   if (!tgt_color_handler.isCapable ())
-      PCL_WARN ("Cannot create curvature color handler!");
+      PCL_WARN ("Cannot create curvature color handler!\n");
 
   PointCloudColorHandlerGenericField<PointNormalT> src_color_handler (cloud_source, "curvature");
   if (!src_color_handler.isCapable ())
-      PCL_WARN ("Cannot create curvature color handler!");
+      PCL_WARN ("Cannot create curvature color handler!\n");
 
 
   p->addPointCloud (cloud_target, tgt_color_handler, "target", vp_2);
@@ -332,11 +332,11 @@ int main (int argc, char** argv)
   // Check user input
   if (data.empty ())
   {
-    PCL_ERROR ("Syntax is: %s <source.pcd> <target.pcd> [*]", argv[0]);
-    PCL_ERROR ("[*] - multiple files can be added. The registration results of (i, i+1) will be registered against (i+2), etc");
+    PCL_ERROR ("Syntax is: %s <source.pcd> <target.pcd> [*]\n", argv[0]);
+    PCL_ERROR ("[*] - multiple files can be added. The registration results of (i, i+1) will be registered against (i+2), etc\n");
     return (-1);
   }
-  PCL_INFO ("Loaded %d datasets.", (int)data.size ());
+  PCL_INFO ("Loaded %d datasets.\n", (int)data.size ());
   
   // Create a PCLVisualizer object
   p = new pcl::visualization::PCLVisualizer (argc, argv, "Pairwise Incremental Registration example");
diff --git a/doc/tutorials/content/sources/passthrough/CMakeLists.txt b/doc/tutorials/content/sources/passthrough/CMakeLists.txt
index b6f4e5d1..f0576845 100644
--- a/doc/tutorials/content/sources/passthrough/CMakeLists.txt
+++ b/doc/tutorials/content/sources/passthrough/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(passthrough)
 
diff --git a/doc/tutorials/content/sources/passthrough/passthrough.cpp b/doc/tutorials/content/sources/passthrough/passthrough.cpp
index 98ec9b9c..b942ec61 100644
--- a/doc/tutorials/content/sources/passthrough/passthrough.cpp
+++ b/doc/tutorials/content/sources/passthrough/passthrough.cpp
@@ -3,7 +3,7 @@
 #include <pcl/filters/passthrough.h>
 
 int
- main (int argc, char** argv)
+ main ()
 {
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
diff --git a/doc/tutorials/content/sources/pcd_read/CMakeLists.txt b/doc/tutorials/content/sources/pcd_read/CMakeLists.txt
index 9bf29bfd..af4ad3d4 100644
--- a/doc/tutorials/content/sources/pcd_read/CMakeLists.txt
+++ b/doc/tutorials/content/sources/pcd_read/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(pcd_read)
 
diff --git a/doc/tutorials/content/sources/pcd_read/pcd_read.cpp b/doc/tutorials/content/sources/pcd_read/pcd_read.cpp
index 0d923829..2b41d3d5 100644
--- a/doc/tutorials/content/sources/pcd_read/pcd_read.cpp
+++ b/doc/tutorials/content/sources/pcd_read/pcd_read.cpp
@@ -3,7 +3,7 @@
 #include <pcl/point_types.h>
 
 int
-main (int argc, char** argv)
+main ()
 {
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
 
diff --git a/doc/tutorials/content/sources/pcd_write/CMakeLists.txt b/doc/tutorials/content/sources/pcd_write/CMakeLists.txt
index 6476e96f..9fe83623 100644
--- a/doc/tutorials/content/sources/pcd_write/CMakeLists.txt
+++ b/doc/tutorials/content/sources/pcd_write/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(pcd_write)
 
diff --git a/doc/tutorials/content/sources/pcd_write/pcd_write.cpp b/doc/tutorials/content/sources/pcd_write/pcd_write.cpp
index 600316b8..f4209887 100644
--- a/doc/tutorials/content/sources/pcd_write/pcd_write.cpp
+++ b/doc/tutorials/content/sources/pcd_write/pcd_write.cpp
@@ -3,7 +3,7 @@
 #include <pcl/point_types.h>
 
 int
-  main (int argc, char** argv)
+  main ()
 {
   pcl::PointCloud<pcl::PointXYZ> cloud;
 
@@ -11,7 +11,7 @@ int
   cloud.width    = 5;
   cloud.height   = 1;
   cloud.is_dense = false;
-  cloud.points.resize (cloud.width * cloud.height);
+  cloud.resize (cloud.width * cloud.height);
 
   for (auto& point: cloud)
   {
diff --git a/doc/tutorials/content/sources/pcl_painter2D/pcl_painter2D_demo.cpp b/doc/tutorials/content/sources/pcl_painter2D/pcl_painter2D_demo.cpp
index 55082baa..9e578a4e 100644
--- a/doc/tutorials/content/sources/pcl_painter2D/pcl_painter2D_demo.cpp
+++ b/doc/tutorials/content/sources/pcl_painter2D/pcl_painter2D_demo.cpp
@@ -6,7 +6,7 @@
 #include <pcl/visualization/pcl_painter2D.h>
 //----------------------------------------------------------------------------
 
-int main (int argc, char * argv [])
+int main ()
 {
   pcl::visualization::PCLPainter2D *painter = new pcl::visualization::PCLPainter2D();
   
@@ -47,4 +47,4 @@ int main (int argc, char * argv [])
 
 
   return 0;
-}
\ No newline at end of file
+}
diff --git a/doc/tutorials/content/sources/pcl_plotter/CMakeLists.txt b/doc/tutorials/content/sources/pcl_plotter/CMakeLists.txt
index 420694d9..404e5ebd 100644
--- a/doc/tutorials/content/sources/pcl_plotter/CMakeLists.txt
+++ b/doc/tutorials/content/sources/pcl_plotter/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 project(pcl_plotter)
 find_package(PCL 1.7 REQUIRED COMPONENTS common visualization)
 include_directories(${PCL_INCLUDE_DIRS})
diff --git a/doc/tutorials/content/sources/pcl_plotter/pcl_plotter_demo.cpp b/doc/tutorials/content/sources/pcl_plotter/pcl_plotter_demo.cpp
index aa832223..d1e173ab 100644
--- a/doc/tutorials/content/sources/pcl_plotter/pcl_plotter_demo.cpp
+++ b/doc/tutorials/content/sources/pcl_plotter/pcl_plotter_demo.cpp
@@ -39,7 +39,7 @@ identity (double val)
 //............................................................................
 
 int
-main (int argc, char * argv [])
+main ()
 {
   //defining a plotter
   PCLPlotter *plotter = new PCLPlotter ("My Plotter");
diff --git a/doc/tutorials/content/sources/pcl_visualizer/pcl_visualizer_demo.cpp b/doc/tutorials/content/sources/pcl_visualizer/pcl_visualizer_demo.cpp
index 376ebe9a..99d98d58 100644
--- a/doc/tutorials/content/sources/pcl_visualizer/pcl_visualizer_demo.cpp
+++ b/doc/tutorials/content/sources/pcl_visualizer/pcl_visualizer_demo.cpp
@@ -3,7 +3,7 @@
 #include <iostream>
 #include <thread>
 
-#include <pcl/common/common_headers.h>
+#include <pcl/common/angles.h> // for pcl::deg2rad
 #include <pcl/features/normal_3d.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/visualization/pcl_visualizer.h>
diff --git a/doc/tutorials/content/sources/planar_segmentation/CMakeLists.txt b/doc/tutorials/content/sources/planar_segmentation/CMakeLists.txt
index 3450cbc4..bb638d5f 100644
--- a/doc/tutorials/content/sources/planar_segmentation/CMakeLists.txt
+++ b/doc/tutorials/content/sources/planar_segmentation/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(planar_segmentation)
 
diff --git a/doc/tutorials/content/sources/planar_segmentation/planar_segmentation.cpp b/doc/tutorials/content/sources/planar_segmentation/planar_segmentation.cpp
index e4e42209..b78fcf97 100644
--- a/doc/tutorials/content/sources/planar_segmentation/planar_segmentation.cpp
+++ b/doc/tutorials/content/sources/planar_segmentation/planar_segmentation.cpp
@@ -7,7 +7,7 @@
 #include <pcl/segmentation/sac_segmentation.h>
 
 int
- main (int argc, char** argv)
+ main ()
 {
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 
@@ -51,7 +51,7 @@ int
 
   if (inliers->indices.size () == 0)
   {
-    PCL_ERROR ("Could not estimate a planar model for the given dataset.");
+    PCL_ERROR ("Could not estimate a planar model for the given dataset.\n");
     return (-1);
   }
 
@@ -61,7 +61,6 @@ int
                                       << coefficients->values[3] << std::endl;
 
   std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;
-  for (std::size_t i = 0; i < inliers->indices.size (); ++i)
   for (const auto& idx: inliers->indices)
     std::cerr << idx << "    " << cloud->points[idx].x << " "
                                << cloud->points[idx].y << " "
diff --git a/doc/tutorials/content/sources/point_cloud_compression/CMakeLists.txt b/doc/tutorials/content/sources/point_cloud_compression/CMakeLists.txt
index 61a6e8a4..5d2c6899 100644
--- a/doc/tutorials/content/sources/point_cloud_compression/CMakeLists.txt
+++ b/doc/tutorials/content/sources/point_cloud_compression/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(point_cloud_compression)
 
diff --git a/doc/tutorials/content/sources/point_cloud_compression/point_cloud_compression.cpp b/doc/tutorials/content/sources/point_cloud_compression/point_cloud_compression.cpp
index 0c1f9350..7621752c 100644
--- a/doc/tutorials/content/sources/point_cloud_compression/point_cloud_compression.cpp
+++ b/doc/tutorials/content/sources/point_cloud_compression/point_cloud_compression.cpp
@@ -90,7 +90,7 @@ public:
 };
 
 int
-main (int argc, char **argv)
+main ()
 {
   SimpleOpenNIViewer v;
   v.run ();
diff --git a/doc/tutorials/content/sources/project_inliers/CMakeLists.txt b/doc/tutorials/content/sources/project_inliers/CMakeLists.txt
index 5fce8321..2c0be2f1 100644
--- a/doc/tutorials/content/sources/project_inliers/CMakeLists.txt
+++ b/doc/tutorials/content/sources/project_inliers/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(project_inliers)
 
diff --git a/doc/tutorials/content/sources/project_inliers/project_inliers.cpp b/doc/tutorials/content/sources/project_inliers/project_inliers.cpp
index 9853bc0b..325a271d 100644
--- a/doc/tutorials/content/sources/project_inliers/project_inliers.cpp
+++ b/doc/tutorials/content/sources/project_inliers/project_inliers.cpp
@@ -1,11 +1,11 @@
 #include <iostream>
-#include <pcl/io/pcd_io.h>
+#include <pcl/point_cloud.h> // for PointCloud
 #include <pcl/point_types.h>
 #include <pcl/ModelCoefficients.h>
 #include <pcl/filters/project_inliers.h>
 
 int
- main (int argc, char** argv)
+ main ()
 {
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected (new pcl::PointCloud<pcl::PointXYZ>);
diff --git a/doc/tutorials/content/sources/qt_colorize_cloud/CMakeLists.txt b/doc/tutorials/content/sources/qt_colorize_cloud/CMakeLists.txt
index b2f1b8a4..5ee1dc81 100644
--- a/doc/tutorials/content/sources/qt_colorize_cloud/CMakeLists.txt
+++ b/doc/tutorials/content/sources/qt_colorize_cloud/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8.11)
+cmake_minimum_required(VERSION 3.5)
 
 project(pcl_colorize_cloud)
 
diff --git a/doc/tutorials/content/sources/qt_colorize_cloud/pclviewer.cpp b/doc/tutorials/content/sources/qt_colorize_cloud/pclviewer.cpp
index 9132ec5c..2a22446b 100644
--- a/doc/tutorials/content/sources/qt_colorize_cloud/pclviewer.cpp
+++ b/doc/tutorials/content/sources/qt_colorize_cloud/pclviewer.cpp
@@ -3,9 +3,9 @@
 
 PCLViewer::PCLViewer (QWidget *parent) :
     QMainWindow (parent),
-    ui (new Ui::PCLViewer),
     filtering_axis_ (1),  // = y
-    color_mode_ (4)  // = Rainbow
+    color_mode_ (4),  // = Rainbow
+    ui (new Ui::PCLViewer)
 {
   ui->setupUi (this);
   this->setWindowTitle ("PCL viewer");
diff --git a/doc/tutorials/content/sources/qt_visualizer/CMakeLists.txt b/doc/tutorials/content/sources/qt_visualizer/CMakeLists.txt
index 7c4d7d4c..e4f3caa8 100644
--- a/doc/tutorials/content/sources/qt_visualizer/CMakeLists.txt
+++ b/doc/tutorials/content/sources/qt_visualizer/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8.11)
+cmake_minimum_required(VERSION 3.5)
 
 project(pcl_visualizer)
 
diff --git a/doc/tutorials/content/sources/radius_outlier_removal/CMakeLists.txt b/doc/tutorials/content/sources/radius_outlier_removal/CMakeLists.txt
index 018599b7..b1236295 100644
--- a/doc/tutorials/content/sources/radius_outlier_removal/CMakeLists.txt
+++ b/doc/tutorials/content/sources/radius_outlier_removal/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(radius_outlier_removal)
 
diff --git a/doc/tutorials/content/sources/radius_outlier_removal/radius_outlier_removal.cpp b/doc/tutorials/content/sources/radius_outlier_removal/radius_outlier_removal.cpp
index 6093fcdd..64bdaea0 100644
--- a/doc/tutorials/content/sources/radius_outlier_removal/radius_outlier_removal.cpp
+++ b/doc/tutorials/content/sources/radius_outlier_removal/radius_outlier_removal.cpp
@@ -3,7 +3,7 @@
 #include <pcl/filters/radius_outlier_removal.h>
 
 int
- main (int argc, char** argv)
+ main ()
 {
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
diff --git a/doc/tutorials/content/sources/random_sample_consensus/CMakeLists.txt b/doc/tutorials/content/sources/random_sample_consensus/CMakeLists.txt
index fddc4c61..9d547639 100644
--- a/doc/tutorials/content/sources/random_sample_consensus/CMakeLists.txt
+++ b/doc/tutorials/content/sources/random_sample_consensus/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(random_sample_consensus)
 
diff --git a/doc/tutorials/content/sources/random_sample_consensus/random_sample_consensus.cpp b/doc/tutorials/content/sources/random_sample_consensus/random_sample_consensus.cpp
index 80f39644..5c5cacf2 100644
--- a/doc/tutorials/content/sources/random_sample_consensus/random_sample_consensus.cpp
+++ b/doc/tutorials/content/sources/random_sample_consensus/random_sample_consensus.cpp
@@ -2,8 +2,8 @@
 #include <thread>
 
 #include <pcl/console/parse.h>
-#include <pcl/filters/extract_indices.h>
-#include <pcl/io/pcd_io.h>
+#include <pcl/point_cloud.h> // for PointCloud
+#include <pcl/common/io.h> // for copyPointCloud
 #include <pcl/point_types.h>
 #include <pcl/sample_consensus/ransac.h>
 #include <pcl/sample_consensus/sac_model_plane.h>
@@ -39,7 +39,7 @@ main(int argc, char** argv)
   cloud->height   = 1;
   cloud->is_dense = false;
   cloud->points.resize (cloud->width * cloud->height);
-  for (pcl::index_t i = 0; i < cloud->size (); ++i)
+  for (pcl::index_t i = 0; i < static_cast<pcl::index_t>(cloud->size ()); ++i)
   {
     if (pcl::console::find_argument (argc, argv, "-s") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)
     {
diff --git a/doc/tutorials/content/sources/range_image_border_extraction/range_image_border_extraction.cpp b/doc/tutorials/content/sources/range_image_border_extraction/range_image_border_extraction.cpp
index b9d99b76..950bb30e 100644
--- a/doc/tutorials/content/sources/range_image_border_extraction/range_image_border_extraction.cpp
+++ b/doc/tutorials/content/sources/range_image_border_extraction/range_image_border_extraction.cpp
@@ -98,7 +98,7 @@ main (int argc, char** argv)
       for (float y=-0.5f; y<=0.5f; y+=0.01f)
       {
         PointType point;  point.x = x;  point.y = y;  point.z = 2.0f - y;
-        point_cloud.points.push_back (point);
+        point_cloud.push_back (point);
       }
     }
     point_cloud.width = point_cloud.size ();  point_cloud.height = 1;
@@ -151,11 +151,11 @@ main (int argc, char** argv)
     for (int x=0; x< (int)range_image.width; ++x)
     {
       if (border_descriptions[y*range_image.width + x].traits[pcl::BORDER_TRAIT__OBSTACLE_BORDER])
-        border_points.points.push_back (range_image[y*range_image.width + x]);
+        border_points.push_back (range_image[y*range_image.width + x]);
       if (border_descriptions[y*range_image.width + x].traits[pcl::BORDER_TRAIT__VEIL_POINT])
-        veil_points.points.push_back (range_image[y*range_image.width + x]);
+        veil_points.push_back (range_image[y*range_image.width + x]);
       if (border_descriptions[y*range_image.width + x].traits[pcl::BORDER_TRAIT__SHADOW_BORDER])
-        shadow_points.points.push_back (range_image[y*range_image.width + x]);
+        shadow_points.push_back (range_image[y*range_image.width + x]);
     }
   }
   pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> border_points_color_handler (border_points_ptr, 0, 255, 0);
diff --git a/doc/tutorials/content/sources/range_image_creation/range_image_creation.cpp b/doc/tutorials/content/sources/range_image_creation/range_image_creation.cpp
index f2b71897..a3d56ab1 100644
--- a/doc/tutorials/content/sources/range_image_creation/range_image_creation.cpp
+++ b/doc/tutorials/content/sources/range_image_creation/range_image_creation.cpp
@@ -1,6 +1,6 @@
 #include <pcl/range_image/range_image.h>
 
-int main (int argc, char** argv) {
+int main () {
   pcl::PointCloud<pcl::PointXYZ> pointCloud;
   
   // Generate the data
@@ -10,7 +10,7 @@ int main (int argc, char** argv) {
       point.x = 2.0f - y;
       point.y = y;
       point.z = z;
-      pointCloud.points.push_back(point);
+      pointCloud.push_back(point);
     }
   }
   pointCloud.width = pointCloud.size();
diff --git a/doc/tutorials/content/sources/range_image_visualization/range_image_visualization.cpp b/doc/tutorials/content/sources/range_image_visualization/range_image_visualization.cpp
index 432033ad..523c1ed2 100644
--- a/doc/tutorials/content/sources/range_image_visualization/range_image_visualization.cpp
+++ b/doc/tutorials/content/sources/range_image_visualization/range_image_visualization.cpp
@@ -1,6 +1,6 @@
 #include <iostream>
 
-#include <pcl/common/common_headers.h>
+
 #include <pcl/range_image/range_image.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/visualization/range_image_visualizer.h>
@@ -106,7 +106,7 @@ main (int argc, char** argv)
       for (float y=-0.5f; y<=0.5f; y+=0.01f)
       {
         PointType point;  point.x = x;  point.y = y;  point.z = 2.0f - y;
-        point_cloud.points.push_back (point);
+        point_cloud.push_back (point);
       }
     }
     point_cloud.width = point_cloud.size ();  point_cloud.height = 1;
diff --git a/doc/tutorials/content/sources/region_growing_rgb_segmentation/CMakeLists.txt b/doc/tutorials/content/sources/region_growing_rgb_segmentation/CMakeLists.txt
index fb4544ea..2ea126e9 100644
--- a/doc/tutorials/content/sources/region_growing_rgb_segmentation/CMakeLists.txt
+++ b/doc/tutorials/content/sources/region_growing_rgb_segmentation/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(region_growing_rgb_segmentation)
 
diff --git a/doc/tutorials/content/sources/region_growing_rgb_segmentation/region_growing_rgb_segmentation.cpp b/doc/tutorials/content/sources/region_growing_rgb_segmentation/region_growing_rgb_segmentation.cpp
index ba2db411..fc1196df 100644
--- a/doc/tutorials/content/sources/region_growing_rgb_segmentation/region_growing_rgb_segmentation.cpp
+++ b/doc/tutorials/content/sources/region_growing_rgb_segmentation/region_growing_rgb_segmentation.cpp
@@ -7,13 +7,13 @@
 #include <pcl/search/search.h>
 #include <pcl/search/kdtree.h>
 #include <pcl/visualization/cloud_viewer.h>
-#include <pcl/filters/passthrough.h>
+#include <pcl/filters/filter_indices.h> // for pcl::removeNaNFromPointCloud
 #include <pcl/segmentation/region_growing_rgb.h>
 
 using namespace std::chrono_literals;
 
 int
-main (int argc, char** argv)
+main ()
 {
   pcl::search::Search <pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
 
@@ -25,11 +25,7 @@ main (int argc, char** argv)
   }
 
   pcl::IndicesPtr indices (new std::vector <int>);
-  pcl::PassThrough<pcl::PointXYZRGB> pass;
-  pass.setInputCloud (cloud);
-  pass.setFilterFieldName ("z");
-  pass.setFilterLimits (0.0, 1.0);
-  pass.filter (*indices);
+  pcl::removeNaNFromPointCloud (*cloud, *indices);
 
   pcl::RegionGrowingRGB<pcl::PointXYZRGB> reg;
   reg.setInputCloud (cloud);
diff --git a/doc/tutorials/content/sources/region_growing_segmentation/CMakeLists.txt b/doc/tutorials/content/sources/region_growing_segmentation/CMakeLists.txt
index f7e99610..8b410b89 100644
--- a/doc/tutorials/content/sources/region_growing_segmentation/CMakeLists.txt
+++ b/doc/tutorials/content/sources/region_growing_segmentation/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(region_growing_segmentation)
 
diff --git a/doc/tutorials/content/sources/region_growing_segmentation/region_growing_segmentation.cpp b/doc/tutorials/content/sources/region_growing_segmentation/region_growing_segmentation.cpp
index 5b472eae..f783f84e 100644
--- a/doc/tutorials/content/sources/region_growing_segmentation/region_growing_segmentation.cpp
+++ b/doc/tutorials/content/sources/region_growing_segmentation/region_growing_segmentation.cpp
@@ -6,11 +6,11 @@
 #include <pcl/search/kdtree.h>
 #include <pcl/features/normal_3d.h>
 #include <pcl/visualization/cloud_viewer.h>
-#include <pcl/filters/passthrough.h>
+#include <pcl/filters/filter_indices.h> // for pcl::removeNaNFromPointCloud
 #include <pcl/segmentation/region_growing.h>
 
 int
-main (int argc, char** argv)
+main ()
 {
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
   if ( pcl::io::loadPCDFile <pcl::PointXYZ> ("region_growing_tutorial.pcd", *cloud) == -1)
@@ -28,11 +28,7 @@ main (int argc, char** argv)
   normal_estimator.compute (*normals);
 
   pcl::IndicesPtr indices (new std::vector <int>);
-  pcl::PassThrough<pcl::PointXYZ> pass;
-  pass.setInputCloud (cloud);
-  pass.setFilterFieldName ("z");
-  pass.setFilterLimits (0.0, 1.0);
-  pass.filter (*indices);
+  pcl::removeNaNFromPointCloud(*cloud, *indices);
 
   pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
   reg.setMinClusterSize (50);
@@ -40,7 +36,7 @@ main (int argc, char** argv)
   reg.setSearchMethod (tree);
   reg.setNumberOfNeighbours (30);
   reg.setInputCloud (cloud);
-  //reg.setIndices (indices);
+  reg.setIndices (indices);
   reg.setInputNormals (normals);
   reg.setSmoothnessThreshold (3.0 / 180.0 * M_PI);
   reg.setCurvatureThreshold (1.0);
@@ -52,7 +48,7 @@ main (int argc, char** argv)
   std::cout << "First cluster has " << clusters[0].indices.size () << " points." << std::endl;
   std::cout << "These are the indices of the points of the initial" <<
     std::endl << "cloud that belong to the first cluster:" << std::endl;
-  int counter = 0;
+  std::size_t counter = 0;
   while (counter < clusters[0].indices.size ())
   {
     std::cout << clusters[0].indices[counter] << ", ";
diff --git a/doc/tutorials/content/sources/registration_api/CMakeLists.txt b/doc/tutorials/content/sources/registration_api/CMakeLists.txt
index 845c1657..152e0cca 100644
--- a/doc/tutorials/content/sources/registration_api/CMakeLists.txt
+++ b/doc/tutorials/content/sources/registration_api/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(registration_api)
 
diff --git a/doc/tutorials/content/sources/registration_api/example2.cpp b/doc/tutorials/content/sources/registration_api/example2.cpp
index fc70b24c..18177a8e 100644
--- a/doc/tutorials/content/sources/registration_api/example2.cpp
+++ b/doc/tutorials/content/sources/registration_api/example2.cpp
@@ -166,8 +166,8 @@ computeTransformation (const PointCloud<PointXYZ>::Ptr &src,
   // Reject correspondences based on their XYZ distance
   rejectBadCorrespondences (all_correspondences, keypoints_src, keypoints_tgt, *good_correspondences);
 
-  for (int i = 0; i < good_correspondences->size (); ++i)
-    std::cerr << good_correspondences->at (i) << std::endl;
+  for (const auto& corr : (*good_correspondences))
+    std::cerr << corr << std::endl;
   // Obtain the best transformation between the two sets of keypoints given the remaining correspondences
   TransformationEstimationSVD<PointXYZ, PointXYZ> trans_est;
   trans_est.estimateRigidTransformation (*keypoints_src, *keypoints_tgt, *good_correspondences, transform);
diff --git a/doc/tutorials/content/sources/remove_outliers/CMakeLists.txt b/doc/tutorials/content/sources/remove_outliers/CMakeLists.txt
index 19c951d3..f9e524f3 100644
--- a/doc/tutorials/content/sources/remove_outliers/CMakeLists.txt
+++ b/doc/tutorials/content/sources/remove_outliers/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(remove_outliers)
 
diff --git a/doc/tutorials/content/sources/resampling/CMakeLists.txt b/doc/tutorials/content/sources/resampling/CMakeLists.txt
index f2a48476..fed55d82 100644
--- a/doc/tutorials/content/sources/resampling/CMakeLists.txt
+++ b/doc/tutorials/content/sources/resampling/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(resampling)
 
diff --git a/doc/tutorials/content/sources/resampling/resampling.cpp b/doc/tutorials/content/sources/resampling/resampling.cpp
index 0efbca9c..a47cb6d1 100644
--- a/doc/tutorials/content/sources/resampling/resampling.cpp
+++ b/doc/tutorials/content/sources/resampling/resampling.cpp
@@ -4,7 +4,7 @@
 #include <pcl/surface/mls.h>
 
 int
-main (int argc, char** argv)
+main ()
 {
   // Load input file into a PointCloud<T> with an appropriate type
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
diff --git a/doc/tutorials/content/sources/rops_feature/CMakeLists.txt b/doc/tutorials/content/sources/rops_feature/CMakeLists.txt
index 98f3fc80..8fb6ce91 100644
--- a/doc/tutorials/content/sources/rops_feature/CMakeLists.txt
+++ b/doc/tutorials/content/sources/rops_feature/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(rops_feature)
 
diff --git a/doc/tutorials/content/sources/statistical_removal/CMakeLists.txt b/doc/tutorials/content/sources/statistical_removal/CMakeLists.txt
index 3bf9b093..04c4fb28 100644
--- a/doc/tutorials/content/sources/statistical_removal/CMakeLists.txt
+++ b/doc/tutorials/content/sources/statistical_removal/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(statistical_removal)
 
diff --git a/doc/tutorials/content/sources/statistical_removal/statistical_removal.cpp b/doc/tutorials/content/sources/statistical_removal/statistical_removal.cpp
index 8ccba826..98e5aebe 100644
--- a/doc/tutorials/content/sources/statistical_removal/statistical_removal.cpp
+++ b/doc/tutorials/content/sources/statistical_removal/statistical_removal.cpp
@@ -4,7 +4,7 @@
 #include <pcl/filters/statistical_outlier_removal.h>
 
 int
-main (int argc, char** argv)
+main ()
 {
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
@@ -35,4 +35,4 @@ main (int argc, char** argv)
   writer.write<pcl::PointXYZ> ("table_scene_lms400_outliers.pcd", *cloud_filtered, false);
 
   return (0);
-}
\ No newline at end of file
+}
diff --git a/doc/tutorials/content/sources/stick_segmentation/CMakeLists.txt b/doc/tutorials/content/sources/stick_segmentation/CMakeLists.txt
index 72a1a3da..dd6fc63b 100644
--- a/doc/tutorials/content/sources/stick_segmentation/CMakeLists.txt
+++ b/doc/tutorials/content/sources/stick_segmentation/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(stick_segmentation)
 
diff --git a/doc/tutorials/content/sources/stick_segmentation/stick_segmentation.cpp b/doc/tutorials/content/sources/stick_segmentation/stick_segmentation.cpp
index 03f85dfa..9ecc8e4c 100644
--- a/doc/tutorials/content/sources/stick_segmentation/stick_segmentation.cpp
+++ b/doc/tutorials/content/sources/stick_segmentation/stick_segmentation.cpp
@@ -3,8 +3,8 @@
 #include <pcl/console/time.h>
 #include <pcl/point_types.h>
 #include <pcl/memory.h>
+#include <pcl/point_cloud.h> // for PointCloud
 #include <pcl/io/pcd_io.h>
-#include <pcl/filters/passthrough.h>
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/sample_consensus/method_types.h>
 #include <pcl/segmentation/sac_segmentation.h>
diff --git a/doc/tutorials/content/sources/supervoxel_clustering/CMakeLists.txt b/doc/tutorials/content/sources/supervoxel_clustering/CMakeLists.txt
index ec50f3ff..f1e758fd 100644
--- a/doc/tutorials/content/sources/supervoxel_clustering/CMakeLists.txt
+++ b/doc/tutorials/content/sources/supervoxel_clustering/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(supervoxel_clustering)
 
diff --git a/doc/tutorials/content/sources/template_alignment/CMakeLists.txt b/doc/tutorials/content/sources/template_alignment/CMakeLists.txt
index 7642bfb2..6044f411 100644
--- a/doc/tutorials/content/sources/template_alignment/CMakeLists.txt
+++ b/doc/tutorials/content/sources/template_alignment/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(template_alignment)
 
diff --git a/doc/tutorials/content/sources/tracking/CMakeLists.txt b/doc/tutorials/content/sources/tracking/CMakeLists.txt
index 6edbeaed..4e60a9ce 100644
--- a/doc/tutorials/content/sources/tracking/CMakeLists.txt
+++ b/doc/tutorials/content/sources/tracking/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(openni_tracking)
 
diff --git a/doc/tutorials/content/sources/tracking/tracking_sample.cpp b/doc/tutorials/content/sources/tracking/tracking_sample.cpp
index 672e6fa2..682b4bbc 100644
--- a/doc/tutorials/content/sources/tracking/tracking_sample.cpp
+++ b/doc/tutorials/content/sources/tracking/tracking_sample.cpp
@@ -1,31 +1,22 @@
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl/io/openni_grabber.h>
-#include <pcl/console/parse.h>
-#include <pcl/common/time.h>
 #include <pcl/common/centroid.h>
+#include <pcl/common/transforms.h> // for transformPointCloud
 
 #include <pcl/visualization/cloud_viewer.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/io/pcd_io.h>
 
 #include <pcl/filters/passthrough.h>
-#include <pcl/filters/voxel_grid.h>
 #include <pcl/filters/approximate_voxel_grid.h>
 
-#include <pcl/sample_consensus/method_types.h>
-#include <pcl/sample_consensus/model_types.h>
-
-#include <pcl/search/pcl_search.h>
-#include <pcl/common/transforms.h>
-
 #include <pcl/tracking/tracking.h>
 #include <pcl/tracking/particle_filter.h>
 #include <pcl/tracking/kld_adaptive_particle_filter_omp.h>
 #include <pcl/tracking/particle_filter_omp.h>
 #include <pcl/tracking/coherence.h>
 #include <pcl/tracking/distance_coherence.h>
-#include <pcl/tracking/hsv_color_coherence.h>
 #include <pcl/tracking/approx_nearest_pair_point_cloud_coherence.h>
 #include <pcl/tracking/nearest_pair_point_cloud_coherence.h>
 
diff --git a/doc/tutorials/content/sources/vfh_recognition/CMakeLists.txt b/doc/tutorials/content/sources/vfh_recognition/CMakeLists.txt
index 7d25d3f6..7c4a9f1f 100644
--- a/doc/tutorials/content/sources/vfh_recognition/CMakeLists.txt
+++ b/doc/tutorials/content/sources/vfh_recognition/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(vfh_cluster_classifier)
 
diff --git a/doc/tutorials/content/sources/vfh_recognition/build_tree.cpp b/doc/tutorials/content/sources/vfh_recognition/build_tree.cpp
index 142d221d..ecc6cf3e 100644
--- a/doc/tutorials/content/sources/vfh_recognition/build_tree.cpp
+++ b/doc/tutorials/content/sources/vfh_recognition/build_tree.cpp
@@ -1,6 +1,5 @@
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
-#include <pcl/console/parse.h>
 #include <pcl/console/print.h>
 #include <pcl/io/pcd_io.h>
 #include <boost/filesystem.hpp>
@@ -8,6 +7,7 @@
 #include <flann/io/hdf5.h>
 #include <fstream>
 
+
 typedef std::pair<std::string, std::vector<float> > vfh_model;
 
 /** \brief Loads an n-D histogram file as a VFH signature
diff --git a/doc/tutorials/content/sources/vfh_recognition/nearest_neighbors.cpp b/doc/tutorials/content/sources/vfh_recognition/nearest_neighbors.cpp
index 86995357..de785b48 100644
--- a/doc/tutorials/content/sources/vfh_recognition/nearest_neighbors.cpp
+++ b/doc/tutorials/content/sources/vfh_recognition/nearest_neighbors.cpp
@@ -1,7 +1,7 @@
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
 #include <pcl/common/common.h>
-#include <pcl/common/transforms.h>
+#include <pcl/common/centroid.h> // for compute3DCentroid
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/console/parse.h>
 #include <pcl/console/print.h>
@@ -10,7 +10,7 @@
 #include <flann/flann.h>
 #include <flann/io/hdf5.h>
 #include <boost/filesystem.hpp>
-
+#include <boost/algorithm/string/replace.hpp> // for replace_last
 typedef std::pair<std::string, std::vector<float> > vfh_model;
 
 /** \brief Loads an n-D histogram file as a VFH signature
diff --git a/doc/tutorials/content/sources/voxel_grid/CMakeLists.txt b/doc/tutorials/content/sources/voxel_grid/CMakeLists.txt
index 6273daaa..e99999de 100644
--- a/doc/tutorials/content/sources/voxel_grid/CMakeLists.txt
+++ b/doc/tutorials/content/sources/voxel_grid/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(voxel_grid)
 
diff --git a/doc/tutorials/content/sources/voxel_grid/voxel_grid.cpp b/doc/tutorials/content/sources/voxel_grid/voxel_grid.cpp
index 95c52067..a59feef7 100644
--- a/doc/tutorials/content/sources/voxel_grid/voxel_grid.cpp
+++ b/doc/tutorials/content/sources/voxel_grid/voxel_grid.cpp
@@ -4,7 +4,7 @@
 #include <pcl/filters/voxel_grid.h>
 
 int
-main (int argc, char** argv)
+main ()
 {
   pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2 ());
   pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
diff --git a/doc/tutorials/content/statistical_outlier.rst b/doc/tutorials/content/statistical_outlier.rst
index fa8ee28c..e94721ed 100644
--- a/doc/tutorials/content/statistical_outlier.rst
+++ b/doc/tutorials/content/statistical_outlier.rst
@@ -8,7 +8,7 @@ from a point cloud dataset using statistical analysis techniques.
 
 .. raw:: html
   
-  <iframe title="Removing outliers using a StatisticalOutlierRemoval filter" width="480" height="390" src="http://www.youtube.com/embed/RjQPp2_GRnI?rel=0" frameborder="0" allowfullscreen></iframe>
+  <iframe title="Removing outliers using a StatisticalOutlierRemoval filter" width="480" height="390" src="https://www.youtube.com/embed/RjQPp2_GRnI?rel=0" frameborder="0" allowfullscreen></iframe>
 
 Background
 ----------
diff --git a/doc/tutorials/content/template_alignment.rst b/doc/tutorials/content/template_alignment.rst
index f8cd1ce4..4aba8f01 100644
--- a/doc/tutorials/content/template_alignment.rst
+++ b/doc/tutorials/content/template_alignment.rst
@@ -7,7 +7,7 @@ This tutorial gives an example of how some of the tools covered in the other tut
 
 .. raw:: html
 
-  <iframe width="560" height="349" style="margin-left:50px" src="http://www.youtube.com/embed/1T5HxTTgE4I" frameborder="0" allowfullscreen></iframe>
+  <iframe width="560" height="349" style="margin-left:50px" src="https://www.youtube.com/embed/1T5HxTTgE4I" frameborder="0" allowfullscreen></iframe>
 
 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).
 
diff --git a/doc/tutorials/content/tracking.rst b/doc/tutorials/content/tracking.rst
index 3c4df9ea..8625bfc3 100644
--- a/doc/tutorials/content/tracking.rst
+++ b/doc/tutorials/content/tracking.rst
@@ -4,7 +4,7 @@ Tracking object in real time
 ----------------------------
 This tutorial explains 6D object tracking and show example code(tracking_sample.cpp) using pcl::tracking libraries. Implementing this example code, you can see the segment track the target object even if you move tracked object or your sensor device. In example, first, you should initialize tracker and you have to pass target object's point cloud to tracker so that tracker should know what to track. So, before this tutorial, you need to make segmented model with PCD file beforehand. Setting the model to tracker, it starts tracking the object.
 
-Following figure shows how  looks like when trakcing works successfully.
+Following figure shows how  looks like when tracking works successfully.
 
 .. figure:: images/tracking/mergePicture.png
   :height: 600
@@ -60,40 +60,40 @@ Now, let's break down the code piece by piece.
 
 .. literalinclude:: sources/tracking/tracking_sample.cpp
    :language: cpp
-   :lines: 227-242
+   :lines: 219-234
 
 
 First, in main function, these lines set the parameters for tracking. 
 
 .. literalinclude:: sources/tracking/tracking_sample.cpp
    :language: cpp
-   :lines: 246-257
+   :lines: 237-249
 
 Here, we set likelihood function which tracker use when calculate weights.  You can add more likelihood function as you like. By default, there are normals likelihood and color likelihood functions. When you want to add other likelihood function, all you have to do is  initialize new Coherence Class and add the Coherence instance to coherence variable with addPointCoherence function.
 
 .. literalinclude:: sources/tracking/tracking_sample.cpp
    :language: cpp
-   :lines: 259-272
+   :lines: 251-264
 
 In this part, we set the point cloud loaded from pcd file as reference model to tracker and also set model's transform values.
 
 .. literalinclude:: sources/tracking/tracking_sample.cpp
    :language: cpp
-   :lines: 173-180
+   :lines: 165-172
 
 
 Until the counter variable become equal to 10, we ignore the input point cloud, because the point cloud at first few frames often have noise. After counter variable reach to 10 frame, at each loop, we set downsampled input point cloud to tracker and the tracker will compute particles movement.
 
 .. literalinclude:: sources/tracking/tracking_sample.cpp
    :language: cpp
-   :lines: 82-82
+   :lines: 74-74
 
 
 In drawParticles function, you can get particles's positions by calling getParticles().
 
 .. literalinclude:: sources/tracking/tracking_sample.cpp
    :language: cpp
-   :lines: 116-117
+   :lines: 108-109
 
 
 
diff --git a/doc/tutorials/content/using_kinfu_large_scale.rst b/doc/tutorials/content/using_kinfu_large_scale.rst
index 0ff75945..f43916ff 100644
--- a/doc/tutorials/content/using_kinfu_large_scale.rst
+++ b/doc/tutorials/content/using_kinfu_large_scale.rst
@@ -14,7 +14,7 @@ This section describes the TSDF Cloud, which is the expected output of KinFu Lar
 
   .. raw:: html
   
-     <iframe width="420" height="315" src="http://www.youtube.com/embed/AjjSZufyprU" frameborder="0" allowfullscreen></iframe>
+     <iframe width="420" height="315" src="https://www.youtube.com/embed/AjjSZufyprU" frameborder="0" allowfullscreen></iframe>
 
 You may be wondering: *"What is the difference between a TSDF cloud and a normal point cloud?"* Well, a TSDF cloud *is* a point cloud. However, the TSDF cloud makes use of how the data is stored within GPU at KinFu runtime. 
 
@@ -62,7 +62,7 @@ Since we used the *-et* option, you will also find a folder called KinFuSnapshot
 
   .. raw:: html
   
-    <iframe width="420" height="315" src="http://www.youtube.com/embed/rF1N-EEIJao" frameborder="0" allowfullscreen></iframe>
+    <iframe width="420" height="315" src="https://www.youtube.com/embed/rF1N-EEIJao" frameborder="0" allowfullscreen></iframe>
 
 The next part of this tutorial will demonstrate how to get a mesh from the TSDF cloud.
 
@@ -85,7 +85,7 @@ where *world.pcd* is the world model we obtained from KinFu Large Scale. The fol
 
   .. raw:: html
   
-    <iframe width="420" height="315" src="http://www.youtube.com/embed/XMJ-ikSZAOE" frameborder="0" allowfullscreen></iframe>
+    <iframe width="420" height="315" src="https://www.youtube.com/embed/XMJ-ikSZAOE" frameborder="0" allowfullscreen></iframe>
 
 The next part of this tutorial will demonstrate how to generate the texture for the mesh we have just created.
 
@@ -104,7 +104,7 @@ The following video shows the process in detail. It also shows the final output
 
   .. raw:: html
   
-     <iframe width="420" height="315" src="http://www.youtube.com/embed/7S7Jj-4cKHs" frameborder="0" allowfullscreen></iframe>
+     <iframe width="420" height="315" src="https://www.youtube.com/embed/7S7Jj-4cKHs" frameborder="0" allowfullscreen></iframe>
 
 Output
 -------
diff --git a/doc/tutorials/content/voxel_grid.rst b/doc/tutorials/content/voxel_grid.rst
index f00d2603..eaaf6985 100644
--- a/doc/tutorials/content/voxel_grid.rst
+++ b/doc/tutorials/content/voxel_grid.rst
@@ -15,7 +15,7 @@ represents the underlying surface more accurately.
 
 .. raw:: html
   
-  <iframe title="Downsampling a PointCloud using a VoxelGrid filter" width="480" height="390" src="http://www.youtube.com/embed/YHR6_OIxtFI?rel=0" frameborder="0" allowfullscreen></iframe>
+  <iframe title="Downsampling a PointCloud using a VoxelGrid filter" width="480" height="390" src="https://www.youtube.com/embed/YHR6_OIxtFI?rel=0" frameborder="0" allowfullscreen></iframe>
 
 The code
 --------
diff --git a/doc/tutorials/content/walkthrough.rst b/doc/tutorials/content/walkthrough.rst
index 8f3a9baa..33c26903 100644
--- a/doc/tutorials/content/walkthrough.rst
+++ b/doc/tutorials/content/walkthrough.rst
@@ -376,7 +376,7 @@ Sample Consensus
 
 **Background**
 
-	The *sample_consensus* library holds SAmple Consensus (SAC) methods like RANSAC and models like planes and cylinders. These can combined freely in order to detect specific models and their parameters in point clouds.
+	The *sample_consensus* library holds SAmple Consensus (SAC) methods like RANSAC and models like planes and cylinders. These can be combined freely in order to detect specific models and their parameters in point clouds.
 	
 	A theoretical primer explaining how sample consensus algorithms work can be found in the `Random Sample Consensus tutorial <http://pointclouds.org/documentation/tutorials/random_sample_consensus.php#random-sample-consensus>`_
 
@@ -724,42 +724,42 @@ This section provides a quick reference for some of the common tools in PCL.
 
 |
 		
-	* ``pcd_convert_NaN_nan``: converts "NaN" values to "nan" values. *(Note: Starting with PCL version 1.0.1 the string representation for NaN is “nan”.)*
+	* ``pcl_pcd_convert_NaN_nan``: converts "NaN" values to "nan" values. *(Note: Starting with PCL version 1.0.1 the string representation for NaN is “nan”.)*
 		
 		**Usage example:**
 		
-		``pcd_convert_NaN_nan input.pcd output.pcd``
+		``pcl_pcd_convert_NaN_nan input.pcd output.pcd``
 	
-	* ``convert_pcd_ascii_binary``: converts PCD (Point Cloud Data) files from ASCII to binary and viceversa. 
+	* ``pcl_convert_pcd_ascii_binary``: converts PCD (Point Cloud Data) files from ASCII to binary and viceversa. 
 	
 	 	**Usage example:**
 		
-		``convert_pcd_ascii_binary <file_in.pcd> <file_out.pcd> 0/1/2 (ascii/binary/binary_compressed) [precision (ASCII)]``
+		``pcl_convert_pcd_ascii_binary <file_in.pcd> <file_out.pcd> 0/1/2 (ascii/binary/binary_compressed) [precision (ASCII)]``
 		
-	* ``concatenate_points_pcd``: concatenates the points of two or more PCD (Point Cloud Data) files into a single PCD file.
+	* ``pcl_concatenate_points_pcd``: concatenates the points of two or more PCD (Point Cloud Data) files into a single PCD file.
 	 	
 	 	**Usage example:**
 	 	
-	 	``concatenate_points_pcd <filename 1..N.pcd>``
+	 	``pcl_concatenate_points_pcd <filename 1..N.pcd>``
 	 	
 	 	*(Note: the resulting PCD file will be ``output.pcd``)*
 		
 	
-	* ``pcd2vtk``: converts PCD (Point Cloud Data) files to the `VTK format <http://www.vtk.org/VTK/img/file-formats.pdf>`_. 
+	* ``pcl_pcd2vtk``: converts PCD (Point Cloud Data) files to the `VTK format <http://www.vtk.org/VTK/img/file-formats.pdf>`_. 
 	
 		**Usage example:**
 		
-		``pcd2vtk input.pcd output.vtk`` 	
+		``pcl_pcd2vtk input.pcd output.vtk`` 	
 
-	* ``pcd2ply``: converts PCD (Point Cloud Data) files to the `PLY format <http://en.wikipedia.org/wiki/PLY_%28file_format%29>`_. 
+	* ``pcl_pcd2ply``: converts PCD (Point Cloud Data) files to the `PLY format <http://en.wikipedia.org/wiki/PLY_%28file_format%29>`_. 
 
 		**Usage example:**
 
-		``pcd2ply input.pcd output.ply``
+		``pcl_pcd2ply input.pcd output.ply``
 
-	* ``mesh2pcd``: convert a CAD model to a PCD (Point Cloud Data) file, using ray tracing operations.
+	* ``pcl_mesh2pcd``: convert a CAD model to a PCD (Point Cloud Data) file, using ray tracing operations.
 	
-	 	**Syntax is: mesh2pcd input.{ply,obj} output.pcd <options>**, where options are:
+	 	**Syntax is: pcl_mesh2pcd input.{ply,obj} output.pcd <options>**, where options are:
 	 	
 		                     -level X      = tessellated sphere level (default: 2)
 		
@@ -770,13 +770,13 @@ This section provides a quick reference for some of the common tools in PCL.
 
 	.. _`octree_viewer`: 
 	
-	* ``octree_viewer``: allows the visualization of `octrees`__
+	* ``pcl_octree_viewer``: allows the visualization of `octrees`__
 	
 		**Syntax is: octree_viewer <file_name.pcd> <octree resolution>**
 		
 		**Usage example:**
 		
-		``Example: ./octree_viewer ../../test/bunny.pcd 0.02``
+		``Example: ./pcl_octree_viewer ../../test/bunny.pcd 0.02``
 		
 		.. image:: images/octree_bunny2.png
 		
diff --git a/doc/tutorials/content/writing_new_classes.rst b/doc/tutorials/content/writing_new_classes.rst
index b9eccd49..4c53f0e7 100644
--- a/doc/tutorials/content/writing_new_classes.rst
+++ b/doc/tutorials/content/writing_new_classes.rst
@@ -160,7 +160,7 @@ Setting up the structure
 
   If you're not familiar with the PCL file structure already, please go ahead
   and read the `PCL C++ Programming Style Guide
-  <http://www.pointclouds.org/documentation/advanced/pcl_style_guide.php>`_ to
+  <https://pcl.readthedocs.io/projects/advanced/en/latest/pcl_style_guide.html>`_ to
   familiarize yourself with the concepts. 
 
 There're two different ways we could set up the structure: i) set up the code
@@ -193,8 +193,7 @@ skeleton:
 .. code-block:: cpp
    :linenos:
 
-    #ifndef PCL_FILTERS_BILATERAL_H_
-    #define PCL_FILTERS_BILATERAL_H_
+    #pragma once
 
     #include <pcl/filters/filter.h>
 
@@ -206,8 +205,6 @@ skeleton:
       };
     }
 
-    #endif // PCL_FILTERS_BILATERAL_H_
-
 bilateral.hpp
 =============
 
@@ -217,12 +214,9 @@ While we're at it, let's set up two skeleton *bilateral.hpp* and
 .. code-block:: cpp
    :linenos:
 
-    #ifndef PCL_FILTERS_BILATERAL_IMPL_H_
-    #define PCL_FILTERS_BILATERAL_IMPL_H_
+    #pragma once
 
     #include <pcl/filters/bilateral.h>
-    
-    #endif // PCL_FILTERS_BILATERAL_IMPL_H_
 
 This should be straightforward. We haven't declared any methods for
 `BilateralFilter` yet, therefore there is no implementation. 
@@ -511,8 +505,7 @@ header file becomes:
 .. code-block:: cpp
    :linenos:
 
-    #ifndef PCL_FILTERS_BILATERAL_H_
-    #define PCL_FILTERS_BILATERAL_H_
+    #pragma once
 
     #include <pcl/filters/filter.h>
     #include <pcl/kdtree/kdtree.h>
@@ -584,8 +577,6 @@ header file becomes:
       };
     }
 
-    #endif // PCL_FILTERS_BILATERAL_H_
-
 bilateral.hpp
 =============
 
@@ -651,8 +642,7 @@ entry for the class:
 .. code-block:: cpp
    :linenos:
 
-    #ifndef PCL_FILTERS_BILATERAL_IMPL_H_
-    #define PCL_FILTERS_BILATERAL_IMPL_H_
+    #pragma once
 
     #include <pcl/filters/bilateral.h>
 
@@ -660,8 +650,6 @@ entry for the class:
 
     #define PCL_INSTANTIATE_BilateralFilter(T) template class PCL_EXPORTS pcl::BilateralFilter<T>;
 
-    #endif // PCL_FILTERS_BILATERAL_IMPL_H_
-
 One additional thing that we can do is error checking on:
 
  * whether the two `sigma_s_` and `sigma_r_` parameters have been given;
@@ -709,8 +697,7 @@ The implementation file header thus becomes:
 .. code-block:: cpp
    :linenos:
 
-    #ifndef PCL_FILTERS_BILATERAL_IMPL_H_
-    #define PCL_FILTERS_BILATERAL_IMPL_H_
+    #pragma once
 
     #include <pcl/filters/bilateral.h>
     #include <pcl/kdtree/kdtree_flann.h>
@@ -770,8 +757,6 @@ The implementation file header thus becomes:
      
     #define PCL_INSTANTIATE_BilateralFilter(T) template class PCL_EXPORTS pcl::BilateralFilter<T>;
 
-    #endif // PCL_FILTERS_BILATERAL_IMPL_H_
-
 
 Taking advantage of other PCL concepts
 --------------------------------------
@@ -824,8 +809,7 @@ The implementation file header thus becomes:
 .. code-block:: cpp
    :linenos:
 
-    #ifndef PCL_FILTERS_BILATERAL_IMPL_H_
-    #define PCL_FILTERS_BILATERAL_IMPL_H_
+    #pragma once
 
     #include <pcl/filters/bilateral.h>
     #include <pcl/kdtree/kdtree_flann.h>
@@ -885,8 +869,6 @@ The implementation file header thus becomes:
      
     #define PCL_INSTANTIATE_BilateralFilter(T) template class PCL_EXPORTS pcl::BilateralFilter<T>;
 
-    #endif // PCL_FILTERS_BILATERAL_IMPL_H_
-
 To make :pcl:`indices_<pcl::PCLBase::indices_>` work without typing the full
 construct, we need to add a new line to *bilateral.h* that specifies the class
 where `indices_` is declared:
@@ -918,42 +900,14 @@ file, as follows:
 .. code-block:: cpp
    :linenos:
 
-    /*
-     * Software License Agreement (BSD License)
-     *
-     *  Point Cloud Library (PCL) - www.pointclouds.org
-     *  Copyright (c) 2010-2011, Willow Garage, Inc.
-     *
-     *  All rights reserved.
-     *
-     *  Redistribution and use in source and binary forms, with or without
-     *  modification, are permitted provided that the following conditions
-     *  are met:
-     *
-     *   * Redistributions of source code must retain the above copyright
-     *     notice, this list of conditions and the following disclaimer.
-     *   * Redistributions in binary form must reproduce the above
-     *     copyright notice, this list of conditions and the following
-     *     disclaimer in the documentation and/or other materials provided
-     *     with the distribution.
-     *   * Neither the name of Willow Garage, Inc. nor the names of its
-     *     contributors may be used to endorse or promote products derived
-     *     from this software without specific prior written permission.
-     *
-     *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-     *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-     *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-     *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-     *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-     *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-     *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-     *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-     *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-     *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-     *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-     *  POSSIBILITY OF SUCH DAMAGE.
-     *
-     */
+   /*
+   * SPDX-License-Identifier: BSD-3-Clause
+   *
+   *  Point Cloud Library (PCL) - www.pointclouds.org
+   *  Copyright (c) 2014-, Open Perception Inc.
+   *
+   *  All rights reserved
+   */
 
 An additional like can be inserted if additional copyright is needed (or the
 original copyright can be changed):
@@ -983,45 +937,16 @@ class look like:
 .. code-block:: cpp
    :linenos:
 
-    /*
-     * Software License Agreement (BSD License)
-     *
-     *  Point Cloud Library (PCL) - www.pointclouds.org
-     *  Copyright (c) 2010-2011, Willow Garage, Inc.
-     *
-     *  All rights reserved.
-     *
-     *  Redistribution and use in source and binary forms, with or without
-     *  modification, are permitted provided that the following conditions
-     *  are met:
-     *
-     *   * Redistributions of source code must retain the above copyright
-     *     notice, this list of conditions and the following disclaimer.
-     *   * Redistributions in binary form must reproduce the above
-     *     copyright notice, this list of conditions and the following
-     *     disclaimer in the documentation and/or other materials provided
-     *     with the distribution.
-     *   * Neither the name of Willow Garage, Inc. nor the names of its
-     *     contributors may be used to endorse or promote products derived
-     *     from this software without specific prior written permission.
-     *
-     *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-     *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-     *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-     *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-     *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-     *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-     *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-     *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-     *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-     *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-     *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-     *  POSSIBILITY OF SUCH DAMAGE.
-     *
-     */
-
-    #ifndef PCL_FILTERS_BILATERAL_H_
-    #define PCL_FILTERS_BILATERAL_H_
+   /*
+   * SPDX-License-Identifier: BSD-3-Clause
+   *
+   *  Point Cloud Library (PCL) - www.pointclouds.org
+   *  Copyright (c) 2014-, Open Perception Inc.
+   *
+   *  All rights reserved
+   */
+
+    #pragma once
 
     #include <pcl/filters/filter.h>
     #include <pcl/kdtree/kdtree.h>
@@ -1131,52 +1056,21 @@ class look like:
       };
     }
 
-    #endif // PCL_FILTERS_BILATERAL_H_
-
 And the *bilateral.hpp* likes:
 
 .. code-block:: cpp
    :linenos:
 
-    /*
-     * Software License Agreement (BSD License)
-     *
-     *  Point Cloud Library (PCL) - www.pointclouds.org
-     *  Copyright (c) 2010-2011, Willow Garage, Inc.
-     *
-     *  All rights reserved.
-     *
-     *  Redistribution and use in source and binary forms, with or without
-     *  modification, are permitted provided that the following conditions
-     *  are met:
-     *
-     *   * Redistributions of source code must retain the above copyright
-     *     notice, this list of conditions and the following disclaimer.
-     *   * Redistributions in binary form must reproduce the above
-     *     copyright notice, this list of conditions and the following
-     *     disclaimer in the documentation and/or other materials provided
-     *     with the distribution.
-     *   * Neither the name of Willow Garage, Inc. nor the names of its
-     *     contributors may be used to endorse or promote products derived
-     *     from this software without specific prior written permission.
-     *
-     *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-     *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-     *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-     *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-     *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-     *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-     *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-     *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-     *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-     *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-     *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-     *  POSSIBILITY OF SUCH DAMAGE.
-     *
-     */
-
-    #ifndef PCL_FILTERS_BILATERAL_IMPL_H_
-    #define PCL_FILTERS_BILATERAL_IMPL_H_
+   /*
+   * SPDX-License-Identifier: BSD-3-Clause
+   *
+   *  Point Cloud Library (PCL) - www.pointclouds.org
+   *  Copyright (c) 2014-, Open Perception Inc.
+   *
+   *  All rights reserved
+   */
+
+    #pragma once
 
     #include <pcl/filters/bilateral.h>
     #include <pcl/kdtree/kdtree_flann.h>
@@ -1249,8 +1143,6 @@ And the *bilateral.hpp* likes:
      
     #define PCL_INSTANTIATE_BilateralFilter(T) template class PCL_EXPORTS pcl::BilateralFilter<T>;
 
-    #endif // PCL_FILTERS_BILATERAL_IMPL_H_
-
 
 Testing the new class
 ---------------------
diff --git a/examples/features/example_difference_of_normals.cpp b/examples/features/example_difference_of_normals.cpp
index 88c4cb3c..ef7af74c 100644
--- a/examples/features/example_difference_of_normals.cpp
+++ b/examples/features/example_difference_of_normals.cpp
@@ -212,7 +212,7 @@ int main (int argc, char *argv[])
   for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it, j++)
   {
     pcl::PointCloud<PointOutT>::Ptr cloud_cluster_don (new pcl::PointCloud<PointOutT>);
-    for (const int &index : it->indices){
+    for (const auto &index : it->indices){
       cloud_cluster_don->points.push_back ((*doncloud)[index]);
     }
 
diff --git a/examples/features/example_fast_point_feature_histograms.cpp b/examples/features/example_fast_point_feature_histograms.cpp
index ef8c96f6..87bd19cb 100644
--- a/examples/features/example_fast_point_feature_histograms.cpp
+++ b/examples/features/example_fast_point_feature_histograms.cpp
@@ -58,7 +58,7 @@ main (int argc, char** argv)
 
   if (pcl::io::loadPCDFile<pcl::PointXYZ> (fileName, *cloud) == -1) // load the file
   {
-    PCL_ERROR ("Couldn't read file");
+    PCL_ERROR ("Couldn't read file\n");
     return (-1);
   }
 
diff --git a/examples/features/example_normal_estimation.cpp b/examples/features/example_normal_estimation.cpp
index 94148a22..613b01be 100644
--- a/examples/features/example_normal_estimation.cpp
+++ b/examples/features/example_normal_estimation.cpp
@@ -52,7 +52,7 @@ main (int, char** argv)
 
   if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud) == -1) // load the file
   {
-    PCL_ERROR ("Couldn't read file");
+    PCL_ERROR ("Couldn't read file\n");
     return -1;
   }
 
diff --git a/examples/features/example_point_feature_histograms.cpp b/examples/features/example_point_feature_histograms.cpp
index ee5996c6..1f4e3a0a 100644
--- a/examples/features/example_point_feature_histograms.cpp
+++ b/examples/features/example_point_feature_histograms.cpp
@@ -53,7 +53,7 @@ main (int, char** argv)
 
   if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud) == -1) //* load the file
   {
-    PCL_ERROR ("Couldn't read file");
+    PCL_ERROR ("Couldn't read file\n");
     return (-1);
   }
 
diff --git a/examples/features/example_principal_curvatures_estimation.cpp b/examples/features/example_principal_curvatures_estimation.cpp
index 6f7996e4..ef94ce28 100644
--- a/examples/features/example_principal_curvatures_estimation.cpp
+++ b/examples/features/example_principal_curvatures_estimation.cpp
@@ -53,7 +53,7 @@ main (int, char** argv)
 
   if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud) == -1) //* load the file
   {
-    PCL_ERROR ("Couldn't read file");
+    PCL_ERROR ("Couldn't read file\n");
     return (-1);
   }
 
diff --git a/examples/features/example_rift_estimation.cpp b/examples/features/example_rift_estimation.cpp
index ab2416a2..f880b8d5 100644
--- a/examples/features/example_rift_estimation.cpp
+++ b/examples/features/example_rift_estimation.cpp
@@ -55,7 +55,7 @@ main (int, char** argv)
 
   if (pcl::io::loadPCDFile<pcl::PointXYZI> (filename, *cloud) == -1) // load the file
   {
-    PCL_ERROR ("Couldn't read file");
+    PCL_ERROR ("Couldn't read file\n");
     return -1;
   }
 
diff --git a/examples/features/example_shape_contexts.cpp b/examples/features/example_shape_contexts.cpp
index 34dddf83..84f21338 100644
--- a/examples/features/example_shape_contexts.cpp
+++ b/examples/features/example_shape_contexts.cpp
@@ -53,7 +53,7 @@ main (int, char** argv)
   if (pcl::io::loadPCDFile <pcl::PointXYZ> (filename, *cloud) == -1)
   // load the file
   {
-    PCL_ERROR ("Couldn't read file");
+    PCL_ERROR ("Couldn't read file\n");
     return (-1);
   }
   std::cout << "Loaded " << cloud->size () << " points." << std::endl;
diff --git a/examples/features/example_spin_images.cpp b/examples/features/example_spin_images.cpp
index 3638cc08..68a54cdc 100644
--- a/examples/features/example_spin_images.cpp
+++ b/examples/features/example_spin_images.cpp
@@ -53,7 +53,7 @@ main (int, char** argv)
   if (pcl::io::loadPCDFile <pcl::PointXYZ> (filename, *cloud) == -1)
   // load the file
   {
-    PCL_ERROR ("Couldn't read file");
+    PCL_ERROR("Couldn't read file\n");
     return (-1);
   }
   std::cout << "Loaded " << cloud->size () << " points." << std::endl;
diff --git a/examples/filters/example_remove_nan_from_point_cloud.cpp b/examples/filters/example_remove_nan_from_point_cloud.cpp
index 762ca062..5fc2d45c 100644
--- a/examples/filters/example_remove_nan_from_point_cloud.cpp
+++ b/examples/filters/example_remove_nan_from_point_cloud.cpp
@@ -61,7 +61,7 @@ main (int, char**)
 
   std::cout << "size: " << cloud->size () << std::endl;
 
-  std::vector<int> indices;
+  pcl::Indices indices;
   pcl::removeNaNFromPointCloud(*cloud, *output_cloud, indices);
   std::cout << "size: " << output_cloud->size () << std::endl;
 
diff --git a/examples/keypoints/CMakeLists.txt b/examples/keypoints/CMakeLists.txt
index 133bfed8..9bc3bc3b 100644
--- a/examples/keypoints/CMakeLists.txt
+++ b/examples/keypoints/CMakeLists.txt
@@ -6,12 +6,11 @@ PCL_SUBSYS_DEPEND(build ${SUBSYS_NAME} DEPS visualization)
 
 ## Find VTK
 if(NOT VTK_FOUND)
-    set(DEFAULT FALSE)
-    set(REASON "VTK was not found.")
+  set(DEFAULT FALSE)
+  set(REASON "VTK was not found.")
 else()
-    set(DEFAULT TRUE)
-    set(REASON)
-    include(${VTK_USE_FILE})
+  set(DEFAULT TRUE)
+  set(REASON)
 endif()
 
 PCL_ADD_EXAMPLE(pcl_example_sift_keypoint_estimation FILES example_sift_keypoint_estimation.cpp
diff --git a/examples/keypoints/example_get_keypoints_indices.cpp b/examples/keypoints/example_get_keypoints_indices.cpp
index f2b6afeb..ff26342b 100644
--- a/examples/keypoints/example_get_keypoints_indices.cpp
+++ b/examples/keypoints/example_get_keypoints_indices.cpp
@@ -35,6 +35,7 @@
  *
  */
 
+#include <pcl/common/time.h> // for StopWatch
 #include <pcl/io/pcd_io.h>
 #include <pcl/keypoints/harris_3d.h>
 
diff --git a/examples/keypoints/example_sift_keypoint_estimation.cpp b/examples/keypoints/example_sift_keypoint_estimation.cpp
index 53173bcd..612e070e 100644
--- a/examples/keypoints/example_sift_keypoint_estimation.cpp
+++ b/examples/keypoints/example_sift_keypoint_estimation.cpp
@@ -51,7 +51,7 @@ main(int, char** argv)
   pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
   if(pcl::io::loadPCDFile<pcl::PointXYZRGB> (filename, *cloud) == -1) // load the file
   {
-  PCL_ERROR ("Couldn't read file");
+  PCL_ERROR ("Couldn't read file\n");
   return -1;
   }
   std::cout << "points: " << cloud->size () <<std::endl;
diff --git a/examples/keypoints/example_sift_normal_keypoint_estimation.cpp b/examples/keypoints/example_sift_normal_keypoint_estimation.cpp
index b2aecf15..c4c1905b 100644
--- a/examples/keypoints/example_sift_normal_keypoint_estimation.cpp
+++ b/examples/keypoints/example_sift_normal_keypoint_estimation.cpp
@@ -57,7 +57,7 @@ main(int, char** argv)
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
   if(pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud_xyz) == -1) // load the file
   {
-    PCL_ERROR ("Couldn't read file");
+    PCL_ERROR("Couldn't read file\n");
     return -1;
   }
   std::cout << "points: " << cloud_xyz->size () <<std::endl;
diff --git a/examples/keypoints/example_sift_z_keypoint_estimation.cpp b/examples/keypoints/example_sift_z_keypoint_estimation.cpp
index 9256a575..146f03f5 100644
--- a/examples/keypoints/example_sift_z_keypoint_estimation.cpp
+++ b/examples/keypoints/example_sift_z_keypoint_estimation.cpp
@@ -69,7 +69,7 @@ main(int, char** argv)
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
   if(pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud_xyz) == -1) // load the file
   {
-    PCL_ERROR ("Couldn't read file");
+    PCL_ERROR("Couldn't read file\n");
     return -1;
   }
   std::cout << "points: " << cloud_xyz->size () <<std::endl;
diff --git a/examples/outofcore/CMakeLists.txt b/examples/outofcore/CMakeLists.txt
index aed932e9..6c624f0e 100644
--- a/examples/outofcore/CMakeLists.txt
+++ b/examples/outofcore/CMakeLists.txt
@@ -9,7 +9,6 @@ if(NOT VTK_FOUND)
 else()
   set(DEFAULT TRUE)
   set(REASON)
-  include(${VTK_USE_FILE})
 endif()
 
 PCL_SUBSYS_DEPEND (build ${SUBSYS_NAME} DEPS outofcore io common octree filters visualization EXT_DEPS vtk)
diff --git a/examples/segmentation/CMakeLists.txt b/examples/segmentation/CMakeLists.txt
index 5e672fd8..b788c4c6 100644
--- a/examples/segmentation/CMakeLists.txt
+++ b/examples/segmentation/CMakeLists.txt
@@ -11,7 +11,6 @@ PCL_ADD_EXAMPLE(pcl_example_region_growing FILES example_region_growing.cpp
 
 ## Find VTK
 if(VTK_FOUND)
-  include(${VTK_USE_FILE})
   PCL_ADD_EXAMPLE(pcl_example_supervoxels FILES example_supervoxels.cpp
                   LINK_WITH pcl_common pcl_features pcl_segmentation pcl_octree pcl_kdtree pcl_visualization)
   PCL_ADD_EXAMPLE(pcl_example_lccp_segmentation FILES example_lccp_segmentation.cpp
diff --git a/examples/segmentation/example_extract_clusters_normals.cpp b/examples/segmentation/example_extract_clusters_normals.cpp
index 31426c75..0d61899d 100644
--- a/examples/segmentation/example_extract_clusters_normals.cpp
+++ b/examples/segmentation/example_extract_clusters_normals.cpp
@@ -87,7 +87,7 @@ main (int, char **argv)
   for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
   {
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
-    for (const int &index : it->indices)
+    for (const auto &index : it->indices)
       cloud_cluster->push_back ((*cloud_ptr)[index]); 
     cloud_cluster->width = cloud_cluster->size ();
     cloud_cluster->height = 1;
diff --git a/examples/segmentation/example_region_growing.cpp b/examples/segmentation/example_region_growing.cpp
index d5913483..89c38b5b 100644
--- a/examples/segmentation/example_region_growing.cpp
+++ b/examples/segmentation/example_region_growing.cpp
@@ -74,7 +74,7 @@ main (int argc, char** av)
   // Remove the nans
   cloud_ptr->is_dense = false;
   cloud_no_nans->is_dense = false;
-  std::vector<int> indices;
+  pcl::Indices indices;
   pcl::removeNaNFromPointCloud (*cloud_ptr, *cloud_no_nans, indices);
   pcl::console::print_highlight("Removed nans from %zu to %zu\n",
                                 static_cast<std::size_t>(cloud_ptr->size()),
@@ -114,11 +114,9 @@ main (int argc, char** av)
     clusters_file.open ("clusters.dat");
     for (std::size_t i = 0; i < clusters.size (); ++i)
     {
-      clusters_file << i << "#" << clusters[i].indices.size () << ": ";
-      std::vector<int>::const_iterator pit = clusters[i].indices.begin ();
-      clusters_file << *pit;
-      for (; pit != clusters[i].indices.end (); ++pit)
-        clusters_file << " " << *pit;
+      clusters_file << i << "#" << clusters[i].indices.size () << ":";
+      for (const auto& cluster_idx : clusters[i].indices)
+        clusters_file << " " << cluster_idx;
       clusters_file << std::endl;
     }
     clusters_file.close ();
diff --git a/examples/stereo/CMakeLists.txt b/examples/stereo/CMakeLists.txt
index 518d1049..1f64b1cf 100644
--- a/examples/stereo/CMakeLists.txt
+++ b/examples/stereo/CMakeLists.txt
@@ -11,7 +11,6 @@ if(NOT VTK_FOUND)
 else()
   set(DEFAULT TRUE)
   set(REASON)
-  include(${VTK_USE_FILE})
 endif()
 
 PCL_ADD_EXAMPLE(pcl_example_stereo_baseline FILES example_stereo_baseline.cpp
diff --git a/examples/surface/CMakeLists.txt b/examples/surface/CMakeLists.txt
index ec9f030d..0cc5d7d2 100644
--- a/examples/surface/CMakeLists.txt
+++ b/examples/surface/CMakeLists.txt
@@ -11,7 +11,6 @@ if(NOT VTK_FOUND)
 else()
   set(DEFAULT TRUE)
   set(REASON)
-  include(${VTK_USE_FILE})
 endif()
 
 PCL_ADD_EXAMPLE(pcl_test_nurbs_fitting_surface
diff --git a/examples/surface/example_nurbs_viewer_surface.cpp b/examples/surface/example_nurbs_viewer_surface.cpp
index 3a71431a..d77d7f02 100644
--- a/examples/surface/example_nurbs_viewer_surface.cpp
+++ b/examples/surface/example_nurbs_viewer_surface.cpp
@@ -1,5 +1,3 @@
-#include <pcl/point_cloud.h>
-
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/surface/on_nurbs/triangulation.h>
 
diff --git a/features/include/pcl/features/3dsc.h b/features/include/pcl/features/3dsc.h
index 2d8d41cb..0d5bba72 100644
--- a/features/include/pcl/features/3dsc.h
+++ b/features/include/pcl/features/3dsc.h
@@ -43,7 +43,6 @@
 #include <random>
 
 #include <pcl/point_types.h>
-#include <pcl/features/boost.h>
 #include <pcl/features/feature.h>
 
 namespace pcl
diff --git a/features/include/pcl/features/board.h b/features/include/pcl/features/board.h
index 693db2cd..40896251 100644
--- a/features/include/pcl/features/board.h
+++ b/features/include/pcl/features/board.h
@@ -282,7 +282,7 @@ namespace pcl
         * \param[in,out] normal the normal to disambiguate, the calculation is performed in place
         */
       void
-      normalDisambiguation (pcl::PointCloud<PointNT> const &normals_cloud, std::vector<int> const &normal_indices,
+      normalDisambiguation (pcl::PointCloud<PointNT> const &normals_cloud, pcl::Indices const &normal_indices,
                             Eigen::Vector3f &normal);
 
       /** \brief Compute Least Square Plane Fitting in a set of 3D points
diff --git a/features/include/pcl/features/boost.h b/features/include/pcl/features/boost.h
index 53f51c52..4f49824a 100644
--- a/features/include/pcl/features/boost.h
+++ b/features/include/pcl/features/boost.h
@@ -42,5 +42,5 @@
 #if defined __GNUC__
 #  pragma GCC system_header 
 #endif
-
+PCL_DEPRECATED_HEADER(1, 15, "Please include the needed boost headers directly.")
 #include <boost/property_map/property_map.hpp>
diff --git a/features/include/pcl/features/boundary.h b/features/include/pcl/features/boundary.h
index 98d28108..c9bc22ad 100644
--- a/features/include/pcl/features/boundary.h
+++ b/features/include/pcl/features/boundary.h
@@ -40,7 +40,6 @@
 
 #pragma once
 
-#include <pcl/features/eigen.h>
 #include <pcl/features/feature.h>
 
 namespace pcl
@@ -116,7 +115,7 @@ namespace pcl
         */
       bool 
       isBoundaryPoint (const pcl::PointCloud<PointInT> &cloud, 
-                       int q_idx, const std::vector<int> &indices, 
+                       int q_idx, const pcl::Indices &indices, 
                        const Eigen::Vector4f &u, const Eigen::Vector4f &v, const float angle_threshold);
 
       /** \brief Check whether a point is a boundary point in a planar patch of projected points given by indices.
@@ -131,7 +130,7 @@ namespace pcl
       bool 
       isBoundaryPoint (const pcl::PointCloud<PointInT> &cloud, 
                        const PointInT &q_point, 
-                       const std::vector<int> &indices, 
+                       const pcl::Indices &indices, 
                        const Eigen::Vector4f &u, const Eigen::Vector4f &v, const float angle_threshold);
 
       /** \brief Set the decision boundary (angle threshold) that marks points as boundary or regular. 
diff --git a/features/include/pcl/features/brisk_2d.h b/features/include/pcl/features/brisk_2d.h
index a40d799f..61433878 100644
--- a/features/include/pcl/features/brisk_2d.h
+++ b/features/include/pcl/features/brisk_2d.h
@@ -41,7 +41,6 @@
 
 // PCL includes
 #include <pcl/features/feature.h>
-#include <pcl/common/eigen.h>
 #include <pcl/common/centroid.h>
 #include <pcl/common/intensity.h>
 
diff --git a/features/include/pcl/features/cppf.h b/features/include/pcl/features/cppf.h
index 84d283ea..789f01c1 100755
--- a/features/include/pcl/features/cppf.h
+++ b/features/include/pcl/features/cppf.h
@@ -40,7 +40,6 @@
 #pragma once
 
 #include <pcl/features/feature.h>
-#include <pcl/features/boost.h>
 
 namespace pcl
 {
diff --git a/features/include/pcl/features/cvfh.h b/features/include/pcl/features/cvfh.h
index a7b20672..98315b19 100644
--- a/features/include/pcl/features/cvfh.h
+++ b/features/include/pcl/features/cvfh.h
@@ -42,8 +42,7 @@
 
 #include <pcl/features/feature.h>
 #include <pcl/features/vfh.h>
-#include <pcl/search/pcl_search.h>
-#include <pcl/common/common.h>
+#include <pcl/search/search.h> // for Search
 
 namespace pcl
 {
@@ -103,8 +102,8 @@ namespace pcl
         * \param[in] threshold threshold value for curvature
         */
       void
-      filterNormalsWithHighCurvature (const pcl::PointCloud<PointNT> & cloud, std::vector<int> & indices_to_use, std::vector<int> &indices_out,
-                                      std::vector<int> &indices_in, float threshold);
+      filterNormalsWithHighCurvature (const pcl::PointCloud<PointNT> & cloud, pcl::Indices & indices_to_use, pcl::Indices &indices_out,
+                                      pcl::Indices &indices_in, float threshold);
 
       /** \brief Set the viewpoint.
         * \param[in] vpx the X coordinate of the viewpoint
diff --git a/features/include/pcl/features/eigen.h b/features/include/pcl/features/eigen.h
index 3fcbb278..56387488 100644
--- a/features/include/pcl/features/eigen.h
+++ b/features/include/pcl/features/eigen.h
@@ -42,6 +42,6 @@
 #if defined __GNUC__
 #  pragma GCC system_header 
 #endif
-
+PCL_DEPRECATED_HEADER(1, 15, "Please include the needed eigen headers directly.")
 #include <Eigen/StdVector>
 #include <Eigen/Geometry>
diff --git a/features/include/pcl/features/feature.h b/features/include/pcl/features/feature.h
index 4a24a658..bafe46a4 100644
--- a/features/include/pcl/features/feature.h
+++ b/features/include/pcl/features/feature.h
@@ -123,8 +123,8 @@ namespace pcl
 
       using PointCloudOut = pcl::PointCloud<PointOutT>;
 
-      using SearchMethod = std::function<int (std::size_t, double, std::vector<int> &, std::vector<float> &)>;
-      using SearchMethodSurface = std::function<int (const PointCloudIn &cloud, std::size_t index, double, std::vector<int> &, std::vector<float> &)>;
+      using SearchMethod = std::function<int (std::size_t, double, pcl::Indices &, std::vector<float> &)>;
+      using SearchMethodSurface = std::function<int (const PointCloudIn &cloud, std::size_t index, double, pcl::Indices &, std::vector<float> &)>;
 
     public:
       /** \brief Empty constructor. */
@@ -269,7 +269,7 @@ namespace pcl
         */
       inline int
       searchForNeighbors (std::size_t index, double parameter,
-                          std::vector<int> &indices, std::vector<float> &distances) const
+                          pcl::Indices &indices, std::vector<float> &distances) const
       {
         return (search_method_surface_ (*input_, index, parameter, indices, distances));
       }
@@ -287,7 +287,7 @@ namespace pcl
         */
       inline int
       searchForNeighbors (const PointCloudIn &cloud, std::size_t index, double parameter,
-                          std::vector<int> &indices, std::vector<float> &distances) const
+                          pcl::Indices &indices, std::vector<float> &distances) const
       {
         return (search_method_surface_ (cloud, index, parameter, indices, distances));
       }
diff --git a/features/include/pcl/features/fpfh.h b/features/include/pcl/features/fpfh.h
index 5d3b64b4..f2e74275 100644
--- a/features/include/pcl/features/fpfh.h
+++ b/features/include/pcl/features/fpfh.h
@@ -41,7 +41,6 @@
 #pragma once
 
 #include <pcl/features/feature.h>
-#include <set>
 
 namespace pcl
 {
@@ -130,8 +129,8 @@ namespace pcl
         */
       void 
       computePointSPFHSignature (const pcl::PointCloud<PointInT> &cloud, 
-                                 const pcl::PointCloud<PointNT> &normals, int p_idx, int row, 
-                                 const std::vector<int> &indices, 
+                                 const pcl::PointCloud<PointNT> &normals, pcl::index_t p_idx, int row, 
+                                 const pcl::Indices &indices, 
                                  Eigen::MatrixXf &hist_f1, Eigen::MatrixXf &hist_f2, Eigen::MatrixXf &hist_f3);
 
       /** \brief Weight the SPFH (Simple Point Feature Histograms) individual histograms to create the final FPFH
@@ -147,7 +146,7 @@ namespace pcl
       weightPointSPFHSignature (const Eigen::MatrixXf &hist_f1, 
                                 const Eigen::MatrixXf &hist_f2, 
                                 const Eigen::MatrixXf &hist_f3, 
-                                const std::vector<int> &indices, 
+                                const pcl::Indices &indices, 
                                 const std::vector<float> &dists, 
                                 Eigen::VectorXf &fpfh_histogram);
 
diff --git a/features/include/pcl/features/from_meshes.h b/features/include/pcl/features/from_meshes.h
index 30ae9070..f8d6b02d 100644
--- a/features/include/pcl/features/from_meshes.h
+++ b/features/include/pcl/features/from_meshes.h
@@ -21,7 +21,7 @@ namespace pcl
       normals.header = cloud.header;
       normals.width = cloud.width;
       normals.height = cloud.height;
-      normals.points.resize(nr_points);
+      normals.resize(nr_points);
 
       for (auto& point: normals.points)
         point.getNormalVector3fMap() = Eigen::Vector3f::Zero();
diff --git a/features/include/pcl/features/gasd.h b/features/include/pcl/features/gasd.h
index c0e39748..b0c912a8 100644
--- a/features/include/pcl/features/gasd.h
+++ b/features/include/pcl/features/gasd.h
@@ -39,8 +39,6 @@
 #pragma once
 
 #include <pcl/features/feature.h>
-#include <pcl/common/common.h>
-#include <pcl/point_cloud.h>
 
 namespace pcl
 {
diff --git a/features/include/pcl/features/gfpfh.h b/features/include/pcl/features/gfpfh.h
index d42f4b7c..02ee2af5 100644
--- a/features/include/pcl/features/gfpfh.h
+++ b/features/include/pcl/features/gfpfh.h
@@ -133,7 +133,7 @@ namespace pcl
 
       /** \brief Return the dominant label of a set of points. */
       std::uint32_t
-      getDominantLabel (const std::vector<int>& indices);
+      getDominantLabel (const pcl::Indices& indices);
 
       /** \brief Compute the fixed-length histograms of transitions. */
       void computeTransitionHistograms (const std::vector< std::vector<int> >& label_histograms,
diff --git a/features/include/pcl/features/grsd.h b/features/include/pcl/features/grsd.h
index 4f9cbfdf..cc395c09 100644
--- a/features/include/pcl/features/grsd.h
+++ b/features/include/pcl/features/grsd.h
@@ -39,7 +39,6 @@
 #pragma once
 
 #include <pcl/features/feature.h>
-#include <pcl/features/rsd.h>
 #include <pcl/filters/voxel_grid.h>
 
 namespace pcl
diff --git a/features/include/pcl/features/impl/3dsc.hpp b/features/include/pcl/features/impl/3dsc.hpp
index 2a904d92..3a3a5c28 100644
--- a/features/include/pcl/features/impl/3dsc.hpp
+++ b/features/include/pcl/features/impl/3dsc.hpp
@@ -46,7 +46,7 @@
 #include <pcl/common/utils.h>
 
 #include <cmath>
-
+#include <numeric> // for partial_sum
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointInT, typename PointNT, typename PointOutT> bool
@@ -139,7 +139,7 @@ pcl::ShapeContext3DEstimation<PointInT, PointNT, PointOutT>::computePoint (
   Eigen::Map<Eigen::Vector3f> normal (rf + 6);
 
   // Find every point within specified search_radius_
-  std::vector<int> nn_indices;
+  pcl::Indices nn_indices;
   std::vector<float> nn_dists;
   const std::size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
   if (neighb_cnt == 0)
@@ -218,7 +218,7 @@ pcl::ShapeContext3DEstimation<PointInT, PointNT, PointOutT>::computePoint (
     const auto l = std::distance(phi_divisions_.cbegin (), std::prev(phi_min));
 
     // Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour
-    std::vector<int> neighbour_indices;
+    pcl::Indices neighbour_indices;
     std::vector<float> neighbour_distances;
     int point_density = searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_distances);
     // point_density is NOT always bigger than 0 (on error, searchForNeighbors returns 0), so we must check for that
diff --git a/features/include/pcl/features/impl/board.hpp b/features/include/pcl/features/impl/board.hpp
index 2305e767..8abf95fe 100644
--- a/features/include/pcl/features/impl/board.hpp
+++ b/features/include/pcl/features/impl/board.hpp
@@ -157,13 +157,13 @@ pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::planeFitt
 template<typename PointInT, typename PointNT, typename PointOutT> void
 pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::normalDisambiguation (
                                                                                              pcl::PointCloud<PointNT> const &normal_cloud,
-                                                                                             std::vector<int> const &normal_indices,
+                                                                                             pcl::Indices const &normal_indices,
                                                                                              Eigen::Vector3f &normal)
 {
   Eigen::Vector3f normal_mean;
   normal_mean.setZero ();
 
-  for (const int &normal_index : normal_indices)
+  for (const auto &normal_index : normal_indices)
   {
     const PointNT& curPt = normal_cloud[normal_index];
 
@@ -186,7 +186,7 @@ pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::computePo
   //find Z axis
 
   //extract support points for Rz radius
-  std::vector<int> neighbours_indices;
+  pcl::Indices neighbours_indices;
   std::vector<float> neighbours_distances;
   std::size_t n_neighbours = this->searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances);
 
diff --git a/features/include/pcl/features/impl/boundary.hpp b/features/include/pcl/features/impl/boundary.hpp
index e83303bb..a127c6d2 100644
--- a/features/include/pcl/features/impl/boundary.hpp
+++ b/features/include/pcl/features/impl/boundary.hpp
@@ -50,7 +50,7 @@
 template <typename PointInT, typename PointNT, typename PointOutT> bool
 pcl::BoundaryEstimation<PointInT, PointNT, PointOutT>::isBoundaryPoint (
       const pcl::PointCloud<PointInT> &cloud, int q_idx, 
-      const std::vector<int> &indices, 
+      const pcl::Indices &indices, 
       const Eigen::Vector4f &u, const Eigen::Vector4f &v, 
       const float angle_threshold)
 {
@@ -61,7 +61,7 @@ pcl::BoundaryEstimation<PointInT, PointNT, PointOutT>::isBoundaryPoint (
 template <typename PointInT, typename PointNT, typename PointOutT> bool
 pcl::BoundaryEstimation<PointInT, PointNT, PointOutT>::isBoundaryPoint (
       const pcl::PointCloud<PointInT> &cloud, const PointInT &q_point, 
-      const std::vector<int> &indices, 
+      const pcl::Indices &indices, 
       const Eigen::Vector4f &u, const Eigen::Vector4f &v, 
       const float angle_threshold)
 {
@@ -76,7 +76,7 @@ pcl::BoundaryEstimation<PointInT, PointNT, PointOutT>::isBoundaryPoint (
   float max_dif = FLT_MIN, dif;
   int cp = 0;
 
-  for (const int &index : indices)
+  for (const auto &index : indices)
   {
     if (!std::isfinite (cloud[index].x) || 
         !std::isfinite (cloud[index].y) || 
@@ -117,7 +117,7 @@ pcl::BoundaryEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointClou
 {
   // Allocate enough space to hold the results
   // \note This resize is irrelevant for a radiusSearch ().
-  std::vector<int> nn_indices (k_);
+  pcl::Indices nn_indices (k_);
   std::vector<float> nn_dists (k_);
 
   Eigen::Vector4f u = Eigen::Vector4f::Zero (), v = Eigen::Vector4f::Zero ();
diff --git a/features/include/pcl/features/impl/cppf.hpp b/features/include/pcl/features/impl/cppf.hpp
index c032114c..975bb98c 100755
--- a/features/include/pcl/features/impl/cppf.hpp
+++ b/features/include/pcl/features/impl/cppf.hpp
@@ -42,7 +42,6 @@
 #define PCL_FEATURES_IMPL_CPPF_H_
 
 #include <pcl/features/cppf.h>
-#include <pcl/features/pfh.h>
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointInT, typename PointNT, typename PointOutT>
@@ -110,7 +109,7 @@ pcl::CPPFEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut
         output.is_dense = false;
       }
 
-      output.points.push_back (p);
+      output.push_back (p);
     }
   }
   // overwrite the sizes done by Feature::initCompute ()
diff --git a/features/include/pcl/features/impl/crh.hpp b/features/include/pcl/features/impl/crh.hpp
index e1e9c827..18c19255 100644
--- a/features/include/pcl/features/impl/crh.hpp
+++ b/features/include/pcl/features/impl/crh.hpp
@@ -55,7 +55,7 @@ pcl::CRHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut
   {
     PCL_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
@@ -63,7 +63,7 @@ pcl::CRHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut
   {
     PCL_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the dataset containing the normals!\n", getClassName ().c_str ());
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
@@ -83,7 +83,7 @@ pcl::CRHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut
   Eigen::Affine3f transformPC (Eigen::AngleAxisf (static_cast<float> (rotation), axis));
 
   pcl::PointCloud<pcl::PointNormal> grid;
-  grid.points.resize (indices_->size ());
+  grid.resize (indices_->size ());
 
   for (std::size_t i = 0; i < indices_->size (); i++)
   {
@@ -119,7 +119,7 @@ pcl::CRHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut
       data.i /= freq_data[0].r;
   }
 
-  output.points.resize (1);
+  output.resize (1);
   output.width = output.height = 1;
 
   output[0].histogram[0] = freq_data[0].r; //dc
diff --git a/features/include/pcl/features/impl/cvfh.hpp b/features/include/pcl/features/impl/cvfh.hpp
index f4fd3870..d5df1e65 100644
--- a/features/include/pcl/features/impl/cvfh.hpp
+++ b/features/include/pcl/features/impl/cvfh.hpp
@@ -52,7 +52,7 @@ pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::compute (PointCloudOut &outpu
   if (!Feature<PointInT, PointOutT>::initCompute ())
   {
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
   // Resize the output dataset
@@ -60,7 +60,7 @@ pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::compute (PointCloudOut &outpu
   // we risk at pre-allocating too much memory which could lead to bad_alloc 
   // (see http://dev.pointclouds.org/issues/657)
   output.width = output.height = 1;
-  output.points.resize (1);
+  output.resize (1);
 
   // Perform the actual feature computation
   computeFeature (output);
@@ -100,7 +100,7 @@ pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::extractEuclideanClustersSmoot
   // Create a bool vector of processed point indices, and initialize it to false
   std::vector<bool> processed (cloud.size (), false);
 
-  std::vector<int> nn_indices;
+  pcl::Indices nn_indices;
   std::vector<float> nn_distances;
   // Process all points in the indices vector
   for (std::size_t i = 0; i < cloud.size (); ++i)
@@ -159,9 +159,9 @@ pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::extractEuclideanClustersSmoot
 template<typename PointInT, typename PointNT, typename PointOutT> void
 pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::filterNormalsWithHighCurvature (
     const pcl::PointCloud<PointNT> & cloud,
-    std::vector<int> &indices_to_use,
-    std::vector<int> &indices_out,
-    std::vector<int> &indices_in,
+    pcl::Indices &indices_to_use,
+    pcl::Indices &indices_out,
+    pcl::Indices &indices_in,
     float threshold)
 {
   indices_out.resize (cloud.size ());
@@ -170,7 +170,7 @@ pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::filterNormalsWithHighCurvatur
   std::size_t in, out;
   in = out = 0;
 
-  for (const int &index : indices_to_use)
+  for (const auto &index : indices_to_use)
   {
     if (cloud[index].curvature > threshold)
     {
@@ -197,22 +197,22 @@ pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut
   {
     PCL_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
   if (normals_->size () != surface_->size ())
   {
     PCL_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the dataset containing the normals!\n", getClassName ().c_str ());
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
   centroids_dominant_orientations_.clear ();
 
   // ---[ Step 0: remove normals with high curvature
-  std::vector<int> indices_out;
-  std::vector<int> indices_in;
+  pcl::Indices indices_out;
+  pcl::Indices indices_in;
   filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
 
   pcl::PointCloud<pcl::PointNormal>::Ptr normals_filtered_cloud (new pcl::PointCloud<pcl::PointNormal> ());
@@ -298,7 +298,7 @@ pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut
     }
 
     //compute modified VFH for all dominant clusters and add them to the list!
-    output.points.resize (dominant_normals_.size ());
+    output.resize (dominant_normals_.size ());
     output.width = dominant_normals_.size ();
 
     for (std::size_t i = 0; i < dominant_normals_.size (); ++i)
@@ -325,7 +325,7 @@ pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut
     pcl::PointCloud<pcl::VFHSignature308> vfh_signature;
     vfh.compute (vfh_signature);
 
-    output.points.resize (1);
+    output.resize (1);
     output.width = 1;
 
     output[0] = vfh_signature[0];
diff --git a/features/include/pcl/features/impl/esf.hpp b/features/include/pcl/features/impl/esf.hpp
index 16c4cc03..b83d0d6b 100644
--- a/features/include/pcl/features/impl/esf.hpp
+++ b/features/include/pcl/features/impl/esf.hpp
@@ -45,6 +45,7 @@
 #include <pcl/common/distances.h>
 #include <pcl/common/transforms.h>
 #include <vector>
+#include <ctime> // for time
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointInT, typename PointOutT> void
@@ -507,7 +508,7 @@ pcl::ESFEstimation<PointInT, PointOutT>::compute (PointCloudOut &output)
   if (!Feature<PointInT, PointOutT>::initCompute ())
   {
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
   // Copy the header
@@ -519,7 +520,7 @@ pcl::ESFEstimation<PointInT, PointOutT>::compute (PointCloudOut &output)
   // (see http://dev.pointclouds.org/issues/657)
   output.width = output.height = 1;
   output.is_dense = input_->is_dense;
-  output.points.resize (1);
+  output.resize (1);
 
   // Perform the actual feature computation
   computeFeature (output);
@@ -540,7 +541,7 @@ pcl::ESFEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
   this->cleanup9 (local_cloud_);
 
   // We only output _1_ signature
-  output.points.resize (1);
+  output.resize (1);
   output.width = 1;
   output.height = 1;
 
diff --git a/features/include/pcl/features/impl/feature.hpp b/features/include/pcl/features/impl/feature.hpp
index f8b4aaac..f185225f 100644
--- a/features/include/pcl/features/impl/feature.hpp
+++ b/features/include/pcl/features/impl/feature.hpp
@@ -41,7 +41,8 @@
 #ifndef PCL_FEATURES_IMPL_FEATURE_H_
 #define PCL_FEATURES_IMPL_FEATURE_H_
 
-#include <pcl/search/pcl_search.h>
+#include <pcl/search/kdtree.h> // for KdTree
+#include <pcl/search/organized.h> // for OrganizedNeighbor
 
 
 namespace pcl
@@ -68,7 +69,7 @@ solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
 //  for (int i = 0; i < 9; ++i)
 //    if (!std::isfinite (covariance_matrix.coeff (i)))
 //    {
-//      //PCL_WARN ("[pcl::solvePlaneParameteres] Covariance matrix has NaN/Inf values!\n");
+//      //PCL_WARN ("[pcl::solvePlaneParameters] Covariance matrix has NaN/Inf values!\n");
 //      nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN ();
 //      return;
 //    }
@@ -145,7 +146,7 @@ Feature<PointInT, PointOutT>::initCompute ()
       search_parameter_ = search_radius_;
       // Declare the search locator definition
       search_method_surface_ = [this] (const PointCloudIn &cloud, int index, double radius,
-                                       std::vector<int> &k_indices, std::vector<float> &k_distances)
+                                       pcl::Indices &k_indices, std::vector<float> &k_distances)
       {
         return tree_->radiusSearch (cloud, index, radius, k_indices, k_distances, 0);
       };
@@ -157,7 +158,7 @@ Feature<PointInT, PointOutT>::initCompute ()
     {
       search_parameter_ = k_;
       // Declare the search locator definition
-      search_method_surface_ = [this] (const PointCloudIn &cloud, int index, int k, std::vector<int> &k_indices,
+      search_method_surface_ = [this] (const PointCloudIn &cloud, int index, int k, pcl::Indices &k_indices,
                                        std::vector<float> &k_distances)
       {
         return tree_->nearestKSearch (cloud, index, k, k_indices, k_distances);
@@ -195,7 +196,7 @@ Feature<PointInT, PointOutT>::compute (PointCloudOut &output)
   if (!initCompute ())
   {
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
@@ -203,8 +204,8 @@ Feature<PointInT, PointOutT>::compute (PointCloudOut &output)
   output.header = input_->header;
 
   // Resize the output dataset
-  if (output.points.size () != indices_->size ())
-    output.points.resize (indices_->size ());
+  if (output.size () != indices_->size ())
+    output.resize (indices_->size ());
 
   // Check if the output will be computed for all points or only a subset
   // If the input width or height are not set, set output width as size
@@ -248,7 +249,7 @@ FeatureFromNormals<PointInT, PointNT, PointOutT>::initCompute ()
   if (normals_->points.size () != surface_->points.size ())
   {
     PCL_ERROR ("[pcl::%s::initCompute] ", getClassName ().c_str ());
-    PCL_ERROR("The number of points in the input dataset (%zu) differs from ",
+    PCL_ERROR("The number of points in the surface dataset (%zu) differs from ",
               static_cast<std::size_t>(surface_->points.size()));
     PCL_ERROR("the number of points in the dataset containing the normals (%zu)!\n",
               static_cast<std::size_t>(normals_->points.size()));
diff --git a/features/include/pcl/features/impl/flare.hpp b/features/include/pcl/features/impl/flare.hpp
index ca58c39c..ba841a2c 100644
--- a/features/include/pcl/features/impl/flare.hpp
+++ b/features/include/pcl/features/impl/flare.hpp
@@ -116,7 +116,7 @@ template<typename PointInT, typename PointNT, typename PointOutT, typename Signe
   //find Z axis
 
   //extract support points for the computation of Z axis
-  std::vector<int> neighbours_indices;
+  pcl::Indices neighbours_indices;
   std::vector<float> neighbours_distances;
 
   const std::size_t n_normal_neighbours =
diff --git a/features/include/pcl/features/impl/fpfh.hpp b/features/include/pcl/features/impl/fpfh.hpp
index 209b6d51..41b0772d 100644
--- a/features/include/pcl/features/impl/fpfh.hpp
+++ b/features/include/pcl/features/impl/fpfh.hpp
@@ -45,6 +45,7 @@
 #include <pcl/common/point_tests.h> // for pcl::isFinite
 #include <pcl/features/pfh_tools.h>
 
+#include <set> // for std::set
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointInT, typename PointNT, typename PointOutT> bool
@@ -62,7 +63,7 @@ pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computePairFeatures (
 template <typename PointInT, typename PointNT, typename PointOutT> void
 pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computePointSPFHSignature (
     const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
-    int p_idx, int row, const std::vector<int> &indices,
+    pcl::index_t p_idx, int row, const pcl::Indices &indices,
     Eigen::MatrixXf &hist_f1, Eigen::MatrixXf &hist_f2, Eigen::MatrixXf &hist_f3)
 {
   Eigen::Vector4f pfh_tuple;
@@ -108,7 +109,7 @@ pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computePointSPFHSignature (
 template <typename PointInT, typename PointNT, typename PointOutT> void
 pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::weightPointSPFHSignature (
     const Eigen::MatrixXf &hist_f1, const Eigen::MatrixXf &hist_f2, const Eigen::MatrixXf &hist_f3,
-    const std::vector<int> &indices, const std::vector<float> &dists, Eigen::VectorXf &fpfh_histogram)
+    const pcl::Indices &indices, const std::vector<float> &dists, Eigen::VectorXf &fpfh_histogram)
 {
   assert (indices.size () == dists.size ());
   // @TODO: use arrays
@@ -183,7 +184,7 @@ pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computeSPFHSignatures (std::v
 {
   // Allocate enough space to hold the NN search results
   // \note This resize is irrelevant for a radiusSearch ().
-  std::vector<int> nn_indices (k_);
+  pcl::Indices nn_indices (k_);
   std::vector<float> nn_dists (k_);
 
   std::set<int> spfh_indices;
@@ -238,7 +239,7 @@ pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut
 {
   // Allocate enough space to hold the NN search results
   // \note This resize is irrelevant for a radiusSearch ().
-  std::vector<int> nn_indices (k_);
+  pcl::Indices nn_indices (k_);
   std::vector<float> nn_dists (k_);
 
   std::vector<int> spfh_hist_lookup;
diff --git a/features/include/pcl/features/impl/fpfh_omp.hpp b/features/include/pcl/features/impl/fpfh_omp.hpp
index 2b72e64d..a81a4b00 100644
--- a/features/include/pcl/features/impl/fpfh_omp.hpp
+++ b/features/include/pcl/features/impl/fpfh_omp.hpp
@@ -73,7 +73,7 @@ pcl::FPFHEstimationOMP<PointInT, PointNT, PointOutT>::computeFeature (PointCloud
   if (surface_ != input_ ||
       indices_->size () != surface_->size ())
   { 
-    std::vector<int> nn_indices (k_); // \note These resizes are irrelevant for a radiusSearch ().
+    pcl::Indices nn_indices (k_); // \note These resizes are irrelevant for a radiusSearch ().
     std::vector<float> nn_dists (k_); 
 
     std::set<int> spfh_indices_set;
@@ -103,7 +103,7 @@ pcl::FPFHEstimationOMP<PointInT, PointNT, PointOutT>::computeFeature (PointCloud
   hist_f2_.setZero (data_size, nr_bins_f2_);
   hist_f3_.setZero (data_size, nr_bins_f3_);
 
-  std::vector<int> nn_indices (k_); // \note These resizes are irrelevant for a radiusSearch ().
+  pcl::Indices nn_indices (k_); // \note These resizes are irrelevant for a radiusSearch ().
   std::vector<float> nn_dists (k_); 
 
   // Compute SPFH signatures for every point that needs them
@@ -119,7 +119,7 @@ pcl::FPFHEstimationOMP<PointInT, PointNT, PointOutT>::computeFeature (PointCloud
     int p_idx = spfh_indices_vec[i];
 
     // Find the neighborhood around p_idx
-    if (!isFinite ((*input_)[p_idx]) ||
+    if (!isFinite ((*surface_)[p_idx]) ||
         this->searchForNeighbors (*surface_, p_idx, search_parameter_, nn_indices, nn_dists) == 0)
       continue;
 
@@ -158,7 +158,7 @@ pcl::FPFHEstimationOMP<PointInT, PointNT, PointOutT>::computeFeature (PointCloud
 
     // ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices 
     // instead of indices into surface_->points
-    for (int &nn_index : nn_indices)
+    for (auto &nn_index : nn_indices)
       nn_index = spfh_hist_lookup[nn_index];
 
     // Compute the FPFH signature (i.e. compute a weighted combination of local SPFH signatures) ...
diff --git a/features/include/pcl/features/impl/gasd.hpp b/features/include/pcl/features/impl/gasd.hpp
index a2e20ba4..1557652c 100644
--- a/features/include/pcl/features/impl/gasd.hpp
+++ b/features/include/pcl/features/impl/gasd.hpp
@@ -40,6 +40,7 @@
 #define PCL_FEATURES_IMPL_GASD_H_
 
 #include <pcl/features/gasd.h>
+#include <pcl/common/common.h> // for getMinMax3D
 #include <pcl/common/transforms.h>
 
 #include <vector>
@@ -51,7 +52,7 @@ pcl::GASDEstimation<PointInT, PointOutT>::compute (PointCloudOut &output)
   if (!Feature<PointInT, PointOutT>::initCompute ())
   {
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
diff --git a/features/include/pcl/features/impl/gfpfh.hpp b/features/include/pcl/features/impl/gfpfh.hpp
index ee352071..06be4cf8 100644
--- a/features/include/pcl/features/impl/gfpfh.hpp
+++ b/features/include/pcl/features/impl/gfpfh.hpp
@@ -55,7 +55,7 @@ pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::compute (PointCloudOut &outp
   if (!Feature<PointInT, PointOutT>::initCompute ())
   {
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
   // Copy the header
@@ -67,7 +67,7 @@ pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::compute (PointCloudOut &outp
   // (see http://dev.pointclouds.org/issues/657)
   output.width = output.height = 1;
   output.is_dense = input_->is_dense;
-  output.points.resize (1);
+  output.resize (1);
 
   // Perform the actual feature computation
   computeFeature (output);
@@ -103,7 +103,7 @@ pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOu
       std::vector<int> histogram;
       for (std::size_t k = 0; k < intersected_cells.size (); ++k)
       {
-        std::vector<int> indices;
+        pcl::Indices indices;
         octree.voxelSearch (intersected_cells[k], indices);
         int label = emptyLabel ();
         if (!indices.empty ())
@@ -129,7 +129,7 @@ pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOu
   output.clear ();
   output.width = 1;
   output.height = 1;
-  output.points.resize (1);
+  output.resize (1);
   std::copy (gfpfh_histogram.cbegin (), gfpfh_histogram.cend (), output[0].histogram);
 }
 
@@ -251,10 +251,10 @@ pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeHIKDistance (const st
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointInT, typename PointNT, typename PointOutT> std::uint32_t
-pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::getDominantLabel (const std::vector<int>& indices)
+pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::getDominantLabel (const pcl::Indices& indices)
 {
   std::vector<std::uint32_t> counts (getNumberOfClasses () + 1, 0);
-  for (const int &nn_index : indices)
+  for (const auto &nn_index : indices)
   {
     std::uint32_t label = (*labels_)[nn_index].label;
     counts[label] += 1;
diff --git a/features/include/pcl/features/impl/grsd.hpp b/features/include/pcl/features/impl/grsd.hpp
index 128d234d..b981a4a6 100644
--- a/features/include/pcl/features/impl/grsd.hpp
+++ b/features/include/pcl/features/impl/grsd.hpp
@@ -40,6 +40,7 @@
 #define PCL_FEATURES_IMPL_GRSD_H_
 
 #include <pcl/features/grsd.h>
+#include <pcl/features/rsd.h> // for RSDEstimation
 ///////// STATIC /////////
 template <typename PointInT, typename PointNT, typename PointOutT> int
 pcl::GRSDEstimation<PointInT, PointNT, PointOutT>::getSimpleType (float min_radius, float max_radius,
@@ -68,7 +69,7 @@ pcl::GRSDEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut
   {
     PCL_ERROR ("[pcl::%s::computeFeature] A voxel cell width needs to be set!\n", getClassName ().c_str ());
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
@@ -113,7 +114,7 @@ pcl::GRSDEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut
   }
 
   // Save feature values
-  output.points.resize (1);
+  output.resize (1);
   output.height = output.width = 1;
   int nrf = 0;
   for (int i = 0; i < NR_CLASS + 1; i++)
diff --git a/features/include/pcl/features/impl/integral_image2D.hpp b/features/include/pcl/features/impl/integral_image2D.hpp
index 7eef7af9..a0f42480 100644
--- a/features/include/pcl/features/impl/integral_image2D.hpp
+++ b/features/include/pcl/features/impl/integral_image2D.hpp
@@ -41,8 +41,6 @@
 #ifndef PCL_INTEGRAL_IMAGE2D_IMPL_H_
 #define PCL_INTEGRAL_IMAGE2D_IMPL_H_
 
-#include <cstddef>
-
 
 namespace pcl
 {
@@ -160,7 +158,7 @@ IntegralImage2D<DataType, Dimension>::computeIntegralImages (
 {
   ElementType* previous_row = &first_order_integral_image_[0];
   ElementType* current_row  = previous_row + (width_ + 1);
-  memset (previous_row, 0, sizeof (ElementType) * (width_ + 1));
+  *previous_row = ElementType::Zero(width_ + 1);
 
   unsigned* count_previous_row = &finite_values_integral_image_[0];
   unsigned* count_current_row  = count_previous_row + (width_ + 1);
@@ -191,7 +189,7 @@ IntegralImage2D<DataType, Dimension>::computeIntegralImages (
   {
     SecondOrderType* so_previous_row = &second_order_integral_image_[0];
     SecondOrderType* so_current_row  = so_previous_row + (width_ + 1);
-    memset (so_previous_row, 0, sizeof (SecondOrderType) * (width_ + 1));
+    *so_previous_row = SecondOrderType::Zero(width_ + 1);
 
     SecondOrderType so_element;
     for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
diff --git a/features/include/pcl/features/impl/intensity_gradient.hpp b/features/include/pcl/features/impl/intensity_gradient.hpp
index 53e69167..a16b6719 100644
--- a/features/include/pcl/features/impl/intensity_gradient.hpp
+++ b/features/include/pcl/features/impl/intensity_gradient.hpp
@@ -48,7 +48,7 @@
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointInT, typename PointNT, typename PointOutT, typename IntensitySelectorT> void
 pcl::IntensityGradientEstimation <PointInT, PointNT, PointOutT, IntensitySelectorT>::computePointIntensityGradient (
-  const pcl::PointCloud <PointInT> &cloud, const std::vector <int> &indices,
+  const pcl::PointCloud <PointInT> &cloud, const pcl::Indices &indices,
   const Eigen::Vector3f &point, float mean_intensity, const Eigen::Vector3f &normal, Eigen::Vector3f &gradient)
 {
   if (indices.size () < 3)
@@ -60,7 +60,7 @@ pcl::IntensityGradientEstimation <PointInT, PointNT, PointOutT, IntensitySelecto
   Eigen::Matrix3f A = Eigen::Matrix3f::Zero ();
   Eigen::Vector3f b = Eigen::Vector3f::Zero ();
 
-  for (const int &nn_index : indices)
+  for (const auto &nn_index : indices)
   {
     PointInT p = cloud[nn_index];
     if (!std::isfinite (p.x) ||
@@ -144,7 +144,7 @@ pcl::IntensityGradientEstimation<PointInT, PointNT, PointOutT, IntensitySelector
 {
   // Allocate enough space to hold the results
   // \note This resize is irrelevant for a radiusSearch ().
-  std::vector<int> nn_indices (k_);
+  pcl::Indices nn_indices (k_);
   std::vector<float> nn_dists (k_);
   output.is_dense = true;
 
@@ -172,7 +172,7 @@ pcl::IntensityGradientEstimation<PointInT, PointNT, PointOutT, IntensitySelector
       float mean_intensity = 0;
       // Initialize to 0
       centroid.setZero ();
-      for (const int &nn_index : nn_indices)
+      for (const auto &nn_index : nn_indices)
       {
         centroid += (*surface_)[nn_index].getVector3fMap ();
         mean_intensity += intensity_ ((*surface_)[nn_index]);
@@ -212,7 +212,7 @@ pcl::IntensityGradientEstimation<PointInT, PointNT, PointOutT, IntensitySelector
       // Initialize to 0
       centroid.setZero ();
       unsigned cp = 0;
-      for (const int &nn_index : nn_indices)
+      for (const auto &nn_index : nn_indices)
       {
         // Check if the point is invalid
         if (!isFinite ((*surface_) [nn_index]))
diff --git a/features/include/pcl/features/impl/intensity_spin.hpp b/features/include/pcl/features/impl/intensity_spin.hpp
index 356ecf34..cffb5056 100644
--- a/features/include/pcl/features/impl/intensity_spin.hpp
+++ b/features/include/pcl/features/impl/intensity_spin.hpp
@@ -48,7 +48,7 @@ template <typename PointInT, typename PointOutT> void
 pcl::IntensitySpinEstimation<PointInT, PointOutT>::computeIntensitySpinImage (
       const PointCloudIn &cloud, float radius, float sigma, 
       int k,
-      const std::vector<int> &indices, 
+      const pcl::Indices &indices, 
       const std::vector<float> &squared_distances, 
       Eigen::MatrixXf &intensity_spin_image)
 {
@@ -115,7 +115,7 @@ pcl::IntensitySpinEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut
     PCL_ERROR ("[pcl::%s::computeFeature] The search radius must be set before computing the feature!\n",
                getClassName ().c_str ());
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
@@ -125,7 +125,7 @@ pcl::IntensitySpinEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut
     PCL_ERROR ("[pcl::%s::computeFeature] The number of intensity bins must be greater than zero!\n",
                getClassName ().c_str ());
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
   if (nr_distance_bins_ <= 0)
@@ -133,13 +133,13 @@ pcl::IntensitySpinEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut
     PCL_ERROR ("[pcl::%s::computeFeature] The number of distance bins must be greater than zero!\n",
                getClassName ().c_str ());
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
   Eigen::MatrixXf intensity_spin_image (nr_intensity_bins_, nr_distance_bins_);
   // Allocate enough space to hold the radiusSearch results
-  std::vector<int> nn_indices (surface_->size ());
+  pcl::Indices nn_indices (surface_->size ());
   std::vector<float> nn_dist_sqr (surface_->size ());
  
   output.is_dense = true;
diff --git a/features/include/pcl/features/impl/moment_invariants.hpp b/features/include/pcl/features/impl/moment_invariants.hpp
index d4899aaa..3f4167ea 100644
--- a/features/include/pcl/features/impl/moment_invariants.hpp
+++ b/features/include/pcl/features/impl/moment_invariants.hpp
@@ -47,7 +47,7 @@
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointInT, typename PointOutT> void
 pcl::MomentInvariantsEstimation<PointInT, PointOutT>::computePointMomentInvariants (
-      const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices,
+      const pcl::PointCloud<PointInT> &cloud, const pcl::Indices &indices,
       float &j1, float &j2, float &j3)
 {
   // Estimate the XYZ centroid
@@ -57,7 +57,7 @@ pcl::MomentInvariantsEstimation<PointInT, PointOutT>::computePointMomentInvarian
   float mu200 = 0, mu020 = 0, mu002 = 0, mu110 = 0, mu101 = 0, mu011  = 0;
 
   // Iterate over the nearest neighbors set
-  for (const int &index : indices)
+  for (const auto &index : indices)
   {
     // Demean the points
     temp_pt_[0] = cloud[index].x - xyz_centroid_[0];
@@ -117,7 +117,7 @@ pcl::MomentInvariantsEstimation<PointInT, PointOutT>::computeFeature (PointCloud
 {
   // Allocate enough space to hold the results
   // \note This resize is irrelevant for a radiusSearch ().
-  std::vector<int> nn_indices (k_);
+  pcl::Indices nn_indices (k_);
   std::vector<float> nn_dists (k_);
 
   output.is_dense = true;
diff --git a/features/include/pcl/features/impl/moment_of_inertia_estimation.hpp b/features/include/pcl/features/impl/moment_of_inertia_estimation.hpp
index 4186d883..42d75ddf 100644
--- a/features/include/pcl/features/impl/moment_of_inertia_estimation.hpp
+++ b/features/include/pcl/features/impl/moment_of_inertia_estimation.hpp
@@ -40,6 +40,8 @@
 #ifndef PCL_MOMENT_OF_INERTIA_ESTIMATION_HPP_
 #define PCL_MOMENT_OF_INERTIA_ESTIMATION_HPP_
 
+#include <Eigen/Eigenvalues> // for EigenSolver
+
 #include <pcl/features/moment_of_inertia_estimation.h>
 #include <pcl/features/feature.h>
 
@@ -565,8 +567,7 @@ pcl::MomentOfInertiaEstimation<PointT>::getMassCenter (Eigen::Vector3f& mass_cen
 template <typename PointT> void
 pcl::MomentOfInertiaEstimation<PointT>::setInputCloud (const PointCloudConstPtr& cloud)
 {
-  input_ = cloud;
-
+  pcl::PCLBase<PointT>::setInputCloud (cloud);
   is_valid_ = false;
 }
 
@@ -574,10 +575,7 @@ pcl::MomentOfInertiaEstimation<PointT>::setInputCloud (const PointCloudConstPtr&
 template <typename PointT> void
 pcl::MomentOfInertiaEstimation<PointT>::setIndices (const IndicesPtr& indices)
 {
-  indices_ = indices;
-  fake_indices_ = false;
-  use_indices_  = true;
-
+  pcl::PCLBase<PointT>::setIndices (indices);
   is_valid_ = false;
 }
 
@@ -585,10 +583,7 @@ pcl::MomentOfInertiaEstimation<PointT>::setIndices (const IndicesPtr& indices)
 template <typename PointT> void
 pcl::MomentOfInertiaEstimation<PointT>::setIndices (const IndicesConstPtr& indices)
 {
-  indices_.reset (new std::vector<int> (*indices));
-  fake_indices_ = false;
-  use_indices_  = true;
-
+  pcl::PCLBase<PointT>::setIndices (indices);
   is_valid_ = false;
 }
 
@@ -596,10 +591,7 @@ pcl::MomentOfInertiaEstimation<PointT>::setIndices (const IndicesConstPtr& indic
 template <typename PointT> void
 pcl::MomentOfInertiaEstimation<PointT>::setIndices (const PointIndicesConstPtr& indices)
 {
-  indices_.reset (new std::vector<int> (indices->indices));
-  fake_indices_ = false;
-  use_indices_  = true;
-
+  pcl::PCLBase<PointT>::setIndices (indices);
   is_valid_ = false;
 }
 
@@ -607,40 +599,7 @@ pcl::MomentOfInertiaEstimation<PointT>::setIndices (const PointIndicesConstPtr&
 template <typename PointT> void
 pcl::MomentOfInertiaEstimation<PointT>::setIndices (std::size_t row_start, std::size_t col_start, std::size_t nb_rows, std::size_t nb_cols)
 {
-  if ((nb_rows > input_->height) || (row_start > input_->height))
-  {
-    PCL_ERROR ("[PCLBase::setIndices] cloud is only %d height", input_->height);
-    return;
-  }
-
-  if ((nb_cols > input_->width) || (col_start > input_->width))
-  {
-    PCL_ERROR ("[PCLBase::setIndices] cloud is only %d width", input_->width);
-    return;
-  }
-
-  const std::size_t row_end = row_start + nb_rows;
-  if (row_end > input_->height)
-  {
-    PCL_ERROR ("[PCLBase::setIndices] %d is out of rows range %d", row_end, input_->height);
-    return;
-  }
-
-  const std::size_t col_end = col_start + nb_cols;
-  if (col_end > input_->width)
-  {
-    PCL_ERROR ("[PCLBase::setIndices] %d is out of columns range %d", col_end, input_->width);
-    return;
-  }
-
-  indices_.reset (new std::vector<int>);
-  indices_->reserve (nb_cols * nb_rows);
-  for(std::size_t i = row_start; i < row_end; i++)
-    for(std::size_t j = col_start; j < col_end; j++)
-      indices_->push_back (static_cast<int> ((i * input_->width) + j));
-  fake_indices_ = false;
-  use_indices_  = true;
-
+  pcl::PCLBase<PointT>::setIndices (row_start, col_start, nb_rows, nb_cols);
   is_valid_ = false;
 }
 
diff --git a/features/include/pcl/features/impl/multiscale_feature_persistence.hpp b/features/include/pcl/features/impl/multiscale_feature_persistence.hpp
index 378d6451..c0afe995 100644
--- a/features/include/pcl/features/impl/multiscale_feature_persistence.hpp
+++ b/features/include/pcl/features/impl/multiscale_feature_persistence.hpp
@@ -202,7 +202,7 @@ pcl::MultiscaleFeaturePersistence<PointSource, PointFeature>::extractUniqueFeatu
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointSource, typename PointFeature> void
 pcl::MultiscaleFeaturePersistence<PointSource, PointFeature>::determinePersistentFeatures (FeatureCloud &output_features,
-                                                                                           shared_ptr<std::vector<int> > &output_indices)
+                                                                                           pcl::IndicesPtr &output_indices)
 {
   if (!initCompute ())
     return;
@@ -229,7 +229,7 @@ pcl::MultiscaleFeaturePersistence<PointSource, PointFeature>::determinePersisten
     {
       if (unique_features_table_[scale_i][*feature_it] == true)
       {
-        output_features.points.push_back ((*features_at_scale_[scale_i])[*feature_it]);
+        output_features.push_back ((*features_at_scale_[scale_i])[*feature_it]);
         output_indices->push_back (feature_estimator_->getIndices ()->at (*feature_it));
       }
     }
@@ -243,7 +243,7 @@ pcl::MultiscaleFeaturePersistence<PointSource, PointFeature>::determinePersisten
 
     if (present_in_all)
     {
-      output_features.points.emplace_back ((*features_at_scale_.front ())[feature]);
+      output_features.emplace_back ((*features_at_scale_.front ())[feature]);
       output_indices->emplace_back (feature_estimator_->getIndices ()->at (feature));
     }
   }
diff --git a/features/include/pcl/features/impl/narf.hpp b/features/include/pcl/features/impl/narf.hpp
index 759082d5..a7b01b0b 100644
--- a/features/include/pcl/features/impl/narf.hpp
+++ b/features/include/pcl/features/impl/narf.hpp
@@ -37,7 +37,6 @@
  */
 
 #include <iostream>
-#include <map>
 #include <pcl/common/norms.h> // for L1_Norm
 #include <pcl/common/eigen.h>
 
diff --git a/features/include/pcl/features/impl/normal_3d.hpp b/features/include/pcl/features/impl/normal_3d.hpp
index 78523e9c..78c46047 100644
--- a/features/include/pcl/features/impl/normal_3d.hpp
+++ b/features/include/pcl/features/impl/normal_3d.hpp
@@ -49,7 +49,7 @@ pcl::NormalEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &outpu
 {
   // Allocate enough space to hold the results
   // \note This resize is irrelevant for a radiusSearch ().
-  std::vector<int> nn_indices (k_);
+  pcl::Indices nn_indices (k_);
   std::vector<float> nn_dists (k_);
 
   output.is_dense = true;
diff --git a/features/include/pcl/features/impl/normal_3d_omp.hpp b/features/include/pcl/features/impl/normal_3d_omp.hpp
index d6f6cb0f..b1a31759 100644
--- a/features/include/pcl/features/impl/normal_3d_omp.hpp
+++ b/features/include/pcl/features/impl/normal_3d_omp.hpp
@@ -63,7 +63,7 @@ pcl::NormalEstimationOMP<PointInT, PointOutT>::computeFeature (PointCloudOut &ou
 {
   // Allocate enough space to hold the results
   // \note This resize is irrelevant for a radiusSearch ().
-  std::vector<int> nn_indices (k_);
+  pcl::Indices nn_indices (k_);
   std::vector<float> nn_dists (k_);
 
   output.is_dense = true;
diff --git a/features/include/pcl/features/impl/normal_based_signature.hpp b/features/include/pcl/features/impl/normal_based_signature.hpp
index 9a9afc7a..4a892edc 100644
--- a/features/include/pcl/features/impl/normal_based_signature.hpp
+++ b/features/include/pcl/features/impl/normal_based_signature.hpp
@@ -54,11 +54,11 @@ pcl::NormalBasedSignatureEstimation<PointT, PointNT, PointFeature>::computeFeatu
     return;
   }
 
-  std::vector<int> k_indices;
+  pcl::Indices k_indices;
   std::vector<float> k_sqr_distances;
 
   tree_->setInputCloud (input_);
-  output.points.resize (indices_->size ());
+  output.resize (indices_->size ());
 
   for (std::size_t index_i = 0; index_i < indices_->size (); ++index_i)
   {
diff --git a/features/include/pcl/features/impl/organized_edge_detection.hpp b/features/include/pcl/features/impl/organized_edge_detection.hpp
index c70b8f31..53763443 100644
--- a/features/include/pcl/features/impl/organized_edge_detection.hpp
+++ b/features/include/pcl/features/impl/organized_edge_detection.hpp
@@ -53,7 +53,7 @@ pcl::OrganizedEdgeBase<PointT, PointLT>::compute (pcl::PointCloud<PointLT>& labe
 {
   pcl::Label invalid_pt;
   invalid_pt.label = unsigned (0);
-  labels.points.resize (input_->size (), invalid_pt);
+  labels.resize (input_->size (), invalid_pt);
   labels.width = input_->width;
   labels.height = input_->height;
   
@@ -115,7 +115,7 @@ pcl::OrganizedEdgeBase<PointT, PointLT>::extractEdges (pcl::PointCloud<PointLT>&
         for (int d_idx = 0; d_idx < num_of_ngbr; d_idx++)
         {
           int nghr_idx = curr_idx + directions[d_idx].d_index;
-          assert (nghr_idx >= 0 && nghr_idx < input_->size ());
+          assert (nghr_idx >= 0 && static_cast<std::size_t>(nghr_idx) < input_->size ());
           if (!std::isfinite ((*input_)[nghr_idx].z))
           {
             found_invalid_neighbor = true;
@@ -158,7 +158,7 @@ pcl::OrganizedEdgeBase<PointT, PointLT>::extractEdges (pcl::PointCloud<PointLT>&
           for (const auto &direction : directions)
           {
             int nghr_idx = curr_idx + direction.d_index;
-            assert (nghr_idx >= 0 && nghr_idx < input_->size ());
+            assert (nghr_idx >= 0 && static_cast<std::size_t>(nghr_idx) < input_->size ());
             if (!std::isfinite ((*input_)[nghr_idx].z))
             {
               dx += direction.d_x;
@@ -227,7 +227,7 @@ pcl::OrganizedEdgeFromRGB<PointT, PointLT>::compute (pcl::PointCloud<PointLT>& l
 {
   pcl::Label invalid_pt;
   invalid_pt.label = unsigned (0);
-  labels.points.resize (input_->size (), invalid_pt);
+  labels.resize (input_->size (), invalid_pt);
   labels.width = input_->width;
   labels.height = input_->height;
 
@@ -275,7 +275,7 @@ pcl::OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::compute (pcl::PointClou
 {
   pcl::Label invalid_pt;
   invalid_pt.label = unsigned (0);
-  labels.points.resize (input_->size (), invalid_pt);
+  labels.resize (input_->size (), invalid_pt);
   labels.width = input_->width;
   labels.height = input_->height;
   
@@ -333,7 +333,7 @@ pcl::OrganizedEdgeFromRGBNormals<PointT, PointNT, PointLT>::compute (pcl::PointC
 {
   pcl::Label invalid_pt;
   invalid_pt.label = unsigned (0);
-  labels.points.resize (input_->size (), invalid_pt);
+  labels.resize (input_->size (), invalid_pt);
   labels.width = input_->width;
   labels.height = input_->height;
   
diff --git a/features/include/pcl/features/impl/our_cvfh.hpp b/features/include/pcl/features/impl/our_cvfh.hpp
index c21d986f..34caf74c 100644
--- a/features/include/pcl/features/impl/our_cvfh.hpp
+++ b/features/include/pcl/features/impl/our_cvfh.hpp
@@ -44,6 +44,8 @@
 #include <pcl/features/our_cvfh.h>
 #include <pcl/features/vfh.h>
 #include <pcl/features/normal_3d.h>
+#include <pcl/common/io.h> // for copyPointCloud
+#include <pcl/common/common.h> // for getMaxDistance
 #include <pcl/common/transforms.h>
 
 //////////////////////////////////////////////////////////////////////////////////////////////
@@ -53,7 +55,7 @@ pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::compute (PointCloudOut &ou
   if (!Feature<PointInT, PointOutT>::initCompute ())
   {
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
   // Resize the output dataset
@@ -61,7 +63,7 @@ pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::compute (PointCloudOut &ou
   // we risk at pre-allocating too much memory which could lead to bad_alloc
   // (see http://dev.pointclouds.org/issues/657)
   output.width = output.height = 1;
-  output.points.resize (1);
+  output.resize (1);
 
   // Perform the actual feature computation
   computeFeature (output);
@@ -99,7 +101,7 @@ pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::extractEuclideanClustersSm
   // Create a bool vector of processed point indices, and initialize it to false
   std::vector<bool> processed (cloud.size (), false);
 
-  std::vector<int> nn_indices;
+  pcl::Indices nn_indices;
   std::vector<float> nn_distances;
   // Process all points in the indices vector
   for (std::size_t i = 0; i < cloud.size (); ++i)
@@ -164,8 +166,8 @@ pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::extractEuclideanClustersSm
 //////////////////////////////////////////////////////////////////////////////////////////////
 template<typename PointInT, typename PointNT, typename PointOutT> void
 pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::filterNormalsWithHighCurvature (const pcl::PointCloud<PointNT> & cloud,
-                                                                                        std::vector<int> &indices_to_use,
-                                                                                        std::vector<int> &indices_out, std::vector<int> &indices_in,
+                                                                                        pcl::Indices &indices_to_use,
+                                                                                        pcl::Indices &indices_out, pcl::Indices &indices_in,
                                                                                         float threshold)
 {
   indices_out.resize (cloud.size ());
@@ -174,7 +176,7 @@ pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::filterNormalsWithHighCurva
   std::size_t in, out;
   in = out = 0;
 
-  for (const int &index : indices_to_use)
+  for (const auto &index : indices_to_use)
   {
     if (cloud[index].curvature > threshold)
     {
@@ -242,7 +244,7 @@ pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::sgurf (Eigen::Vector3f & c
   scatter.setZero ();
   float sum_w = 0.f;
 
-  for (const int &index : indices.indices)
+  for (const auto &index : indices.indices)
   {
     Eigen::Vector3f pvector = (*grid)[index].getVector3fMap ();
     float d_k = (pvector).norm ();
@@ -515,7 +517,7 @@ pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::computeRFAndShapeDistribut
 
       //copy to the cvfh signature
       PointCloudOut vfh_signature;
-      vfh_signature.points.resize (1);
+      vfh_signature.resize (1);
       vfh_signature.width = vfh_signature.height = 1;
       for (int d = 0; d < 308; ++d)
         vfh_signature[0].histogram[d] = output[i].histogram[d];
@@ -535,7 +537,7 @@ pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::computeRFAndShapeDistribut
     }
   }
 
-  if (!ourcvfh_output.points.empty ())
+  if (!ourcvfh_output.empty ())
   {
     ourcvfh_output.height = 1;
   }
@@ -554,14 +556,14 @@ pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloud
   {
     PCL_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
   if (normals_->size () != surface_->size ())
   {
     PCL_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the dataset containing the normals!\n", getClassName ().c_str ());
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
@@ -571,8 +573,8 @@ pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloud
   dominant_normals_.clear ();
 
   // ---[ Step 0: remove normals with high curvature
-  std::vector<int> indices_out;
-  std::vector<int> indices_in;
+  pcl::Indices indices_out;
+  pcl::Indices indices_in;
   filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
 
   pcl::PointCloud<pcl::PointNormal>::Ptr normals_filtered_cloud (new pcl::PointCloud<pcl::PointNormal> ());
@@ -580,7 +582,7 @@ pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloud
   normals_filtered_cloud->height = 1;
   normals_filtered_cloud->points.resize (normals_filtered_cloud->width);
 
-  std::vector<int> indices_from_nfc_to_indices;
+  pcl::Indices indices_from_nfc_to_indices;
   indices_from_nfc_to_indices.resize (indices_in.size ());
 
   for (std::size_t i = 0; i < indices_in.size (); ++i)
@@ -700,7 +702,7 @@ pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloud
     }
 
     //compute modified VFH for all dominant clusters and add them to the list!
-    output.points.resize (dominant_normals_.size ());
+    output.resize (dominant_normals_.size ());
     output.width = dominant_normals_.size ();
 
     for (std::size_t i = 0; i < dominant_normals_.size (); ++i)
@@ -734,7 +736,7 @@ pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloud
     pcl::PointCloud<pcl::VFHSignature308> vfh_signature;
     vfh.compute (vfh_signature);
 
-    output.points.resize (1);
+    output.resize (1);
     output.width = 1;
 
     output[0] = vfh_signature[0];
diff --git a/features/include/pcl/features/impl/pfh.hpp b/features/include/pcl/features/impl/pfh.hpp
index 3ca4ebab..03d57dbd 100644
--- a/features/include/pcl/features/impl/pfh.hpp
+++ b/features/include/pcl/features/impl/pfh.hpp
@@ -39,6 +39,7 @@
 #pragma once
 
 #include <pcl/features/pfh.h>
+#include <pcl/features/pfh_tools.h> // for computePairFeatures
 
 #include <pcl/common/point_tests.h> // for pcl::isFinite
 
@@ -59,7 +60,7 @@ pcl::PFHEstimation<PointInT, PointNT, PointOutT>::computePairFeatures (
 template <typename PointInT, typename PointNT, typename PointOutT> void
 pcl::PFHEstimation<PointInT, PointNT, PointOutT>::computePointPFHSignature (
       const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
-      const std::vector<int> &indices, int nr_split, Eigen::VectorXf &pfh_histogram)
+      const pcl::Indices &indices, int nr_split, Eigen::VectorXf &pfh_histogram)
 {
   int h_index, h_p;
 
@@ -174,7 +175,7 @@ pcl::PFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut
 
   // Allocate enough space to hold the results
   // \note This resize is irrelevant for a radiusSearch ().
-  std::vector<int> nn_indices (k_);
+  pcl::Indices nn_indices (k_);
   std::vector<float> nn_dists (k_);
 
   output.is_dense = true;
diff --git a/features/include/pcl/features/impl/pfhrgb.hpp b/features/include/pcl/features/impl/pfhrgb.hpp
index 2df24277..f6363f69 100644
--- a/features/include/pcl/features/impl/pfhrgb.hpp
+++ b/features/include/pcl/features/impl/pfhrgb.hpp
@@ -63,7 +63,7 @@ pcl::PFHRGBEstimation<PointInT, PointNT, PointOutT>::computeRGBPairFeatures (
 template <typename PointInT, typename PointNT, typename PointOutT> void
 pcl::PFHRGBEstimation<PointInT, PointNT, PointOutT>::computePointPFHRGBSignature (
     const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
-    const std::vector<int> &indices, int nr_split, Eigen::VectorXf &pfhrgb_histogram)
+    const pcl::Indices &indices, int nr_split, Eigen::VectorXf &pfhrgb_histogram)
 {
   int h_index, h_p;
 
@@ -140,7 +140,7 @@ pcl::PFHRGBEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudO
 
   // Allocate enough space to hold the results
   // \note This resize is irrelevant for a radiusSearch ().
-  std::vector<int> nn_indices (k_);
+  pcl::Indices nn_indices (k_);
   std::vector<float> nn_dists (k_);
 
   // Iterating over the entire index vector
diff --git a/features/include/pcl/features/impl/ppf.hpp b/features/include/pcl/features/impl/ppf.hpp
index 4e78e152..8cdb914a 100644
--- a/features/include/pcl/features/impl/ppf.hpp
+++ b/features/include/pcl/features/impl/ppf.hpp
@@ -42,6 +42,7 @@
 
 #include <pcl/features/ppf.h>
 #include <pcl/features/pfh.h>
+#include <pcl/features/pfh_tools.h> // for computePairFeatures
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointInT, typename PointNT, typename PointOutT>
@@ -60,7 +61,7 @@ template <typename PointInT, typename PointNT, typename PointOutT> void
 pcl::PPFEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
 {
   // Initialize output container - overwrite the sizes done by Feature::initCompute ()
-  output.points.resize (indices_->size () * input_->size ());
+  output.resize (indices_->size () * input_->size ());
   output.height = 1;
   output.width = output.size ();
   output.is_dense = true;
diff --git a/features/include/pcl/features/impl/ppfrgb.hpp b/features/include/pcl/features/impl/ppfrgb.hpp
index 183372fd..87f74548 100644
--- a/features/include/pcl/features/impl/ppfrgb.hpp
+++ b/features/include/pcl/features/impl/ppfrgb.hpp
@@ -58,7 +58,7 @@ template <typename PointInT, typename PointNT, typename PointOutT> void
 pcl::PPFRGBEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
 {
   // Initialize output container - overwrite the sizes done by Feature::initCompute ()
-  output.points.resize (indices_->size () * input_->size ());
+  output.resize (indices_->size () * input_->size ());
   output.height = 1;
   output.width = output.size ();
 
@@ -125,8 +125,8 @@ pcl::PPFRGBRegionEstimation<PointInT, PointNT, PointOutT>::computeFeature (Point
   output.resize (indices_->size ());
   for (std::size_t index_i = 0; index_i < indices_->size (); ++index_i)
   {
-    int i = (*indices_)[index_i];
-    std::vector<int> nn_indices;
+    auto i = (*indices_)[index_i];
+    pcl::Indices nn_indices;
     std::vector<float> nn_distances;
     tree_->radiusSearch (i, static_cast<float> (search_radius_), nn_indices, nn_distances);
 
@@ -135,7 +135,7 @@ pcl::PPFRGBRegionEstimation<PointInT, PointNT, PointOutT>::computeFeature (Point
     average_feature_nn.f1 = average_feature_nn.f2 = average_feature_nn.f3 = average_feature_nn.f4 =
         average_feature_nn.r_ratio = average_feature_nn.g_ratio = average_feature_nn.b_ratio = 0.0f;
 
-    for (const int &j : nn_indices)
+    for (const auto &j : nn_indices)
     {
       if (i != j)
       {
diff --git a/features/include/pcl/features/impl/principal_curvatures.hpp b/features/include/pcl/features/impl/principal_curvatures.hpp
index 2bb2ad2a..ba32ee2a 100644
--- a/features/include/pcl/features/impl/principal_curvatures.hpp
+++ b/features/include/pcl/features/impl/principal_curvatures.hpp
@@ -48,7 +48,7 @@
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointInT, typename PointNT, typename PointOutT> void
 pcl::PrincipalCurvaturesEstimation<PointInT, PointNT, PointOutT>::computePointPrincipalCurvatures (
-      const pcl::PointCloud<PointNT> &normals, int p_idx, const std::vector<int> &indices,
+      const pcl::PointCloud<PointNT> &normals, int p_idx, const pcl::Indices &indices,
       float &pcx, float &pcy, float &pcz, float &pc1, float &pc2)
 {
   EIGEN_ALIGN16 Eigen::Matrix3f I = Eigen::Matrix3f::Identity ();
@@ -116,7 +116,7 @@ pcl::PrincipalCurvaturesEstimation<PointInT, PointNT, PointOutT>::computeFeature
 {
   // Allocate enough space to hold the results
   // \note This resize is irrelevant for a radiusSearch ().
-  std::vector<int> nn_indices (k_);
+  pcl::Indices nn_indices (k_);
   std::vector<float> nn_dists (k_);
 
   output.is_dense = true;
diff --git a/features/include/pcl/features/impl/rift.hpp b/features/include/pcl/features/impl/rift.hpp
index cf90c309..b988a183 100644
--- a/features/include/pcl/features/impl/rift.hpp
+++ b/features/include/pcl/features/impl/rift.hpp
@@ -47,7 +47,7 @@
 template <typename PointInT, typename GradientT, typename PointOutT> void
 pcl::RIFTEstimation<PointInT, GradientT, PointOutT>::computeRIFT (
       const PointCloudIn &cloud, const PointCloudGradient &gradient, 
-      int p_idx, float radius, const std::vector<int> &indices, 
+      int p_idx, float radius, const pcl::Indices &indices, 
       const std::vector<float> &sqr_distances, Eigen::MatrixXf &rift_descriptor)
 {
   if (indices.empty ())
@@ -118,7 +118,7 @@ pcl::RIFTEstimation<PointInT, GradientT, PointOutT>::computeFeature (PointCloudO
     PCL_ERROR ("[pcl::%s::computeFeature] The search radius must be set before computing the feature!\n",
                getClassName ().c_str ());
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
@@ -128,7 +128,7 @@ pcl::RIFTEstimation<PointInT, GradientT, PointOutT>::computeFeature (PointCloudO
     PCL_ERROR ("[pcl::%s::computeFeature] The number of gradient bins must be greater than zero!\n",
                getClassName ().c_str ());
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
   if (nr_distance_bins_ <= 0)
@@ -136,7 +136,7 @@ pcl::RIFTEstimation<PointInT, GradientT, PointOutT>::computeFeature (PointCloudO
     PCL_ERROR ("[pcl::%s::computeFeature] The number of distance bins must be greater than zero!\n",
                getClassName ().c_str ());
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
@@ -145,7 +145,7 @@ pcl::RIFTEstimation<PointInT, GradientT, PointOutT>::computeFeature (PointCloudO
   {
     PCL_ERROR ("[pcl::%s::computeFeature] No input gradient was given!\n", getClassName ().c_str ());
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
   if (gradient_->size () != surface_->size ())
@@ -153,12 +153,12 @@ pcl::RIFTEstimation<PointInT, GradientT, PointOutT>::computeFeature (PointCloudO
     PCL_ERROR ("[pcl::%s::computeFeature] ", getClassName ().c_str ());
     PCL_ERROR ("The number of points in the input dataset differs from the number of points in the gradient!\n");
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
   Eigen::MatrixXf rift_descriptor (nr_distance_bins_, nr_gradient_bins_);
-  std::vector<int> nn_indices;
+  pcl::Indices nn_indices;
   std::vector<float> nn_dist_sqr;
  
   // Iterating over the entire index vector
diff --git a/features/include/pcl/features/impl/rops_estimation.hpp b/features/include/pcl/features/impl/rops_estimation.hpp
index 2351ba8a..b8fe64c6 100644
--- a/features/include/pcl/features/impl/rops_estimation.hpp
+++ b/features/include/pcl/features/impl/rops_estimation.hpp
@@ -43,6 +43,8 @@
 #include <pcl/features/rops_estimation.h>
 
 #include <array>
+#include <numeric> // for accumulate
+#include <Eigen/Eigenvalues> // for EigenSolver
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointInT, typename PointOutT>
@@ -136,7 +138,7 @@ pcl::ROPSEstimation <PointInT, PointOutT>::computeFeature (PointCloudOut &output
 {
   if (triangles_.empty ())
   {
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
@@ -145,13 +147,13 @@ pcl::ROPSEstimation <PointInT, PointOutT>::computeFeature (PointCloudOut &output
   //feature size = number_of_rotations * number_of_axis_to_rotate_around * number_of_projections * number_of_central_moments
   unsigned int feature_size = number_of_rotations_ * 3 * 3 * 5;
   const auto number_of_points = indices_->size ();
-  output.points.clear ();
-  output.points.reserve (number_of_points);
+  output.clear ();
+  output.reserve (number_of_points);
 
   for (const auto& idx: *indices_)
   {
     std::set <unsigned int> local_triangles;
-    std::vector <int> local_points;
+    pcl::Indices local_points;
     getLocalSurface ((*input_)[idx], local_triangles, local_points);
 
     Eigen::Matrix3f lrf_matrix;
@@ -203,9 +205,9 @@ pcl::ROPSEstimation <PointInT, PointOutT>::computeFeature (PointCloudOut &output
     else
       invert_norm = 1.0f / norm;
 
-    output.points.emplace_back ();
+    output.emplace_back ();
     for (std::size_t i_dim = 0; i_dim < feature_size; i_dim++)
-      output.points.back().histogram[i_dim] = feature[i_dim] * invert_norm;
+      output.back().histogram[i_dim] = feature[i_dim] * invert_norm;
   }
 }
 
@@ -226,7 +228,7 @@ pcl::ROPSEstimation <PointInT, PointOutT>::buildListOfPointsTriangles ()
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointInT, typename PointOutT> void
-pcl::ROPSEstimation <PointInT, PointOutT>::getLocalSurface (const PointInT& point, std::set <unsigned int>& local_triangles, std::vector <int>& local_points) const
+pcl::ROPSEstimation <PointInT, PointOutT>::getLocalSurface (const PointInT& point, std::set <unsigned int>& local_triangles, pcl::Indices& local_points) const
 {
   std::vector <float> distances;
   tree_->radiusSearch (point, support_radius_, local_points, distances);
@@ -386,11 +388,11 @@ pcl::ROPSEstimation <PointInT, PointOutT>::computeEigenVectors (const Eigen::Mat
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointInT, typename PointOutT> void
-pcl::ROPSEstimation <PointInT, PointOutT>::transformCloud (const PointInT& point, const Eigen::Matrix3f& matrix, const std::vector <int>& local_points, PointCloudIn& transformed_cloud) const
+pcl::ROPSEstimation <PointInT, PointOutT>::transformCloud (const PointInT& point, const Eigen::Matrix3f& matrix, const pcl::Indices& local_points, PointCloudIn& transformed_cloud) const
 {
   const auto number_of_points = local_points.size ();
-  transformed_cloud.points.clear ();
-  transformed_cloud.points.reserve (number_of_points);
+  transformed_cloud.clear ();
+  transformed_cloud.reserve (number_of_points);
 
   for (const auto& idx: local_points)
   {
@@ -404,7 +406,7 @@ pcl::ROPSEstimation <PointInT, PointOutT>::transformCloud (const PointInT& point
     new_point.x = transformed_point (0);
     new_point.y = transformed_point (1);
     new_point.z = transformed_point (2);
-    transformed_cloud.points.emplace_back (new_point);
+    transformed_cloud.emplace_back (new_point);
   }
 }
 
@@ -428,8 +430,8 @@ pcl::ROPSEstimation <PointInT, PointOutT>::rotateCloud (const PointInT& axis, co
   rotated_cloud.header = cloud.header;
   rotated_cloud.width = number_of_points;
   rotated_cloud.height = 1;
-  rotated_cloud.points.clear ();
-  rotated_cloud.points.reserve (number_of_points);
+  rotated_cloud.clear ();
+  rotated_cloud.reserve (number_of_points);
 
   min (0) = std::numeric_limits <float>::max ();
   min (1) = std::numeric_limits <float>::max ();
@@ -447,7 +449,7 @@ pcl::ROPSEstimation <PointInT, PointOutT>::rotateCloud (const PointInT& axis, co
     rotated_point.x = point (0);
     rotated_point.y = point (1);
     rotated_point.z = point (2);
-    rotated_cloud.points.emplace_back (rotated_point);
+    rotated_cloud.emplace_back (rotated_point);
 
     for (int i = 0; i < 3; ++i)
     {
diff --git a/features/include/pcl/features/impl/rsd.hpp b/features/include/pcl/features/impl/rsd.hpp
index 591c307c..0e7d661c 100644
--- a/features/include/pcl/features/impl/rsd.hpp
+++ b/features/include/pcl/features/impl/rsd.hpp
@@ -47,7 +47,7 @@
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointInT, typename PointNT, typename PointOutT> Eigen::MatrixXf
 pcl::computeRSD (const pcl::PointCloud<PointInT> &surface, const pcl::PointCloud<PointNT> &normals,
-		 const std::vector<int> &indices, double max_dist,
+		 const pcl::Indices &indices, double max_dist,
 		 int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram)
 {
   // Check if the full histogram has to be saved or not
@@ -74,7 +74,7 @@ pcl::computeRSD (const pcl::PointCloud<PointInT> &surface, const pcl::PointCloud
   min_max_angle_by_dist[0][0] = min_max_angle_by_dist[0][1] = 0.0;
 
   // Compute distance by normal angle distribution for points
-  std::vector<int>::const_iterator i, begin (indices.begin()), end (indices.end());
+  pcl::Indices::const_iterator i, begin (indices.begin()), end (indices.end());
   for (i = begin+1; i != end; ++i)
   {
     // compute angle between the two lines going through normals (disregard orientation!)
@@ -147,7 +147,7 @@ pcl::computeRSD (const pcl::PointCloud<PointInT> &surface, const pcl::PointCloud
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointNT, typename PointOutT> Eigen::MatrixXf
 pcl::computeRSD (const pcl::PointCloud<PointNT> &normals,
-		 const std::vector<int> &indices, const std::vector<float> &sqr_dists, double max_dist,
+		 const pcl::Indices &indices, const std::vector<float> &sqr_dists, double max_dist,
 		 int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram)
 {
   // Check if the full histogram has to be saved or not
@@ -175,7 +175,7 @@ pcl::computeRSD (const pcl::PointCloud<PointNT> &normals,
   }
   
   // Compute distance by normal angle distribution for points
-  std::vector<int>::const_iterator i, begin (indices.begin()), end (indices.end());
+  pcl::Indices::const_iterator i, begin (indices.begin()), end (indices.end());
   for (i = begin+1; i != end; ++i)
   {
     // compute angle between the two lines going through normals (disregard orientation!)
@@ -252,13 +252,13 @@ pcl::RSDEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut
   {
     PCL_ERROR ("[pcl::%s::computeFeature] A search radius needs to be set!\n", getClassName ().c_str ());
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
   // List of indices and corresponding squared distances for a neighborhood
   // \note resize is irrelevant for a radiusSearch ().
-  std::vector<int> nn_indices;
+  pcl::Indices nn_indices;
   std::vector<float> nn_sqr_dists;
 
   // Check if the full histogram has to be saved or not
diff --git a/features/include/pcl/features/impl/shot.hpp b/features/include/pcl/features/impl/shot.hpp
index 4f1ccedb..73a50e00 100644
--- a/features/include/pcl/features/impl/shot.hpp
+++ b/features/include/pcl/features/impl/shot.hpp
@@ -43,6 +43,8 @@
 #include <pcl/features/shot.h>
 #include <pcl/features/shot_lrf.h>
 
+#include <pcl/common/colors.h>  // for RGB2sRGB_LUT, XYZ2LAB_LUT
+
 // Useful constants.
 #define PST_PI 3.1415926535897932384626433832795
 #define PST_RAD_45 0.78539816339744830961566084581988
@@ -84,12 +86,14 @@ areEquals (float val1, float val2, float zeroFloatEps = zeroFloatEps8)
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> float
-pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::sRGB_LUT[256] = {- 1};
+template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT>
+std::array<float, 256>
+    pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::sRGB_LUT = pcl::RGB2sRGB_LUT<float, 8>();
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> float
-pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::sXYZ_LUT[4000] = {- 1};
+template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT>
+std::array<float, 4000>
+    pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::sXYZ_LUT = pcl::XYZ2LAB_LUT<float, 4000>();
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
@@ -97,28 +101,6 @@ pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::RGB2CIELAB (un
                                                                               unsigned char B, float &L, float &A,
                                                                               float &B2)
 {
-  // @TODO: C++17 supports constexpr lambda for compile time init
-  if (sRGB_LUT[0] < 0)
-  {
-    for (int i = 0; i < 256; i++)
-    {
-      float f = static_cast<float> (i) / 255.0f;
-      if (f > 0.04045)
-        sRGB_LUT[i] = powf ((f + 0.055f) / 1.055f, 2.4f);
-      else
-        sRGB_LUT[i] = f / 12.92f;
-    }
-
-    for (int i = 0; i < 4000; i++)
-    {
-      float f = static_cast<float> (i) / 4000.0f;
-      if (f > 0.008856)
-        sXYZ_LUT[i] = static_cast<float> (powf (f, 0.3333f));
-      else
-        sXYZ_LUT[i] = static_cast<float>((7.787 * f) + (16.0 / 116.0));
-    }
-  }
-
   float fr = sRGB_LUT[R];
   float fg = sRGB_LUT[G];
   float fb = sRGB_LUT[B];
@@ -193,7 +175,7 @@ pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::initCompute ()
 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
 pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::createBinDistanceShape (
     int index,
-    const std::vector<int> &indices,
+    const pcl::Indices &indices,
     std::vector<double> &bin_distance_shape)
 {
   bin_distance_shape.resize (indices.size ());
@@ -253,7 +235,7 @@ pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::normalizeHistog
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
 pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::interpolateSingleChannel (
-    const std::vector<int> &indices,
+    const pcl::Indices &indices,
     const std::vector<float> &sqr_dists,
     const int index,
     std::vector<double> &binDistance,
@@ -428,7 +410,7 @@ pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::interpolateSing
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
 pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::interpolateDoubleChannel (
-  const std::vector<int> &indices,
+  const pcl::Indices &indices,
   const std::vector<float> &sqr_dists,
   const int index,
   std::vector<double> &binDistanceShape,
@@ -644,7 +626,7 @@ pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::interpolateDou
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
 pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::computePointSHOT (
-  const int index, const std::vector<int> &indices, const std::vector<float> &sqr_dists, Eigen::VectorXf &shot)
+  const int index, const pcl::Indices &indices, const std::vector<float> &sqr_dists, Eigen::VectorXf &shot)
 {
   // Clear the resultant shot
   shot.setZero ();
@@ -729,7 +711,7 @@ pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::computePointSH
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
 pcl::SHOTEstimation<PointInT, PointNT, PointOutT, PointRFT>::computePointSHOT (
-  const int index, const std::vector<int> &indices, const std::vector<float> &sqr_dists, Eigen::VectorXf &shot)
+  const int index, const pcl::Indices &indices, const std::vector<float> &sqr_dists, Eigen::VectorXf &shot)
 {
   //Skip the current feature if the number of its neighbors is not sufficient for its description
   if (indices.size () < 5)
@@ -773,7 +755,7 @@ pcl::SHOTEstimation<PointInT, PointNT, PointOutT, PointRFT>::computeFeature (pcl
 
   // Allocate enough space to hold the results
   // \note This resize is irrelevant for a radiusSearch ().
-  std::vector<int> nn_indices (k_);
+  pcl::Indices nn_indices (k_);
   std::vector<float> nn_dists (k_);
 
   output.is_dense = true;
@@ -845,7 +827,7 @@ pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::computeFeature
 
   // Allocate enough space to hold the results
   // \note This resize is irrelevant for a radiusSearch ().
-  std::vector<int> nn_indices (k_);
+  pcl::Indices nn_indices (k_);
   std::vector<float> nn_dists (k_);
 
   output.is_dense = true;
diff --git a/features/include/pcl/features/impl/shot_lrf.hpp b/features/include/pcl/features/impl/shot_lrf.hpp
index f63697d4..1d99772b 100644
--- a/features/include/pcl/features/impl/shot_lrf.hpp
+++ b/features/include/pcl/features/impl/shot_lrf.hpp
@@ -40,6 +40,7 @@
 #ifndef PCL_FEATURES_IMPL_SHOT_LRF_H_
 #define PCL_FEATURES_IMPL_SHOT_LRF_H_
 
+#include <Eigen/Eigenvalues> // for SelfAdjointEigenSolver
 #include <utility>
 #include <pcl/features/shot_lrf.h>
 
@@ -49,7 +50,7 @@ template<typename PointInT, typename PointOutT> float
 pcl::SHOTLocalReferenceFrameEstimation<PointInT, PointOutT>::getLocalRF (const int& current_point_idx, Eigen::Matrix3f &rf)
 {
   const Eigen::Vector4f& central_point = (*input_)[current_point_idx].getVector4fMap ();
-  std::vector<int> n_indices;
+  pcl::Indices n_indices;
   std::vector<float> n_sqr_distances;
 
   this->searchForNeighbors (current_point_idx, search_parameter_, n_indices, n_sqr_distances);
diff --git a/features/include/pcl/features/impl/shot_lrf_omp.hpp b/features/include/pcl/features/impl/shot_lrf_omp.hpp
index 814bb38e..a28123b3 100644
--- a/features/include/pcl/features/impl/shot_lrf_omp.hpp
+++ b/features/include/pcl/features/impl/shot_lrf_omp.hpp
@@ -40,7 +40,6 @@
 #ifndef PCL_FEATURES_IMPL_SHOT_LRF_OMP_H_
 #define PCL_FEATURES_IMPL_SHOT_LRF_OMP_H_
 
-#include <utility>
 #include <pcl/features/shot_lrf_omp.h>
 #include <pcl/features/shot_lrf.h>
 
@@ -85,7 +84,7 @@ pcl::SHOTLocalReferenceFrameEstimationOMP<PointInT, PointOutT>::computeFeature (
     //output_rf.confidence = getLocalRF ((*indices_)[i], rf);
     //if (output_rf.confidence == std::numeric_limits<float>::max ())
 
-    std::vector<int> n_indices;
+    pcl::Indices n_indices;
     std::vector<float> n_sqr_distances;
     this->searchForNeighbors ((*indices_)[i], search_parameter_, n_indices, n_sqr_distances);
     if (getLocalRF ((*indices_)[i], rf) == std::numeric_limits<float>::max ())
diff --git a/features/include/pcl/features/impl/shot_omp.hpp b/features/include/pcl/features/impl/shot_omp.hpp
index 0ccc877c..4a0a01a1 100644
--- a/features/include/pcl/features/impl/shot_omp.hpp
+++ b/features/include/pcl/features/impl/shot_omp.hpp
@@ -173,7 +173,7 @@ pcl::SHOTEstimationOMP<PointInT, PointNT, PointOutT, PointRFT>::computeFeature (
 
     // Allocate enough space to hold the results
     // \note This resize is irrelevant for a radiusSearch ().
-    std::vector<int> nn_indices (k_);
+    pcl::Indices nn_indices (k_);
     std::vector<float> nn_dists (k_);
 
     if (!isFinite ((*input_)[(*indices_)[idx]]) || lrf_is_nan || this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices,
@@ -248,7 +248,7 @@ pcl::SHOTColorEstimationOMP<PointInT, PointNT, PointOutT, PointRFT>::computeFeat
 
     // Allocate enough space to hold the results
     // \note This resize is irrelevant for a radiusSearch ().
-    std::vector<int> nn_indices (k_);
+    pcl::Indices nn_indices (k_);
     std::vector<float> nn_dists (k_);
 
     bool lrf_is_nan = false;
diff --git a/features/include/pcl/features/impl/spin_image.hpp b/features/include/pcl/features/impl/spin_image.hpp
index 393a34fa..319ea73e 100644
--- a/features/include/pcl/features/impl/spin_image.hpp
+++ b/features/include/pcl/features/impl/spin_image.hpp
@@ -97,7 +97,7 @@ pcl::SpinImageEstimation<PointInT, PointNT, PointOutT>::computeSiForPoint (int i
   else
     bin_size = search_radius_ / image_width_ / sqrt(2.0);
 
-  std::vector<int> nn_indices;
+  pcl::Indices nn_indices;
   std::vector<float> nn_sqr_dists;
   const int neighb_cnt = this->searchForNeighbors (index, search_radius_, nn_indices, nn_sqr_dists);
   if (neighb_cnt < static_cast<int> (min_pts_neighb_))
diff --git a/features/include/pcl/features/impl/statistical_multiscale_interest_region_extraction.hpp b/features/include/pcl/features/impl/statistical_multiscale_interest_region_extraction.hpp
index e3978c74..c803b343 100644
--- a/features/include/pcl/features/impl/statistical_multiscale_interest_region_extraction.hpp
+++ b/features/include/pcl/features/impl/statistical_multiscale_interest_region_extraction.hpp
@@ -43,7 +43,7 @@
 #include <pcl/features/statistical_multiscale_interest_region_extraction.h>
 #include <pcl/kdtree/kdtree_flann.h>
 #include <pcl/common/distances.h>
-#include <pcl/features/boost.h>
+#include <pcl/console/print.h> // for PCL_INFO, PCL_ERROR
 #include <boost/graph/adjacency_list.hpp>
 #include <boost/graph/johnson_all_pairs_shortest.hpp>
 
@@ -63,7 +63,7 @@ pcl::StatisticalMultiscaleInterestRegionExtraction<PointT>::generateCloudGraph (
 
   for (std::size_t point_i = 0; point_i < input_->size (); ++point_i)
   {
-    std::vector<int> k_indices (16);
+    pcl::Indices k_indices (16);
     std::vector<float> k_distances (16);
     kdtree.nearestKSearch (static_cast<int> (point_i), 16, k_indices, k_distances);
 
@@ -212,7 +212,7 @@ pcl::StatisticalMultiscaleInterestRegionExtraction<PointT>::extractExtrema (std:
       std::vector<int> nn_indices;
       geodesicFixedRadiusSearch (point_i, scale_values_[scale_i], nn_indices);
       bool is_max_point = true, is_min_point = true;
-      for (const int &nn_index : nn_indices)
+      for (const auto &nn_index : nn_indices)
         if (F_scales_[scale_i][point_i] < F_scales_[scale_i][nn_index])
           is_max_point = false;
         else
@@ -234,7 +234,7 @@ pcl::StatisticalMultiscaleInterestRegionExtraction<PointT>::extractExtrema (std:
           (is_max[scale_i - 1][point_i] && is_max[scale_i][point_i] && is_max[scale_i + 1][point_i]))
         {
         // add the point to the result vector
-        IndicesPtr region (new std::vector<int>);
+        IndicesPtr region (new pcl::Indices);
         region->push_back (static_cast<int> (point_i));
 
         // and also add its scale-sized geodesic neighborhood
diff --git a/features/include/pcl/features/impl/usc.hpp b/features/include/pcl/features/impl/usc.hpp
index fecb07db..e3e1c784 100644
--- a/features/include/pcl/features/impl/usc.hpp
+++ b/features/include/pcl/features/impl/usc.hpp
@@ -40,6 +40,7 @@
 
 #pragma once
 
+#include <numeric> // for partial_sum
 #include <pcl/features/usc.h>
 #include <pcl/features/shot_lrf.h>
 #include <pcl/common/angles.h>
@@ -155,7 +156,7 @@ pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::computePointDescriptor (
                                 (*frames_)[index].z_axis[2]);
 
   // Find every point within specified search_radius_
-  std::vector<int> nn_indices;
+  pcl::Indices nn_indices;
   std::vector<float> nn_dists;
   const std::size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
   // For each point within radius
@@ -200,7 +201,7 @@ pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::computePointDescriptor (
     const auto l = std::distance(phi_divisions_.cbegin (), std::prev(phi_min));
 
     /// Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour
-    std::vector<int> neighbour_indices;
+    pcl::Indices neighbour_indices;
     std::vector<float> neighbour_didtances;
     float point_density = static_cast<float> (searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_didtances));
     /// point_density is always bigger than 0 because FindPointsWithinRadius returns at least the point itself
diff --git a/features/include/pcl/features/impl/vfh.hpp b/features/include/pcl/features/impl/vfh.hpp
index f70fa98d..af706a33 100644
--- a/features/include/pcl/features/impl/vfh.hpp
+++ b/features/include/pcl/features/impl/vfh.hpp
@@ -67,7 +67,7 @@ pcl::VFHEstimation<PointInT, PointNT, PointOutT>::compute (PointCloudOut &output
   if (!initCompute ())
   {
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
   // Copy the header
@@ -79,7 +79,7 @@ pcl::VFHEstimation<PointInT, PointNT, PointOutT>::compute (PointCloudOut &output
   // (see http://dev.pointclouds.org/issues/657)
   output.width = output.height = 1;
   output.is_dense = input_->is_dense;
-  output.points.resize (1);
+  output.resize (1);
 
   // Perform the actual feature computation
   computeFeature (output);
@@ -93,7 +93,7 @@ pcl::VFHEstimation<PointInT, PointNT, PointOutT>::computePointSPFHSignature (con
                                                                              const Eigen::Vector4f &centroid_n,
                                                                              const pcl::PointCloud<PointInT> &cloud,
                                                                              const pcl::PointCloud<PointNT> &normals,
-                                                                             const std::vector<int> &indices)
+                                                                             const pcl::Indices &indices)
 {
   Eigen::Vector4f pfh_tuple;
   // Reset the whole thing
@@ -130,7 +130,7 @@ pcl::VFHEstimation<PointInT, PointNT, PointOutT>::computePointSPFHSignature (con
     hist_incr_size_component = hist_incr;
 
   // Iterate over all the points in the neighborhood
-  for (const int &index : indices)
+  for (const auto &index : indices)
   {
     // Compute the pair P to NNi
     if (!computePairFeatures (centroid_p, centroid_n, cloud[index].getVector4fMap (),
@@ -234,7 +234,7 @@ pcl::VFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut
   }
 
   // We only output _1_ signature
-  output.points.resize (1);
+  output.resize (1);
   output.width = 1;
   output.height = 1;
 
diff --git a/features/include/pcl/features/integral_image_normal.h b/features/include/pcl/features/integral_image_normal.h
index 5219830c..183a6688 100644
--- a/features/include/pcl/features/integral_image_normal.h
+++ b/features/include/pcl/features/integral_image_normal.h
@@ -41,7 +41,6 @@
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
 #include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
 #include <pcl/features/feature.h>
 #include <pcl/features/integral_image2D.h>
 
diff --git a/features/include/pcl/features/intensity_gradient.h b/features/include/pcl/features/intensity_gradient.h
index 07dc44f6..e8a2a1aa 100644
--- a/features/include/pcl/features/intensity_gradient.h
+++ b/features/include/pcl/features/intensity_gradient.h
@@ -98,7 +98,7 @@ namespace pcl
         */
       void
       computePointIntensityGradient (const pcl::PointCloud<PointInT> &cloud,
-                                     const std::vector<int> &indices,
+                                     const pcl::Indices &indices,
                                      const Eigen::Vector3f &point, 
                                      float mean_intensity, 
                                      const Eigen::Vector3f &normal,
diff --git a/features/include/pcl/features/intensity_spin.h b/features/include/pcl/features/intensity_spin.h
index 14e24490..6af285a1 100644
--- a/features/include/pcl/features/intensity_spin.h
+++ b/features/include/pcl/features/intensity_spin.h
@@ -92,7 +92,7 @@ namespace pcl
       void 
       computeIntensitySpinImage (const PointCloudIn &cloud, 
                                  float radius, float sigma, int k, 
-                                 const std::vector<int> &indices, 
+                                 const pcl::Indices &indices, 
                                  const std::vector<float> &squared_distances, 
                                  Eigen::MatrixXf &intensity_spin_image);
 
diff --git a/features/include/pcl/features/linear_least_squares_normal.h b/features/include/pcl/features/linear_least_squares_normal.h
index 892a8057..e6691f39 100644
--- a/features/include/pcl/features/linear_least_squares_normal.h
+++ b/features/include/pcl/features/linear_least_squares_normal.h
@@ -38,8 +38,6 @@
 
 #pragma once
 
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
 #include <pcl/features/feature.h>
 
 namespace pcl
diff --git a/features/include/pcl/features/moment_invariants.h b/features/include/pcl/features/moment_invariants.h
index c744bd59..6b860dc0 100644
--- a/features/include/pcl/features/moment_invariants.h
+++ b/features/include/pcl/features/moment_invariants.h
@@ -82,7 +82,7 @@ namespace pcl
         */
       void 
       computePointMomentInvariants (const pcl::PointCloud<PointInT> &cloud, 
-                                    const std::vector<int> &indices, 
+                                    const pcl::Indices &indices, 
                                     float &j1, float &j2, float &j3);
 
       /** \brief Compute the 3 moment invariants (j1, j2, j3) for a given set of points, using their indices.
diff --git a/features/include/pcl/features/moment_of_inertia_estimation.h b/features/include/pcl/features/moment_of_inertia_estimation.h
index 3866a976..f309efbd 100644
--- a/features/include/pcl/features/moment_of_inertia_estimation.h
+++ b/features/include/pcl/features/moment_of_inertia_estimation.h
@@ -43,7 +43,6 @@
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
 #include <pcl/pcl_base.h>
-#include <pcl/PointIndices.h>
 
 namespace pcl
 {
diff --git a/features/include/pcl/features/multiscale_feature_persistence.h b/features/include/pcl/features/multiscale_feature_persistence.h
index ca55b609..acabe3a4 100644
--- a/features/include/pcl/features/multiscale_feature_persistence.h
+++ b/features/include/pcl/features/multiscale_feature_persistence.h
@@ -89,7 +89,7 @@ namespace pcl
        */
       void
       determinePersistentFeatures (FeatureCloud &output_features,
-                                   shared_ptr<std::vector<int> > &output_indices);
+                                   pcl::IndicesPtr &output_indices);
 
       /** \brief Method for setting the scale parameters for the algorithm
        * \param scale_values vector of scales to determine the characteristic of each scaling step
diff --git a/features/include/pcl/features/narf.h b/features/include/pcl/features/narf.h
index 34dbfacf..6d807513 100644
--- a/features/include/pcl/features/narf.h
+++ b/features/include/pcl/features/narf.h
@@ -40,7 +40,6 @@
 
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
-#include <pcl/features/eigen.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_representation.h>
 
diff --git a/features/include/pcl/features/narf_descriptor.h b/features/include/pcl/features/narf_descriptor.h
index b2b7c46d..c454a3b4 100644
--- a/features/include/pcl/features/narf_descriptor.h
+++ b/features/include/pcl/features/narf_descriptor.h
@@ -70,14 +70,14 @@ namespace pcl
       
       // =====CONSTRUCTOR & DESTRUCTOR=====
       /** Constructor */
-      NarfDescriptor (const RangeImage* range_image=nullptr, const std::vector<int>* indices=nullptr);
+      NarfDescriptor (const RangeImage* range_image=nullptr, const pcl::Indices* indices=nullptr);
       /** Destructor */
       ~NarfDescriptor();
       
       // =====METHODS=====
       //! Set input data
       void 
-      setRangeImage (const RangeImage* range_image, const std::vector<int>* indices=nullptr);
+      setRangeImage (const RangeImage* range_image, const pcl::Indices* indices=nullptr);
       
       //! Overwrite the compute function of the base class
       void 
diff --git a/features/include/pcl/features/normal_3d.h b/features/include/pcl/features/normal_3d.h
index 39618043..8fe9c32a 100644
--- a/features/include/pcl/features/normal_3d.h
+++ b/features/include/pcl/features/normal_3d.h
@@ -91,7 +91,7 @@ namespace pcl
     * \ingroup features
     */
   template <typename PointT> inline bool
-  computePointNormal (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
+  computePointNormal (const pcl::PointCloud<PointT> &cloud, const pcl::Indices &indices,
                       Eigen::Vector4f &plane_parameters, float &curvature)
   {
     // Placeholder for the 3x3 covariance matrix at each surface patch
@@ -202,12 +202,12 @@ namespace pcl
     */
   template<typename PointNT> inline bool
   flipNormalTowardsNormalsMean ( pcl::PointCloud<PointNT> const &normal_cloud,
-                                 std::vector<int> const &normal_indices,
+                                 pcl::Indices const &normal_indices,
                                  Eigen::Vector3f &normal)
   {
     Eigen::Vector3f normal_mean = Eigen::Vector3f::Zero ();
 
-    for (const int &normal_index : normal_indices)
+    for (const auto &normal_index : normal_indices)
     {
       const PointNT& cur_pt = normal_cloud[normal_index];
 
@@ -281,7 +281,7 @@ namespace pcl
         * \f]
         */
       inline bool
-      computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices,
+      computePointNormal (const pcl::PointCloud<PointInT> &cloud, const pcl::Indices &indices,
                           Eigen::Vector4f &plane_parameters, float &curvature)
       {
         if (indices.size () < 3 ||
@@ -310,7 +310,7 @@ namespace pcl
         * \f]
         */
       inline bool
-      computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices,
+      computePointNormal (const pcl::PointCloud<PointInT> &cloud, const pcl::Indices &indices,
                           float &nx, float &ny, float &nz, float &curvature)
       {
         if (indices.size () < 3 ||
diff --git a/features/include/pcl/features/our_cvfh.h b/features/include/pcl/features/our_cvfh.h
index 66ca5b5e..a40503fd 100644
--- a/features/include/pcl/features/our_cvfh.h
+++ b/features/include/pcl/features/our_cvfh.h
@@ -41,8 +41,6 @@
 #pragma once
 
 #include <pcl/features/feature.h>
-#include <pcl/search/pcl_search.h>
-#include <pcl/common/common.h>
 
 namespace pcl
 {
@@ -148,8 +146,8 @@ namespace pcl
        * \param[in] threshold threshold value for curvature
        */
       void
-      filterNormalsWithHighCurvature (const pcl::PointCloud<PointNT> & cloud, std::vector<int> & indices_to_use, std::vector<int> &indices_out,
-                                      std::vector<int> &indices_in, float threshold);
+      filterNormalsWithHighCurvature (const pcl::PointCloud<PointNT> & cloud, pcl::Indices & indices_to_use, pcl::Indices &indices_out,
+                                      pcl::Indices &indices_in, float threshold);
 
       /** \brief Set the viewpoint.
        * \param[in] vpx the X coordinate of the viewpoint
diff --git a/features/include/pcl/features/pfh.h b/features/include/pcl/features/pfh.h
index 13ec004d..30df4603 100644
--- a/features/include/pcl/features/pfh.h
+++ b/features/include/pcl/features/pfh.h
@@ -42,8 +42,8 @@
 
 #include <pcl/point_types.h>
 #include <pcl/features/feature.h>
-#include <pcl/features/pfh_tools.h>
 #include <map>
+#include <queue> // for std::queue
 
 namespace pcl
 {
@@ -177,7 +177,7 @@ namespace pcl
         */
       void 
       computePointPFHSignature (const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals, 
-                                const std::vector<int> &indices, int nr_split, Eigen::VectorXf &pfh_histogram);
+                                const pcl::Indices &indices, int nr_split, Eigen::VectorXf &pfh_histogram);
 
     protected:
       /** \brief Estimate the Point Feature Histograms (PFH) descriptors at a set of points given by
diff --git a/features/include/pcl/features/pfhrgb.h b/features/include/pcl/features/pfhrgb.h
index fd4bf293..60d702f7 100644
--- a/features/include/pcl/features/pfhrgb.h
+++ b/features/include/pcl/features/pfhrgb.h
@@ -40,7 +40,6 @@
 #pragma once
 
 #include <pcl/features/feature.h>
-#include <pcl/features/pfh_tools.h>
 
 namespace pcl
 {
@@ -72,7 +71,7 @@ namespace pcl
 
       void
       computePointPFHRGBSignature (const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
-                                   const std::vector<int> &indices, int nr_split, Eigen::VectorXf &pfhrgb_histogram);
+                                   const pcl::Indices &indices, int nr_split, Eigen::VectorXf &pfhrgb_histogram);
 
     protected:
       void
diff --git a/features/include/pcl/features/ppfrgb.h b/features/include/pcl/features/ppfrgb.h
index def06b33..c50fee1d 100644
--- a/features/include/pcl/features/ppfrgb.h
+++ b/features/include/pcl/features/ppfrgb.h
@@ -38,7 +38,6 @@
 #pragma once
 
 #include <pcl/features/feature.h>
-#include <pcl/features/boost.h>
 
 namespace pcl
 {
diff --git a/features/include/pcl/features/principal_curvatures.h b/features/include/pcl/features/principal_curvatures.h
index ff70d153..62025994 100644
--- a/features/include/pcl/features/principal_curvatures.h
+++ b/features/include/pcl/features/principal_curvatures.h
@@ -40,7 +40,6 @@
 
 #pragma once
 
-#include <pcl/features/eigen.h>
 #include <pcl/features/feature.h>
 
 namespace pcl
@@ -99,7 +98,7 @@ namespace pcl
        */
       void
       computePointPrincipalCurvatures (const pcl::PointCloud<PointNT> &normals,
-                                       int p_idx, const std::vector<int> &indices,
+                                       int p_idx, const pcl::Indices &indices,
                                        float &pcx, float &pcy, float &pcz, float &pc1, float &pc2);
 
     protected:
diff --git a/features/include/pcl/features/rift.h b/features/include/pcl/features/rift.h
index 36e94b5d..061b262e 100644
--- a/features/include/pcl/features/rift.h
+++ b/features/include/pcl/features/rift.h
@@ -127,7 +127,7 @@ namespace pcl
         */
       void 
       computeRIFT (const PointCloudIn &cloud, const PointCloudGradient &gradient, int p_idx, float radius,
-                   const std::vector<int> &indices, const std::vector<float> &squared_distances, 
+                   const pcl::Indices &indices, const std::vector<float> &squared_distances, 
                    Eigen::MatrixXf &rift_descriptor);
 
     protected:
diff --git a/features/include/pcl/features/rops_estimation.h b/features/include/pcl/features/rops_estimation.h
index ee87b68e..60e44f12 100644
--- a/features/include/pcl/features/rops_estimation.h
+++ b/features/include/pcl/features/rops_estimation.h
@@ -41,6 +41,7 @@
 
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
+#include <pcl/Vertices.h> // for Vertices
 #include <pcl/features/feature.h>
 #include <set>
 
@@ -136,7 +137,7 @@ namespace pcl
         * \param[out] local_points stores the indices of the points that belong to the local surface
         */
       void
-      getLocalSurface (const PointInT& point, std::set <unsigned int>& local_triangles, std::vector <int>& local_points) const;
+      getLocalSurface (const PointInT& point, std::set <unsigned int>& local_triangles, pcl::Indices& local_points) const;
 
       /** \brief This method computes LRF (Local Reference Frame) matrix for the given point.
         * \param[in] point point for which the LRF is computed
@@ -166,7 +167,7 @@ namespace pcl
         * \param[out] transformed_cloud stores the transformed cloud
         */
       void
-      transformCloud (const PointInT& point, const Eigen::Matrix3f& matrix, const std::vector <int>& local_points, PointCloudIn& transformed_cloud) const;
+      transformCloud (const PointInT& point, const Eigen::Matrix3f& matrix, const pcl::Indices& local_points, PointCloudIn& transformed_cloud) const;
 
       /** \brief This method rotates the cloud around the given axis and computes AABB of the rotated cloud.
         * \param[in] axis axis around which cloud must be rotated
diff --git a/features/include/pcl/features/rsd.h b/features/include/pcl/features/rsd.h
index dca37083..5cdea911 100644
--- a/features/include/pcl/features/rsd.h
+++ b/features/include/pcl/features/rsd.h
@@ -56,7 +56,7 @@ namespace pcl
   template <int N> void
   getFeaturePointCloud (const std::vector<Eigen::MatrixXf, Eigen::aligned_allocator<Eigen::MatrixXf> > &histograms2D, PointCloud<Histogram<N> > &histogramsPC)
   {
-    histogramsPC.points.resize (histograms2D.size ());
+    histogramsPC.resize (histograms2D.size ());
     histogramsPC.width    = histograms2D.size ();
     histogramsPC.height   = 1;
     histogramsPC.is_dense = true;
@@ -64,7 +64,7 @@ namespace pcl
     const int rows  = histograms2D.at(0).rows();
     const int cols = histograms2D.at(0).cols();
 
-    typename PointCloud<Histogram<N> >::VectorType::iterator it = histogramsPC.points.begin ();
+    typename PointCloud<Histogram<N> >::VectorType::iterator it = histogramsPC.begin ();
     for (const Eigen::MatrixXf& h : histograms2D)
     {
       Eigen::Map<Eigen::MatrixXf> histogram (&(it->histogram[0]), rows, cols);
@@ -86,19 +86,9 @@ namespace pcl
     */
   template <typename PointInT, typename PointNT, typename PointOutT> Eigen::MatrixXf
   computeRSD (const pcl::PointCloud<PointInT> &surface, const pcl::PointCloud<PointNT> &normals,
-              const std::vector<int> &indices, double max_dist,
+              const pcl::Indices &indices, double max_dist,
               int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram = false);
 
-  template <typename PointInT, typename PointNT, typename PointOutT>
-  PCL_DEPRECATED(1, 12, "use computeRSD() overload that takes input point clouds by const reference")
-  Eigen::MatrixXf
-  computeRSD (typename pcl::PointCloud<PointInT>::ConstPtr &surface, typename pcl::PointCloud<PointNT>::ConstPtr &normals,
-              const std::vector<int> &indices, double max_dist,
-              int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram = false)
-  {
-    return computeRSD (*surface, *normals, indices, max_dist, nr_subdiv, plane_radius, radii, compute_histogram);
-  }
-
   /** \brief Estimate the Radius-based Surface Descriptor (RSD) for a given point based on its spatial neighborhood of 3D points with normals
     * \param[in] normals the dataset containing the surface normals at each point in the dataset
     * \param[in] indices the neighborhood point indices in the dataset (first point is used as the reference)
@@ -112,19 +102,9 @@ namespace pcl
     */
   template <typename PointNT, typename PointOutT> Eigen::MatrixXf
   computeRSD (const pcl::PointCloud<PointNT> &normals,
-              const std::vector<int> &indices, const std::vector<float> &sqr_dists, double max_dist,
+              const pcl::Indices &indices, const std::vector<float> &sqr_dists, double max_dist,
               int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram = false);
 
-  template <typename PointNT, typename PointOutT>
-  PCL_DEPRECATED(1, 12, "use computeRSD() overload that takes input point cloud by const reference")
-  Eigen::MatrixXf
-  computeRSD (typename pcl::PointCloud<PointNT>::ConstPtr &normals,
-              const std::vector<int> &indices, const std::vector<float> &sqr_dists, double max_dist,
-              int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram = false)
-  {
-    return computeRSD (*normals, indices, sqr_dists, max_dist, nr_subdiv, plane_radius, radii, compute_histogram);
-  }
-
   /** \brief @b RSDEstimation estimates the Radius-based Surface Descriptor (minimal and maximal radius of the local surface's curves)
     * for a given point cloud dataset containing points and normals.
     *
diff --git a/features/include/pcl/features/shot.h b/features/include/pcl/features/shot.h
index 19a02470..0e3581fe 100644
--- a/features/include/pcl/features/shot.h
+++ b/features/include/pcl/features/shot.h
@@ -42,6 +42,8 @@
 #include <pcl/point_types.h>
 #include <pcl/features/feature.h>
 
+#include <array>  // for sRGB_LUT, sXYZ_LUT
+
 namespace pcl
 {
   /** \brief SHOTEstimation estimates the Signature of Histograms of OrienTations (SHOT) descriptor for
@@ -98,7 +100,6 @@ namespace pcl
       {
         feature_name_ = "SHOTEstimation";
       };
-      
 
     public:
 
@@ -113,7 +114,7 @@ namespace pcl
          */
       virtual void
       computePointSHOT (const int index,
-                        const std::vector<int> &indices,
+                        const pcl::Indices &indices,
                         const std::vector<float> &sqr_dists,
                         Eigen::VectorXf &shot) = 0;
 
@@ -141,7 +142,7 @@ namespace pcl
         * \param[out] shot the resultant SHOT histogram
         */
       void
-      interpolateSingleChannel (const std::vector<int> &indices,
+      interpolateSingleChannel (const pcl::Indices &indices,
                                 const std::vector<float> &sqr_dists,
                                 const int index,
                                 std::vector<double> &binDistance,
@@ -162,7 +163,7 @@ namespace pcl
         * \param[out] bin_distance_shape the resultant histogram
         */
       void
-      createBinDistanceShape (int index, const std::vector<int> &indices,
+      createBinDistanceShape (int index, const pcl::Indices &indices,
                               std::vector<double> &bin_distance_shape);
 
       /** \brief The number of bins in each shape histogram. */
@@ -261,7 +262,7 @@ namespace pcl
         */
       void
       computePointSHOT (const int index,
-                        const std::vector<int> &indices,
+                        const pcl::Indices &indices,
                         const std::vector<float> &sqr_dists,
                         Eigen::VectorXf &shot) override;
     protected:
@@ -347,7 +348,7 @@ namespace pcl
         */
       void
       computePointSHOT (const int index,
-                        const std::vector<int> &indices,
+                        const pcl::Indices &indices,
                         const std::vector<float> &sqr_dists,
                         Eigen::VectorXf &shot) override;
     protected:
@@ -370,7 +371,7 @@ namespace pcl
         * \param[out] shot the resultant SHOT histogram
         */
       void
-      interpolateDoubleChannel (const std::vector<int> &indices,
+      interpolateDoubleChannel (const pcl::Indices &indices,
                                 const std::vector<float> &sqr_dists,
                                 const int index,
                                 std::vector<double> &binDistanceShape,
@@ -400,8 +401,8 @@ namespace pcl
       static void
       RGB2CIELAB (unsigned char R, unsigned char G, unsigned char B, float &L, float &A, float &B2);
 
-      static float sRGB_LUT[256];
-      static float sXYZ_LUT[4000];
+      static std::array<float, 256> sRGB_LUT;
+      static std::array<float, 4000> sXYZ_LUT;
   };
 }
 
diff --git a/features/include/pcl/features/statistical_multiscale_interest_region_extraction.h b/features/include/pcl/features/statistical_multiscale_interest_region_extraction.h
index 35345d52..f71cf541 100644
--- a/features/include/pcl/features/statistical_multiscale_interest_region_extraction.h
+++ b/features/include/pcl/features/statistical_multiscale_interest_region_extraction.h
@@ -64,7 +64,7 @@ namespace pcl
   class StatisticalMultiscaleInterestRegionExtraction : public PCLBase<PointT>
   {
     public:
-      using IndicesPtr = shared_ptr<std::vector<int> >;
+      using IndicesPtr = shared_ptr<pcl::Indices >;
       using Ptr = shared_ptr<StatisticalMultiscaleInterestRegionExtraction<PointT> >;
       using ConstPtr = shared_ptr<const StatisticalMultiscaleInterestRegionExtraction<PointT> >;
 
diff --git a/features/include/pcl/features/vfh.h b/features/include/pcl/features/vfh.h
index c21b7a60..d3dae278 100644
--- a/features/include/pcl/features/vfh.h
+++ b/features/include/pcl/features/vfh.h
@@ -114,7 +114,7 @@ namespace pcl
       void
       computePointSPFHSignature (const Eigen::Vector4f &centroid_p, const Eigen::Vector4f &centroid_n,
                                  const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
-                                 const std::vector<int> &indices);
+                                 const pcl::Indices &indices);
 
       /** \brief Set the viewpoint.
         * \param[in] vpx the X coordinate of the viewpoint
diff --git a/features/src/narf.cpp b/features/src/narf.cpp
index d1faaeb9..235e98eb 100644
--- a/features/src/narf.cpp
+++ b/features/src/narf.cpp
@@ -601,7 +601,7 @@ Narf::loadBinary (const std::string& filename)
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-NarfDescriptor::NarfDescriptor (const RangeImage* range_image, const std::vector<int>* indices) : 
+NarfDescriptor::NarfDescriptor (const RangeImage* range_image, const pcl::Indices* indices) : 
   range_image_ ()
 {
   setRangeImage (range_image, indices);
@@ -614,12 +614,12 @@ NarfDescriptor::~NarfDescriptor ()
 
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 void 
-NarfDescriptor::setRangeImage (const RangeImage* range_image, const std::vector<int>* indices)
+NarfDescriptor::setRangeImage (const RangeImage* range_image, const pcl::Indices* indices)
 {
   range_image_ = range_image;
   if (indices != nullptr)
   {
-    IndicesPtr indicesptr (new std::vector<int> (*indices));
+    IndicesPtr indicesptr (new pcl::Indices (*indices));
     setIndices (indicesptr);
   }
 }
@@ -630,7 +630,7 @@ NarfDescriptor::computeFeature(NarfDescriptor::PointCloudOut& output)
 {
   //std::cout << __PRETTY_FUNCTION__ << " called.\n";
   
-  output.points.clear();
+  output.clear();
   
   if (range_image_==nullptr)
   {
@@ -638,7 +638,7 @@ NarfDescriptor::computeFeature(NarfDescriptor::PointCloudOut& output)
               << ": RangeImage is not set. Sorry, the NARF descriptor calculation works on range images, not on normal point clouds."
               << " Use setRangeImage(...).\n\n";
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
   if (parameters_.support_size <= 0.0f)
@@ -646,7 +646,7 @@ NarfDescriptor::computeFeature(NarfDescriptor::PointCloudOut& output)
     std::cerr << __PRETTY_FUNCTION__
               << ": support size is not set. Use getParameters().support_size = ...\n\n";
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
   std::vector<Narf*> feature_list;
@@ -673,7 +673,7 @@ NarfDescriptor::computeFeature(NarfDescriptor::PointCloudOut& output)
   }
   
   // Copy to NARF36 struct
-  output.points.resize(feature_list.size());
+  output.resize(feature_list.size());
   for (std::size_t i = 0; i < feature_list.size(); ++i)
   {
     feature_list[i]->copyToNarf36(output[i]);
diff --git a/features/src/range_image_border_extractor.cpp b/features/src/range_image_border_extractor.cpp
index 5e40a160..43a74969 100644
--- a/features/src/range_image_border_extractor.cpp
+++ b/features/src/range_image_border_extractor.cpp
@@ -671,14 +671,14 @@ RangeImageBorderExtractor::blurSurfaceChanges ()
 void
 RangeImageBorderExtractor::computeFeature (PointCloudOut& output)
 {
-  output.points.clear();
+  output.clear();
 
   if (indices_)
   {
     std::cerr << __PRETTY_FUNCTION__
               << ": Sorry, usage of indices for the extraction is not supported for range image border extraction (yet).\n\n";
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
@@ -688,7 +688,7 @@ RangeImageBorderExtractor::computeFeature (PointCloudOut& output)
               << ": RangeImage is not set. Sorry, the border extraction works on range images, not on normal point clouds."
               << " Use setRangeImage(...).\n\n";
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
   output = getBorderDescriptions ();
diff --git a/filters/CMakeLists.txt b/filters/CMakeLists.txt
index aac03b7a..bb91d7bb 100644
--- a/filters/CMakeLists.txt
+++ b/filters/CMakeLists.txt
@@ -58,7 +58,6 @@ set(incs
   "include/pcl/${SUBSYS_NAME}/extract_indices.h"
   "include/pcl/${SUBSYS_NAME}/filter.h"
   "include/pcl/${SUBSYS_NAME}/filter_indices.h"
-  "include/pcl/${SUBSYS_NAME}/experimental/functor_filter.h"
   "include/pcl/${SUBSYS_NAME}/passthrough.h"
   "include/pcl/${SUBSYS_NAME}/shadowpoints.h"
   "include/pcl/${SUBSYS_NAME}/project_inliers.h"
@@ -87,6 +86,9 @@ set(incs
   "include/pcl/${SUBSYS_NAME}/local_maximum.h"
   "include/pcl/${SUBSYS_NAME}/model_outlier_removal.h"
 )
+set(experimental_incs
+  "include/pcl/${SUBSYS_NAME}/experimental/functor_filter.h"
+)
 
 set(impl_incs
   "include/pcl/${SUBSYS_NAME}/impl/conditional_removal.hpp"
@@ -133,4 +135,5 @@ PCL_MAKE_PKGCONFIG(${LIB_NAME} COMPONENT ${SUBSYS_NAME} DESC ${SUBSYS_DESC} PCL_
 
 # Install include files
 PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs})
+PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/experimental" ${experimental_incs})
 PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs})
diff --git a/filters/include/pcl/filters/approximate_voxel_grid.h b/filters/include/pcl/filters/approximate_voxel_grid.h
index 1d9b4c8a..b58c4d8a 100644
--- a/filters/include/pcl/filters/approximate_voxel_grid.h
+++ b/filters/include/pcl/filters/approximate_voxel_grid.h
@@ -37,7 +37,6 @@
 
 #pragma once
 
-#include <pcl/filters/boost.h>
 #include <pcl/filters/filter.h>
 
 namespace pcl
diff --git a/filters/include/pcl/filters/bilateral.h b/filters/include/pcl/filters/bilateral.h
index ff9956a2..ad83c441 100644
--- a/filters/include/pcl/filters/bilateral.h
+++ b/filters/include/pcl/filters/bilateral.h
@@ -40,7 +40,7 @@
 #pragma once
 
 #include <pcl/filters/filter.h>
-#include <pcl/search/pcl_search.h>
+#include <pcl/search/search.h> // for Search
 
 namespace pcl
 {
@@ -89,7 +89,7 @@ namespace pcl
         * \return the intensity average at a given point index
         */
       double 
-      computePointWeight (const int pid, const std::vector<int> &indices, const std::vector<float> &distances);
+      computePointWeight (const int pid, const Indices &indices, const std::vector<float> &distances);
 
       /** \brief Set the half size of the Gaussian bilateral filter window.
         * \param[in] sigma_s the half size of the Gaussian bilateral filter window to use
diff --git a/filters/include/pcl/filters/boost.h b/filters/include/pcl/filters/boost.h
index 85b20ffb..273c1e67 100644
--- a/filters/include/pcl/filters/boost.h
+++ b/filters/include/pcl/filters/boost.h
@@ -43,6 +43,7 @@
 #ifdef __GNUC__
 #pragma GCC system_header 
 #endif
+PCL_DEPRECATED_HEADER(1, 15, "Please include the needed boost headers directly.")
 
 // Marking all Boost headers as system headers to remove warnings
 #include <boost/random.hpp>
diff --git a/filters/include/pcl/filters/box_clipper3D.h b/filters/include/pcl/filters/box_clipper3D.h
index baf4a6df..97c1cd12 100644
--- a/filters/include/pcl/filters/box_clipper3D.h
+++ b/filters/include/pcl/filters/box_clipper3D.h
@@ -105,7 +105,7 @@ namespace pcl
       clipPlanarPolygon3D (const std::vector<PointT, Eigen::aligned_allocator<PointT> >& polygon, std::vector<PointT, Eigen::aligned_allocator<PointT> >& clipped_polygon) const override;
 
       void
-      clipPointCloud3D (const pcl::PointCloud<PointT> &cloud_in, std::vector<int>& clipped, const std::vector<int>& indices = std::vector<int> ()) const override;
+      clipPointCloud3D (const pcl::PointCloud<PointT> &cloud_in, Indices& clipped, const Indices& indices = Indices ()) const override;
 
       Clipper3D<PointT>*
       clone () const override;
diff --git a/filters/include/pcl/filters/clipper3D.h b/filters/include/pcl/filters/clipper3D.h
index b741ff7e..5c55483a 100644
--- a/filters/include/pcl/filters/clipper3D.h
+++ b/filters/include/pcl/filters/clipper3D.h
@@ -103,7 +103,7 @@ namespace pcl
         * \return list of indices of remaining points after clipping.
         */
       virtual void
-      clipPointCloud3D (const pcl::PointCloud<PointT> &cloud_in, std::vector<int>& clipped, const std::vector<int>& indices = std::vector<int> ()) const = 0;
+      clipPointCloud3D (const pcl::PointCloud<PointT> &cloud_in, Indices& clipped, const Indices& indices = Indices ()) const = 0;
 
       /**
         * \brief polymorphic method to clone the underlying clipper with its parameters.
diff --git a/filters/include/pcl/filters/conditional_removal.h b/filters/include/pcl/filters/conditional_removal.h
index df7b58a5..3b925018 100644
--- a/filters/include/pcl/filters/conditional_removal.h
+++ b/filters/include/pcl/filters/conditional_removal.h
@@ -38,8 +38,7 @@
 #pragma once
 
 #include <pcl/memory.h>
-#include <pcl/pcl_macros.h>
-#include <pcl/common/eigen.h>
+#include <pcl/pcl_config.h> // for PCL_NO_PRECOMPILE
 #include <pcl/filters/filter.h>
 
 namespace pcl
diff --git a/filters/include/pcl/filters/convolution_3d.h b/filters/include/pcl/filters/convolution_3d.h
index b8ba2a71..056ecbf8 100644
--- a/filters/include/pcl/filters/convolution_3d.h
+++ b/filters/include/pcl/filters/convolution_3d.h
@@ -40,8 +40,6 @@
 #pragma once
 
 #include <pcl/pcl_base.h>
-#include <pcl/filters/boost.h>
-#include <pcl/search/pcl_search.h>
 
 namespace pcl
 {
@@ -77,7 +75,7 @@ namespace pcl
           * \return the convolved point
           */
         virtual PointOutT
-        operator() (const std::vector<int>& indices, const std::vector<float>& distances) = 0;
+        operator() (const Indices& indices, const std::vector<float>& distances) = 0;
 
         /** \brief Must call this method before doing any computation
           * \note make sure to override this with at least
@@ -153,7 +151,7 @@ namespace pcl
         bool initCompute ();
 
         virtual PointOutT
-        operator() (const std::vector<int>& indices, const std::vector<float>& distances);
+        operator() (const Indices& indices, const std::vector<float>& distances);
 
       protected:
         float sigma_;
@@ -187,7 +185,7 @@ namespace pcl
         ~GaussianKernelRGB () {}
 
         PointOutT
-        operator() (const std::vector<int>& indices, const std::vector<float>& distances);
+        operator() (const Indices& indices, const std::vector<float>& distances);
     };
 
     /** Convolution3D handles the non organized case where width and height are unknown or if you
diff --git a/filters/include/pcl/filters/covariance_sampling.h b/filters/include/pcl/filters/covariance_sampling.h
index 76231a24..d44b2e66 100644
--- a/filters/include/pcl/filters/covariance_sampling.h
+++ b/filters/include/pcl/filters/covariance_sampling.h
@@ -152,7 +152,7 @@ namespace pcl
         * \param[out] indices the resultant point cloud indices
         */
       void
-      applyFilter (std::vector<int> &indices) override;
+      applyFilter (Indices &indices) override;
 
       static bool
       sort_dot_list_function (std::pair<int, double> a,
diff --git a/filters/include/pcl/filters/crop_box.h b/filters/include/pcl/filters/crop_box.h
index 4beee5a9..d8d350fe 100644
--- a/filters/include/pcl/filters/crop_box.h
+++ b/filters/include/pcl/filters/crop_box.h
@@ -39,10 +39,7 @@
 
 #pragma once
 
-#include <pcl/point_types.h>
 #include <pcl/filters/filter_indices.h>
-#include <pcl/common/transforms.h>
-#include <pcl/common/eigen.h>
 
 namespace pcl
 {
@@ -178,7 +175,7 @@ namespace pcl
         * \param[out] indices the resultant point cloud indices
         */
       void
-      applyFilter (std::vector<int> &indices) override;
+      applyFilter (Indices &indices) override;
     private:
       /** \brief The minimum point of the box. */
       Eigen::Vector4f min_pt_;
@@ -319,7 +316,7 @@ namespace pcl
         * \param indices the resultant point cloud indices
         */
       void
-      applyFilter (std::vector<int> &indices) override;
+      applyFilter (Indices &indices) override;
 
       /** \brief The minimum point of the box. */
       Eigen::Vector4f min_pt_;
diff --git a/filters/include/pcl/filters/crop_hull.h b/filters/include/pcl/filters/crop_hull.h
index 62bf2590..26c90654 100644
--- a/filters/include/pcl/filters/crop_hull.h
+++ b/filters/include/pcl/filters/crop_hull.h
@@ -37,7 +37,6 @@
 
 #pragma once
 
-#include <pcl/point_types.h>
 #include <pcl/Vertices.h>
 #include <pcl/filters/filter_indices.h>
 
@@ -140,7 +139,7 @@ namespace pcl
         * \param[out] indices the indices of the set of points that passed the filter.
         */
       void        
-      applyFilter (std::vector<int> &indices) override;
+      applyFilter (Indices &indices) override;
 
     private:  
       /** \brief Return the size of the hull point cloud in line with coordinate axes.
@@ -167,7 +166,7 @@ namespace pcl
         *                     2D polygon filter.
         */
       template<unsigned PlaneDim1, unsigned PlaneDim2> void
-      applyFilter2D (std::vector<int> &indices);
+      applyFilter2D (Indices &indices);
 
        /** \brief Apply the three-dimensional hull filter.
          * Polygon-ray crossings are used for three rays cast from each point
@@ -189,7 +188,7 @@ namespace pcl
         *                     polygon hull filter.
         */
       void
-      applyFilter3D (std::vector<int> &indices);
+      applyFilter3D (Indices &indices);
 
       /** \brief Test an individual point against a 2D polygon.
         * PlaneDim1 and PlaneDim2 specify the x/y/z coordinate axes to use.
diff --git a/filters/include/pcl/filters/experimental/functor_filter.h b/filters/include/pcl/filters/experimental/functor_filter.h
index e29ea525..d03c1828 100644
--- a/filters/include/pcl/filters/experimental/functor_filter.h
+++ b/filters/include/pcl/filters/experimental/functor_filter.h
@@ -14,61 +14,70 @@
 
 namespace pcl {
 namespace experimental {
+/**
+ * \brief Checks if the function object meets the usage in `FunctorFilter` class
+ * \details `Function` needs to be callable with a const reference to a PointCloud
+ * and an index value. The return type should be implicitly convertible to a boolean
+ */
 template <typename PointT, typename Function>
-constexpr static bool is_functor_for_filter_v =
-    pcl::is_invocable_r_v<bool,
-                          Function,
-                          const pcl::remove_cvref_t<pcl::PointCloud<PointT>>&,
-                          pcl::index_t>;
+constexpr static bool is_function_object_for_filter_v =
+    is_invocable_r_v<bool, Function, const PointCloud<PointT>&, index_t>;
 
+namespace advanced {
 /**
- * \brief Filter point clouds and indices based on a functor passed in the ctor
+ * \brief Filter point clouds and indices based on a function object passed in the ctor
+ * \details The function object can be anything (lambda, std::function, invocable class,
+ * etc.) that can be moved into the class. Additionally, it must satisfy the condition
+ * `is_function_object_for_filter_v`
  * \ingroup filters
  */
-template <typename PointT, typename Functor>
+template <typename PointT, typename FunctionObject>
 class FunctorFilter : public FilterIndices<PointT> {
   using Base = FilterIndices<PointT>;
-  using PCLBase = pcl::PCLBase<PointT>;
+  using PCL_Base = PCLBase<PointT>;
 
 public:
-  using FunctorT = Functor;
+  using FunctionObjectT = FunctionObject;
   // using in type would complicate signature
-  static_assert(is_functor_for_filter_v<PointT, FunctorT>,
-                "Functor signature must be similar to `bool(const PointCloud<PointT>&, "
-                "index_t)`");
+  static_assert(is_function_object_for_filter_v<PointT, FunctionObjectT>,
+                "Function object signature must be similar to `bool(const "
+                "PointCloud<PointT>&, index_t)`");
 
 protected:
   using Base::extract_removed_indices_;
   using Base::filter_name_;
   using Base::negative_;
   using Base::removed_indices_;
-  using PCLBase::indices_;
-  using PCLBase::input_;
+  using PCL_Base::indices_;
+  using PCL_Base::input_;
 
-  // need to hold a value because functors can only be copy or move constructed
-  FunctorT functor_;
+private:
+  // need to hold a value because lambdas can only be copy or move constructed in C++14
+  FunctionObjectT functionObject_;
 
 public:
   /** \brief Constructor.
+   * \param[in] function_object Object of effective type `FilterFunction` in order to
+   * filter out the indices for which it returns false
    * \param[in] extract_removed_indices Set to true if you want to be able to
    * extract the indices of points being removed (default = false).
    */
-  FunctorFilter(FunctorT functor, bool extract_removed_indices = false)
-  : Base(extract_removed_indices), functor_(std::move(functor))
+  FunctorFilter(FunctionObjectT function_object, bool extract_removed_indices = false)
+  : Base(extract_removed_indices), functionObject_(std::move(function_object))
   {
     filter_name_ = "functor_filter";
   }
 
-  const FunctorT&
-  getFunctor() const noexcept
+  const FunctionObjectT&
+  getFunctionObject() const noexcept
   {
-    return functor_;
+    return functionObject_;
   }
 
-  FunctorT&
-  getFunctor() noexcept
+  FunctionObjectT&
+  getFunctionObject() noexcept
   {
-    return functor_;
+    return functionObject_;
   }
 
   /**
@@ -79,15 +88,15 @@ public:
   applyFilter(Indices& indices) override
   {
     indices.clear();
-    indices.reserve(input_->size());
+    indices.reserve(indices_->size());
     if (extract_removed_indices_) {
       removed_indices_->clear();
-      removed_indices_->reserve(input_->size());
+      removed_indices_->reserve(indices_->size());
     }
 
     for (const auto index : *indices_) {
-      // functor returns true for points that should be selected
-      if (negative_ != functor_(*input_, index)) {
+      // function object returns true for points that should be selected
+      if (negative_ != functionObject_(*input_, index)) {
         indices.push_back(index);
       }
       else if (extract_removed_indices_) {
@@ -95,6 +104,37 @@ public:
       }
     }
   }
+
+protected:
+  /**
+   * \brief ctor to be used by derived classes with member function as FilterFunction
+   * \param[in] extract_removed_indices Set to true if you want to be able to
+   * extract the indices of points being removed (default = false).
+   * \note The class would be ill-defined until `setFunctionObject` has been called
+   * Do not call any filter routine until then
+   */
+  FunctorFilter(bool extract_removed_indices = false) : Base(extract_removed_indices)
+  {
+    filter_name_ = "functor_filter";
+  }
+
+  /**
+   * \brief utility function for derived class
+   * \param[in] function_object Object of effective type `FilterFunction` in order to
+   * filter out the indices for which it returns false
+   */
+  void
+  setFunctionObject(FunctionObjectT function_object) const noexcept
+  {
+    functionObject_ = std::move(function_object);
+  }
 };
+} // namespace advanced
+
+template <class PointT>
+using FilterFunction = std::function<bool(const PointCloud<PointT>&, index_t)>;
+
+template <class PointT>
+using FunctionFilter = advanced::FunctorFilter<PointT, FilterFunction<PointT>>;
 } // namespace experimental
 } // namespace pcl
diff --git a/filters/include/pcl/filters/extract_indices.h b/filters/include/pcl/filters/extract_indices.h
index e046318f..218654d9 100644
--- a/filters/include/pcl/filters/extract_indices.h
+++ b/filters/include/pcl/filters/extract_indices.h
@@ -122,7 +122,7 @@ namespace pcl
         * \param[out] indices The resultant indices.
         */
       void
-      applyFilter (std::vector<int> &indices) override
+      applyFilter (Indices &indices) override
       {
         applyFilterIndices (indices);
       }
@@ -131,7 +131,7 @@ namespace pcl
         * \param[out] indices The resultant indices.
         */
       void
-      applyFilterIndices (std::vector<int> &indices);
+      applyFilterIndices (Indices &indices);
   };
 
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -191,7 +191,7 @@ namespace pcl
         * \param indices the resultant indices
         */
       void
-      applyFilter (std::vector<int> &indices) override;
+      applyFilter (Indices &indices) override;
   };
 }
 
diff --git a/filters/include/pcl/filters/filter.h b/filters/include/pcl/filters/filter.h
index 373dce10..4a0b44ff 100644
--- a/filters/include/pcl/filters/filter.h
+++ b/filters/include/pcl/filters/filter.h
@@ -40,10 +40,7 @@
 #pragma once
 
 #include <pcl/pcl_base.h>
-#include <pcl/common/io.h>
-#include <pcl/conversions.h>
-#include <pcl/filters/boost.h>
-#include <cfloat>
+#include <pcl/common/io.h> // for copyPointCloud
 #include <pcl/PointIndices.h>
 
 namespace pcl
@@ -59,7 +56,7 @@ namespace pcl
   template<typename PointT> void
   removeNaNFromPointCloud (const pcl::PointCloud<PointT> &cloud_in,
                            pcl::PointCloud<PointT> &cloud_out,
-                           std::vector<int> &index);
+                           Indices &index);
 
   /** \brief Removes points that have their normals invalid (i.e., equal to NaN)
     * \param[in] cloud_in the input point cloud
@@ -72,7 +69,7 @@ namespace pcl
   template<typename PointT> void
   removeNaNNormalsFromPointCloud (const pcl::PointCloud<PointT> &cloud_in,
                                   pcl::PointCloud<PointT> &cloud_out,
-                                  std::vector<int> &index);
+                                  Indices &index);
 
   ////////////////////////////////////////////////////////////////////////////////////////////
   /** \brief Filter represents the base filter class. All filters must inherit from this interface.
@@ -96,7 +93,7 @@ namespace pcl
         * separate list. Default: false.
         */
       Filter (bool extract_removed_indices = false) :
-        removed_indices_ (new std::vector<int>),
+        removed_indices_ (new Indices),
         extract_removed_indices_ (extract_removed_indices)
       {
       }
@@ -201,7 +198,7 @@ namespace pcl
         * separate list. Default: false.
         */
       Filter (bool extract_removed_indices = false) :
-        removed_indices_ (new std::vector<int>),
+        removed_indices_ (new Indices),
         extract_removed_indices_ (extract_removed_indices)
       {
       }
diff --git a/filters/include/pcl/filters/filter_indices.h b/filters/include/pcl/filters/filter_indices.h
index 2c1d13c4..a5bdfb75 100644
--- a/filters/include/pcl/filters/filter_indices.h
+++ b/filters/include/pcl/filters/filter_indices.h
@@ -58,12 +58,12 @@ namespace pcl
     * \ingroup filters
     */
   template<typename PointT> void
-  removeNaNFromPointCloud (const pcl::PointCloud<PointT> &cloud_in, std::vector<int> &index);
+  removeNaNFromPointCloud (const pcl::PointCloud<PointT> &cloud_in, Indices &index);
 
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   /** \brief @b FilterIndices represents the base class for filters that are about binary point removal.
     * <br>
-    * All derived classes have to implement the \a filter (PointCloud &output) and the \a filter (std::vector<int> &indices) methods.
+    * All derived classes have to implement the \a filter (PointCloud &output) and the \a filter (Indices &indices) methods.
     * Ideally they also make use of the \a negative_, \a keep_organized_ and \a extract_removed_indices_ systems.
     * The distinguishment between the \a negative_ and \a extract_removed_indices_ systems only makes sense if the class automatically
     * filters non-finite entries in the filtering methods (recommended).
@@ -98,7 +98,7 @@ namespace pcl
         * \param[out] indices the resultant filtered point cloud indices
         */
       void
-      filter (std::vector<int> &indices)
+      filter (Indices &indices)
       {
         if (!initCompute ())
           return;
@@ -175,7 +175,7 @@ namespace pcl
 
       /** \brief Abstract filter method for point cloud indices. */
       virtual void
-      applyFilter (std::vector<int> &indices) = 0;
+      applyFilter (Indices &indices) = 0;
 
       /** \brief Abstract filter method for point cloud. */
       void
@@ -185,7 +185,7 @@ namespace pcl
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   /** \brief @b FilterIndices represents the base class for filters that are about binary point removal.
     * <br>
-    * All derived classes have to implement the \a filter (PointCloud &output) and the \a filter (std::vector<int> &indices) methods.
+    * All derived classes have to implement the \a filter (PointCloud &output) and the \a filter (Indices &indices) methods.
     * Ideally they also make use of the \a negative_, \a keep_organized_ and \a extract_removed_indices_ systems.
     * The distinguishment between the \a negative_ and \a extract_removed_indices_ systems only makes sense if the class automatically
     * filters non-finite entries in the filtering methods (recommended).
@@ -215,7 +215,7 @@ namespace pcl
         * \param[out] indices the resultant filtered point cloud indices
         */
       void
-      filter (std::vector<int> &indices);
+      filter (Indices &indices);
 
       /** \brief Set whether the regular conditions for points filtering should apply, or the inverted conditions.
         * \param[in] negative false = normal filter behavior (default), true = inverted behavior.
@@ -278,7 +278,7 @@ namespace pcl
 
       /** \brief Abstract filter method for point cloud indices. */
       virtual void
-      applyFilter (std::vector<int> &indices) = 0;
+      applyFilter (Indices &indices) = 0;
 
       /** \brief Abstract filter method for point cloud. */
       void
diff --git a/filters/include/pcl/filters/frustum_culling.h b/filters/include/pcl/filters/frustum_culling.h
index 1e06be3b..aa07a138 100644
--- a/filters/include/pcl/filters/frustum_culling.h
+++ b/filters/include/pcl/filters/frustum_culling.h
@@ -38,11 +38,9 @@
 #pragma once
 
 #include <pcl/memory.h>
-#include <pcl/pcl_macros.h>
+#include <pcl/pcl_config.h> // for PCL_NO_PRECOMPILE
 #include <pcl/point_types.h>
 #include <pcl/filters/filter_indices.h>
-#include <pcl/common/transforms.h>
-#include <pcl/common/eigen.h>
 
 namespace pcl
 {
@@ -209,7 +207,7 @@ namespace pcl
         * \param[out] indices the resultant point cloud indices
         */
       void
-      applyFilter (std::vector<int> &indices) override;
+      applyFilter (Indices &indices) override;
 
     private:
 
diff --git a/filters/include/pcl/filters/grid_minimum.h b/filters/include/pcl/filters/grid_minimum.h
index 49c35654..6c039ce8 100644
--- a/filters/include/pcl/filters/grid_minimum.h
+++ b/filters/include/pcl/filters/grid_minimum.h
@@ -41,7 +41,6 @@
 
 #pragma once
 
-#include <pcl/filters/boost.h>
 #include <pcl/filters/filter.h>
 #include <pcl/filters/filter_indices.h>
 
@@ -114,7 +113,7 @@ namespace pcl
         * \param[out] indices The resultant indices.
         */
       void
-      applyFilter (std::vector<int> &indices) override
+      applyFilter (Indices &indices) override
       {
         applyFilterIndices (indices);
       }
@@ -123,7 +122,7 @@ namespace pcl
         * \param[out] indices The resultant indices.
         */
       void
-      applyFilterIndices (std::vector<int> &indices);
+      applyFilterIndices (Indices &indices);
 
   };
 }
diff --git a/filters/include/pcl/filters/impl/approximate_voxel_grid.hpp b/filters/include/pcl/filters/impl/approximate_voxel_grid.hpp
index 60ebb8df..e0ece6ef 100644
--- a/filters/include/pcl/filters/impl/approximate_voxel_grid.hpp
+++ b/filters/include/pcl/filters/impl/approximate_voxel_grid.hpp
@@ -40,6 +40,7 @@
 
 #include <pcl/common/io.h>
 #include <pcl/filters/approximate_voxel_grid.h>
+#include <boost/mpl/size.hpp> // for size
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
@@ -86,7 +87,7 @@ pcl::ApproximateVoxelGrid<PointT>::applyFilter (PointCloud &output)
   }
   Eigen::VectorXf scratch = Eigen::VectorXf::Zero (centroid_size);
 
-  output.points.resize (input_->size ());   // size output for worst case
+  output.resize (input_->size ());   // size output for worst case
   std::size_t op = 0;    // output pointer
   for (const auto& point: *input_)
   {
@@ -126,7 +127,7 @@ pcl::ApproximateVoxelGrid<PointT>::applyFilter (PointCloud &output)
     if (hhe->count)
       flush (output, op++, hhe, rgba_index, centroid_size);
   }
-  output.points.resize (op);
+  output.resize (op);
   output.width = output.size ();
   output.height       = 1;                    // downsampling breaks the organized structure
   output.is_dense     = false;                 // we filter out invalid points
diff --git a/filters/include/pcl/filters/impl/bilateral.hpp b/filters/include/pcl/filters/impl/bilateral.hpp
index ab1c17bd..2c707d38 100644
--- a/filters/include/pcl/filters/impl/bilateral.hpp
+++ b/filters/include/pcl/filters/impl/bilateral.hpp
@@ -41,11 +41,13 @@
 #define PCL_FILTERS_BILATERAL_IMPL_H_
 
 #include <pcl/filters/bilateral.h>
+#include <pcl/search/organized.h> // for OrganizedNeighbor
+#include <pcl/search/kdtree.h> // for KdTree
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> double
 pcl::BilateralFilter<PointT>::computePointWeight (const int pid, 
-                                                  const std::vector<int> &indices,
+                                                  const Indices &indices,
                                                   const std::vector<float> &distances)
 {
   double BF = 0, W = 0;
@@ -90,20 +92,20 @@ pcl::BilateralFilter<PointT>::applyFilter (PointCloud &output)
   }
   tree_->setInputCloud (input_);
 
-  std::vector<int> k_indices;
+  Indices k_indices;
   std::vector<float> k_distances;
 
   // Copy the input data into the output
   output = *input_;
 
   // For all the indices given (equal to the entire cloud if none given)
-  for (std::size_t i = 0; i < indices_->size (); ++i)
+  for (const auto& idx : (*indices_))
   {
     // Perform a radius search to find the nearest neighbors
-    tree_->radiusSearch ((*indices_)[i], sigma_s_ * 2, k_indices, k_distances);
+    tree_->radiusSearch (idx, sigma_s_ * 2, k_indices, k_distances);
 
     // Overwrite the intensity value with the computed average
-    output[(*indices_)[i]].intensity = static_cast<float> (computePointWeight ((*indices_)[i], k_indices, k_distances));
+    output[idx].intensity = static_cast<float> (computePointWeight (idx, k_indices, k_distances));
   }
 }
  
diff --git a/filters/include/pcl/filters/impl/box_clipper3D.hpp b/filters/include/pcl/filters/impl/box_clipper3D.hpp
index 2cb641c9..106d19ca 100644
--- a/filters/include/pcl/filters/impl/box_clipper3D.hpp
+++ b/filters/include/pcl/filters/impl/box_clipper3D.hpp
@@ -196,7 +196,7 @@ pcl::BoxClipper3D<PointT>::clipPlanarPolygon3D (std::vector<PointT, Eigen::align
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 // /ToDo: write fast version using eigen map and single matrix vector multiplication, that uses advantages of eigens SSE operations.
 template<typename PointT> void
-pcl::BoxClipper3D<PointT>::clipPointCloud3D (const pcl::PointCloud<PointT>& cloud_in, std::vector<int>& clipped, const std::vector<int>& indices) const
+pcl::BoxClipper3D<PointT>::clipPointCloud3D (const pcl::PointCloud<PointT>& cloud_in, Indices& clipped, const Indices& indices) const
 {
   clipped.clear ();
   if (indices.empty ())
@@ -208,7 +208,7 @@ pcl::BoxClipper3D<PointT>::clipPointCloud3D (const pcl::PointCloud<PointT>& clou
   }
   else
   {
-    for (const int &index : indices)
+    for (const auto &index : indices)
       if (clipPoint3D (cloud_in[index]))
         clipped.push_back (index);
   }
diff --git a/filters/include/pcl/filters/impl/conditional_removal.hpp b/filters/include/pcl/filters/impl/conditional_removal.hpp
index a60fe130..a7b08151 100644
--- a/filters/include/pcl/filters/impl/conditional_removal.hpp
+++ b/filters/include/pcl/filters/impl/conditional_removal.hpp
@@ -682,7 +682,7 @@ pcl::ConditionalRemoval<PointT>::applyFilter (PointCloud &output)
     output.width    = this->input_->width;
     output.is_dense = this->input_->is_dense;
   }
-  output.points.resize (input_->size ());
+  output.resize (input_->size ());
   removed_indices_->resize (input_->size ());
 
   int nr_removed_p = 0;
@@ -723,11 +723,11 @@ pcl::ConditionalRemoval<PointT>::applyFilter (PointCloud &output)
     }
 
     output.width = nr_p;
-    output.points.resize (nr_p);
+    output.resize (nr_p);
   }
   else
   {
-    std::vector<int> indices = *Filter<PointT>::indices_;
+    Indices indices = *Filter<PointT>::indices_;
     std::sort (indices.begin (), indices.end ());   //TODO: is this necessary or can we assume the indices to be sorted?
     bool removed_p = false;
     std::size_t ci = 0;
diff --git a/filters/include/pcl/filters/impl/convolution_3d.hpp b/filters/include/pcl/filters/impl/convolution_3d.hpp
index 3cd4ff4b..7cde0a5b 100644
--- a/filters/include/pcl/filters/impl/convolution_3d.hpp
+++ b/filters/include/pcl/filters/impl/convolution_3d.hpp
@@ -102,7 +102,7 @@ pcl::filters::GaussianKernel<PointInT, PointOutT>::initCompute ()
 
 ///////////////////////////////////////////////////////////////////////////////////////////////////
 template<typename PointInT, typename PointOutT> PointOutT
-pcl::filters::GaussianKernel<PointInT, PointOutT>::operator() (const std::vector<int>& indices,
+pcl::filters::GaussianKernel<PointInT, PointOutT>::operator() (const Indices& indices,
                                                                const std::vector<float>& distances)
 {
   using namespace pcl::common;
@@ -110,7 +110,7 @@ pcl::filters::GaussianKernel<PointInT, PointOutT>::operator() (const std::vector
   float total_weight = 0;
   std::vector<float>::const_iterator dist_it = distances.begin ();
 
-  for (std::vector<int>::const_iterator idx_it = indices.begin ();
+  for (Indices::const_iterator idx_it = indices.begin ();
        idx_it != indices.end ();
        ++idx_it, ++dist_it)
   {
@@ -131,7 +131,7 @@ pcl::filters::GaussianKernel<PointInT, PointOutT>::operator() (const std::vector
 
 ///////////////////////////////////////////////////////////////////////////////////////////////////////
 template<typename PointInT, typename PointOutT> PointOutT
-pcl::filters::GaussianKernelRGB<PointInT, PointOutT>::operator() (const std::vector<int>& indices, const std::vector<float>& distances)
+pcl::filters::GaussianKernelRGB<PointInT, PointOutT>::operator() (const Indices& indices, const std::vector<float>& distances)
 {
   using namespace pcl::common;
   PointOutT result;
@@ -139,7 +139,7 @@ pcl::filters::GaussianKernelRGB<PointInT, PointOutT>::operator() (const std::vec
   float r = 0, g = 0, b = 0;
   std::vector<float>::const_iterator dist_it = distances.begin ();
 
-  for (std::vector<int>::const_iterator idx_it = indices.begin ();
+  for (Indices::const_iterator idx_it = indices.begin ();
        idx_it != indices.end ();
        ++idx_it, ++dist_it)
   {
@@ -204,14 +204,14 @@ pcl::filters::Convolution3D<PointInT, PointOutT, KernelT>::initCompute ()
   // Do a fast check to see if the search parameters are well defined
   if (search_radius_ <= 0.0)
   {
-    PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] search radius (%f) must be > 0",
+    PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] search radius (%f) must be > 0\n",
                search_radius_);
     return (false);
   }
   // Make sure the provided kernel implements the required interface
   if (dynamic_cast<ConvolvingKernel<PointInT, PointOutT>* > (&kernel_) == 0)
   {
-    PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] init failed");
+    PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] init failed : ");
     PCL_ERROR ("kernel_ must implement ConvolvingKernel interface\n!");
     return (false);
   }
@@ -238,7 +238,7 @@ pcl::filters::Convolution3D<PointInT, PointOutT, KernelT>::convolve (PointCloudO
   output.width = surface_->width;
   output.height = surface_->height;
   output.is_dense = surface_->is_dense;
-  std::vector<int> nn_indices;
+  Indices nn_indices;
   std::vector<float> nn_distances;
 
 #pragma omp parallel for \
diff --git a/filters/include/pcl/filters/impl/covariance_sampling.hpp b/filters/include/pcl/filters/impl/covariance_sampling.hpp
index 04d5307e..9a0c62d7 100644
--- a/filters/include/pcl/filters/impl/covariance_sampling.hpp
+++ b/filters/include/pcl/filters/impl/covariance_sampling.hpp
@@ -41,9 +41,9 @@
 #ifndef PCL_FILTERS_IMPL_COVARIANCE_SAMPLING_H_
 #define PCL_FILTERS_IMPL_COVARIANCE_SAMPLING_H_
 
-#include <pcl/common/eigen.h>
 #include <pcl/filters/covariance_sampling.h>
 #include <list>
+#include <Eigen/Eigenvalues> // for SelfAdjointEigenSolver
 
 ///////////////////////////////////////////////////////////////////////////////
 template<typename PointT, typename PointNT> bool
@@ -127,7 +127,7 @@ pcl::CovarianceSampling<PointT, PointNT>::computeCovarianceMatrix (Eigen::Matrix
 
 ///////////////////////////////////////////////////////////////////////////////
 template<typename PointT, typename PointNT> void
-pcl::CovarianceSampling<PointT, PointNT>::applyFilter (std::vector<int> &sampled_indices)
+pcl::CovarianceSampling<PointT, PointNT>::applyFilter (Indices &sampled_indices)
 {
   Eigen::Matrix<double, 6, 6> c_mat;
   // Invokes initCompute()
@@ -202,7 +202,7 @@ pcl::CovarianceSampling<PointT, PointNT>::applyFilter (std::vector<int> &sampled
   }
 
   // Remap the sampled_indices to the input_ cloud
-  for (int &sampled_index : sampled_indices)
+  for (auto &sampled_index : sampled_indices)
     sampled_index = (*indices_)[candidate_indices[sampled_index]];
 }
 
@@ -211,7 +211,7 @@ pcl::CovarianceSampling<PointT, PointNT>::applyFilter (std::vector<int> &sampled
 template<typename PointT, typename PointNT> void
 pcl::CovarianceSampling<PointT, PointNT>::applyFilter (Cloud &output)
 {
-  std::vector<int> sampled_indices;
+  Indices sampled_indices;
   applyFilter (sampled_indices);
 
   output.resize (sampled_indices.size ());
diff --git a/filters/include/pcl/filters/impl/crop_box.hpp b/filters/include/pcl/filters/impl/crop_box.hpp
index 339c6d73..a9da3041 100644
--- a/filters/include/pcl/filters/impl/crop_box.hpp
+++ b/filters/include/pcl/filters/impl/crop_box.hpp
@@ -42,10 +42,13 @@
 #define PCL_FILTERS_IMPL_CROP_BOX_H_
 
 #include <pcl/filters/crop_box.h>
+#include <pcl/common/eigen.h> // for getTransformation
+#include <pcl/common/point_tests.h> // for isFinite
+#include <pcl/common/transforms.h> // for transformPoint
 
 ///////////////////////////////////////////////////////////////////////////////
 template<typename PointT> void
-pcl::CropBox<PointT>::applyFilter (std::vector<int> &indices)
+pcl::CropBox<PointT>::applyFilter (Indices &indices)
 {
   indices.resize (input_->size ());
   removed_indices_->resize (input_->size ());
@@ -99,13 +102,13 @@ pcl::CropBox<PointT>::applyFilter (std::vector<int> &indices)
       if (negative_)
         indices[indices_count++] = index;
       else if (extract_removed_indices_)
-        (*removed_indices_)[removed_indices_count++] = static_cast<int> (index);
+        (*removed_indices_)[removed_indices_count++] = index;
     }
     // If inside the cropbox
     else
     {
       if (negative_ && extract_removed_indices_)
-        (*removed_indices_)[removed_indices_count++] = static_cast<int> (index);
+        (*removed_indices_)[removed_indices_count++] = index;
       else if (!negative_) 
         indices[indices_count++] = index;
     }
diff --git a/filters/include/pcl/filters/impl/crop_hull.hpp b/filters/include/pcl/filters/impl/crop_hull.hpp
index 2a784c10..b63bc0dd 100644
--- a/filters/include/pcl/filters/impl/crop_hull.hpp
+++ b/filters/include/pcl/filters/impl/crop_hull.hpp
@@ -67,7 +67,7 @@ pcl::CropHull<PointT>::applyFilter (PointCloud &output)
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template<typename PointT> void
-pcl::CropHull<PointT>::applyFilter (std::vector<int> &indices)
+pcl::CropHull<PointT>::applyFilter (Indices &indices)
 {
   if (dim_ == 2)
   {
@@ -104,16 +104,16 @@ pcl::CropHull<PointT>::getHullCloudRange ()
     -std::numeric_limits<float>::max (),
     -std::numeric_limits<float>::max ()
   );
-  for (std::size_t index = 0; index < indices_->size (); index++)
+  for (pcl::Vertices const & poly : hull_polygons_)
   {
-    Eigen::Vector3f pt = (*input_)[(*indices_)[index]].getVector3fMap ();
-    for (int i = 0; i < 3; i++)
+    for (auto const & idx : poly.vertices)
     {
-      if (pt[i] < cloud_min[i]) cloud_min[i] = pt[i];
-      if (pt[i] > cloud_max[i]) cloud_max[i] = pt[i];
+      Eigen::Vector3f pt = (*hull_cloud_)[idx].getVector3fMap ();
+      cloud_min = cloud_min.cwiseMin(pt);
+      cloud_max = cloud_max.cwiseMax(pt);
     }
   }
-  
+
   return (cloud_max - cloud_min);
 }
 
@@ -148,7 +148,7 @@ pcl::CropHull<PointT>::applyFilter2D (PointCloud &output)
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template<typename PointT> template<unsigned PlaneDim1, unsigned PlaneDim2> void 
-pcl::CropHull<PointT>::applyFilter2D (std::vector<int> &indices)
+pcl::CropHull<PointT>::applyFilter2D (Indices &indices)
 {
   // see comments in (PointCloud& output) overload
   for (std::size_t index = 0; index < indices_->size (); index++)
@@ -208,7 +208,7 @@ pcl::CropHull<PointT>::applyFilter3D (PointCloud &output)
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template<typename PointT> void 
-pcl::CropHull<PointT>::applyFilter3D (std::vector<int> &indices)
+pcl::CropHull<PointT>::applyFilter3D (Indices &indices)
 {
   // see comments in applyFilter3D (PointCloud& output)
   for (std::size_t index = 0; index < indices_->size (); index++)
diff --git a/filters/include/pcl/filters/impl/extract_indices.hpp b/filters/include/pcl/filters/impl/extract_indices.hpp
index 8050053a..5def9d5f 100644
--- a/filters/include/pcl/filters/impl/extract_indices.hpp
+++ b/filters/include/pcl/filters/impl/extract_indices.hpp
@@ -41,12 +41,13 @@
 #define PCL_FILTERS_IMPL_EXTRACT_INDICES_HPP_
 
 #include <pcl/filters/extract_indices.h>
+#include <numeric> // for std::iota
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
 pcl::ExtractIndices<PointT>::filterDirectly (PointCloudPtr &cloud)
 {
-  std::vector<int> indices;
+  Indices indices;
   bool temp = extract_removed_indices_;
   extract_removed_indices_ = true;
   this->setInputCloud (cloud);
@@ -55,9 +56,9 @@ pcl::ExtractIndices<PointT>::filterDirectly (PointCloudPtr &cloud)
 
   std::vector<pcl::PCLPointField> fields;
   pcl::for_each_type<FieldList> (pcl::detail::FieldAdder<PointT> (fields));
-  for (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii)  // rii = removed indices iterator
+  for (const auto& rii : (*removed_indices_)) // rii = removed indices iterator
   {
-    std::size_t pt_index = (std::size_t) (*removed_indices_)[rii];
+    uindex_t pt_index = (uindex_t) rii;
     if (pt_index >= input_->size ())
     {
       PCL_ERROR ("[pcl::%s::filterDirectly] The index exceeds the size of the input. Do nothing.\n",
@@ -77,7 +78,7 @@ pcl::ExtractIndices<PointT>::filterDirectly (PointCloudPtr &cloud)
 template <typename PointT> void
 pcl::ExtractIndices<PointT>::applyFilter (PointCloud &output)
 {
-  std::vector<int> indices;
+  Indices indices;
   if (keep_organized_)
   {
     bool temp = extract_removed_indices_;
@@ -114,7 +115,7 @@ pcl::ExtractIndices<PointT>::applyFilter (PointCloud &output)
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
-pcl::ExtractIndices<PointT>::applyFilterIndices (std::vector<int> &indices)
+pcl::ExtractIndices<PointT>::applyFilterIndices (Indices &indices)
 {
   if (indices_->size () > input_->size ())
   {
@@ -131,12 +132,11 @@ pcl::ExtractIndices<PointT>::applyFilterIndices (std::vector<int> &indices)
     if (extract_removed_indices_)
     {
       // Set up the full indices set
-      std::vector<int> full_indices (input_->size ());
-      for (int fii = 0; fii < static_cast<int> (full_indices.size ()); ++fii)  // fii = full indices iterator
-        full_indices[fii] = fii;
+      Indices full_indices (input_->size ());
+      std::iota (full_indices.begin (), full_indices.end (), static_cast<index_t> (0));
 
       // Set up the sorted input indices
-      std::vector<int> sorted_input_indices = *indices_;
+      Indices sorted_input_indices = *indices_;
       std::sort (sorted_input_indices.begin (), sorted_input_indices.end ());
 
       // Store the difference in removed_indices
@@ -147,12 +147,11 @@ pcl::ExtractIndices<PointT>::applyFilterIndices (std::vector<int> &indices)
   else  // Inverted functionality
   {
     // Set up the full indices set
-    std::vector<int> full_indices (input_->size ());
-    for (int fii = 0; fii < static_cast<int> (full_indices.size ()); ++fii)  // fii = full indices iterator
-      full_indices[fii] = fii;
+    Indices full_indices (input_->size ());
+    std::iota (full_indices.begin (), full_indices.end (), static_cast<index_t> (0));
 
     // Set up the sorted input indices
-    std::vector<int> sorted_input_indices = *indices_;
+    Indices sorted_input_indices = *indices_;
     std::sort (sorted_input_indices.begin (), sorted_input_indices.end ());
 
     // Store the difference in indices
diff --git a/filters/include/pcl/filters/impl/fast_bilateral_omp.hpp b/filters/include/pcl/filters/impl/fast_bilateral_omp.hpp
index 66113b3b..c86983ac 100644
--- a/filters/include/pcl/filters/impl/fast_bilateral_omp.hpp
+++ b/filters/include/pcl/filters/impl/fast_bilateral_omp.hpp
@@ -42,7 +42,6 @@
 
 #include <pcl/filters/fast_bilateral_omp.h>
 #include <pcl/common/io.h>
-#include <cassert>
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
diff --git a/filters/include/pcl/filters/impl/filter.hpp b/filters/include/pcl/filters/impl/filter.hpp
index 958890df..9379ab65 100644
--- a/filters/include/pcl/filters/impl/filter.hpp
+++ b/filters/include/pcl/filters/impl/filter.hpp
@@ -37,7 +37,7 @@
 
 #pragma once
 
-#include <pcl/pcl_macros.h>
+#include <pcl/pcl_exports.h> // for PCL_EXPORTS
 #include <pcl/common/point_tests.h> // for pcl::isFinite
 #include <pcl/filters/filter.h>
 
@@ -45,13 +45,13 @@
 template <typename PointT> void
 pcl::removeNaNFromPointCloud (const pcl::PointCloud<PointT> &cloud_in,
                               pcl::PointCloud<PointT> &cloud_out,
-                              std::vector<int> &index)
+                              Indices &index)
 {
   // If the clouds are not the same, prepare the output
   if (&cloud_in != &cloud_out)
   {
     cloud_out.header = cloud_in.header;
-    cloud_out.points.resize (cloud_in.size ());
+    cloud_out.resize (cloud_in.size ());
     cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
     cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
   }
@@ -82,7 +82,7 @@ pcl::removeNaNFromPointCloud (const pcl::PointCloud<PointT> &cloud_in,
     if (j != cloud_in.size ())
     {
       // Resize to the correct size
-      cloud_out.points.resize (j);
+      cloud_out.resize (j);
       index.resize (j);
     }
 
@@ -98,13 +98,13 @@ pcl::removeNaNFromPointCloud (const pcl::PointCloud<PointT> &cloud_in,
 template <typename PointT> void
 pcl::removeNaNNormalsFromPointCloud (const pcl::PointCloud<PointT> &cloud_in,
                                      pcl::PointCloud<PointT> &cloud_out,
-                                     std::vector<int> &index)
+                                     Indices &index)
 {
   // If the clouds are not the same, prepare the output
   if (&cloud_in != &cloud_out)
   {
     cloud_out.header = cloud_in.header;
-    cloud_out.points.resize (cloud_in.size ());
+    cloud_out.resize (cloud_in.size ());
     cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
     cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
   }
@@ -130,7 +130,7 @@ pcl::removeNaNNormalsFromPointCloud (const pcl::PointCloud<PointT> &cloud_in,
   if (j != cloud_in.size ())
   {
     // Resize to the correct size
-    cloud_out.points.resize (j);
+    cloud_out.resize (j);
     index.resize (j);
   }
 
@@ -139,6 +139,6 @@ pcl::removeNaNNormalsFromPointCloud (const pcl::PointCloud<PointT> &cloud_in,
 }
 
 
-#define PCL_INSTANTIATE_removeNaNFromPointCloud(T) template PCL_EXPORTS void pcl::removeNaNFromPointCloud<T>(const pcl::PointCloud<T>&, pcl::PointCloud<T>&, std::vector<int>&);
-#define PCL_INSTANTIATE_removeNaNNormalsFromPointCloud(T) template PCL_EXPORTS void pcl::removeNaNNormalsFromPointCloud<T>(const pcl::PointCloud<T>&, pcl::PointCloud<T>&, std::vector<int>&);
+#define PCL_INSTANTIATE_removeNaNFromPointCloud(T) template PCL_EXPORTS void pcl::removeNaNFromPointCloud<T>(const pcl::PointCloud<T>&, pcl::PointCloud<T>&, Indices&);
+#define PCL_INSTANTIATE_removeNaNNormalsFromPointCloud(T) template PCL_EXPORTS void pcl::removeNaNNormalsFromPointCloud<T>(const pcl::PointCloud<T>&, pcl::PointCloud<T>&, Indices&);
 
diff --git a/filters/include/pcl/filters/impl/filter_indices.hpp b/filters/include/pcl/filters/impl/filter_indices.hpp
index cedc0681..d408ee8d 100644
--- a/filters/include/pcl/filters/impl/filter_indices.hpp
+++ b/filters/include/pcl/filters/impl/filter_indices.hpp
@@ -42,7 +42,7 @@
 
 template <typename PointT> void
 pcl::removeNaNFromPointCloud (const pcl::PointCloud<PointT> &cloud_in,
-                              std::vector<int> &index)
+                              Indices &index)
 {
   // Reserve enough space for the indices
   index.resize (cloud_in.size ());
@@ -76,7 +76,7 @@ pcl::removeNaNFromPointCloud (const pcl::PointCloud<PointT> &cloud_in,
 template<typename PointT> void
 pcl::FilterIndices<PointT>::applyFilter (PointCloud &output)
 {
-  std::vector<int> indices;
+  Indices indices;
   if (keep_organized_)
   {
     if (!extract_removed_indices_)
@@ -107,7 +107,7 @@ pcl::FilterIndices<PointT>::applyFilter (PointCloud &output)
 }
 
 
-#define PCL_INSTANTIATE_removeNanFromPointCloud(T) template PCL_EXPORTS void pcl::removeNaNFromPointCloud<T>(const pcl::PointCloud<T>&, std::vector<int>&);
+#define PCL_INSTANTIATE_removeNanFromPointCloud(T) template PCL_EXPORTS void pcl::removeNaNFromPointCloud<T>(const pcl::PointCloud<T>&, Indices&);
 #define PCL_INSTANTIATE_FilterIndices(T) template class PCL_EXPORTS  pcl::FilterIndices<T>;
 
 #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 223ed4bc..035048eb 100644
--- a/filters/include/pcl/filters/impl/frustum_culling.hpp
+++ b/filters/include/pcl/filters/impl/frustum_culling.hpp
@@ -43,7 +43,7 @@
 
 ///////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
-pcl::FrustumCulling<PointT>::applyFilter (std::vector<int> &indices)
+pcl::FrustumCulling<PointT>::applyFilter (Indices &indices)
 {
   Eigen::Vector4f pl_n; // near plane 
   Eigen::Vector4f pl_f; // far plane
diff --git a/filters/include/pcl/filters/impl/grid_minimum.hpp b/filters/include/pcl/filters/impl/grid_minimum.hpp
index ea28c7c6..7b0b1915 100644
--- a/filters/include/pcl/filters/impl/grid_minimum.hpp
+++ b/filters/include/pcl/filters/impl/grid_minimum.hpp
@@ -44,6 +44,7 @@
 
 #include <pcl/common/common.h>
 #include <pcl/common/io.h>
+#include <pcl/common/point_tests.h> // for isXYZFinite
 #include <pcl/filters/grid_minimum.h>
 
 struct point_index_idx
@@ -64,11 +65,11 @@ pcl::GridMinimum<PointT>::applyFilter (PointCloud &output)
   {
     PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
-  std::vector<int> indices;
+  Indices indices;
 
   output.is_dense = true;
   applyFilterIndices (indices);
@@ -77,7 +78,7 @@ pcl::GridMinimum<PointT>::applyFilter (PointCloud &output)
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
-pcl::GridMinimum<PointT>::applyFilterIndices (std::vector<int> &indices)
+pcl::GridMinimum<PointT>::applyFilterIndices (Indices &indices)
 {
   indices.resize (indices_->size ());
   int oii = 0;
@@ -92,7 +93,7 @@ pcl::GridMinimum<PointT>::applyFilterIndices (std::vector<int> &indices)
 
   if ((dx*dy) > static_cast<std::int64_t> (std::numeric_limits<std::int32_t>::max ()))
   {
-    PCL_WARN ("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName ().c_str ());
+    PCL_WARN ("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.\n", getClassName ().c_str ());
     return;
   }
 
@@ -117,21 +118,19 @@ pcl::GridMinimum<PointT>::applyFilterIndices (std::vector<int> &indices)
   // First pass: go over all points and insert them into the index_vector vector
   // with calculated idx. Points with the same idx value will contribute to the
   // same point of resulting CloudPoint
-  for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
+  for (const auto& index : (*indices_))
   {
     if (!input_->is_dense)
       // Check if the point is invalid
-      if (!std::isfinite ((*input_)[*it].x) ||
-          !std::isfinite ((*input_)[*it].y) ||
-          !std::isfinite ((*input_)[*it].z))
+      if (!isXYZFinite ((*input_)[index]))
         continue;
 
-    int ijk0 = static_cast<int> (std::floor ((*input_)[*it].x * inverse_resolution_) - static_cast<float> (min_b[0]));
-    int ijk1 = static_cast<int> (std::floor ((*input_)[*it].y * inverse_resolution_) - static_cast<float> (min_b[1]));
+    int ijk0 = static_cast<int> (std::floor ((*input_)[index].x * inverse_resolution_) - static_cast<float> (min_b[0]));
+    int ijk1 = static_cast<int> (std::floor ((*input_)[index].y * inverse_resolution_) - static_cast<float> (min_b[1]));
 
     // Compute the grid cell index
     int idx = ijk0 * divb_mul[0] + ijk1 * divb_mul[1];
-    index_vector.emplace_back(static_cast<unsigned int> (idx), *it);
+    index_vector.emplace_back(static_cast<unsigned int> (idx), index);
   }
   
   // Second pass: sort the index_vector vector using value representing target cell as index
diff --git a/filters/include/pcl/filters/impl/local_maximum.hpp b/filters/include/pcl/filters/impl/local_maximum.hpp
index 98199541..da5e960a 100644
--- a/filters/include/pcl/filters/impl/local_maximum.hpp
+++ b/filters/include/pcl/filters/impl/local_maximum.hpp
@@ -46,6 +46,8 @@
 #include <pcl/filters/local_maximum.h>
 #include <pcl/filters/project_inliers.h>
 #include <pcl/ModelCoefficients.h>
+#include <pcl/search/organized.h> // for OrganizedNeighbor
+#include <pcl/search/kdtree.h> // for KdTree
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
@@ -56,11 +58,11 @@ pcl::LocalMaximum<PointT>::applyFilter (PointCloud &output)
   {
     PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
-  std::vector<int> indices;
+  Indices indices;
 
   output.is_dense = true;
   applyFilterIndices (indices);
@@ -69,7 +71,7 @@ pcl::LocalMaximum<PointT>::applyFilter (PointCloud &output)
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
-pcl::LocalMaximum<PointT>::applyFilterIndices (std::vector<int> &indices)
+pcl::LocalMaximum<PointT>::applyFilterIndices (Indices &indices)
 {
   typename PointCloud::Ptr cloud_projected (new PointCloud);
 
@@ -108,28 +110,28 @@ pcl::LocalMaximum<PointT>::applyFilterIndices (std::vector<int> &indices)
   // Find all points within xy radius (i.e., a vertical cylinder) of the query
   // point, removing those that are locally maximal (i.e., highest z within the
   // cylinder)
-  for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)
+  for (const auto& iii : (*indices_))
   {
-    if (!isFinite ((*input_)[(*indices_)[iii]]))
+    if (!isFinite ((*input_)[iii]))
     {
       continue;
     }
 
     // Points in the neighborhood of a previously identified local max, will
     // not be maximal in their own neighborhood
-    if (point_is_visited[(*indices_)[iii]] && !point_is_max[(*indices_)[iii]])
+    if (point_is_visited[iii] && !point_is_max[iii])
     {
       continue;
     }
 
     // Assume the current query point is the maximum, mark as visited
-    point_is_max[(*indices_)[iii]] = true;
-    point_is_visited[(*indices_)[iii]] = true;
+    point_is_max[iii] = true;
+    point_is_visited[iii] = true;
 
     // Perform the radius search in the projected cloud
-    std::vector<int> radius_indices;
+    Indices radius_indices;
     std::vector<float> radius_dists;
-    PointT p = (*cloud_projected)[(*indices_)[iii]];
+    PointT p = (*cloud_projected)[iii];
     if (searcher_->radiusSearch (p, radius_, radius_indices, radius_dists) == 0)
     {
       PCL_WARN ("[pcl::%s::applyFilter] Searching for neighbors within radius %f failed.\n", getClassName ().c_str (), radius_);
@@ -139,24 +141,24 @@ pcl::LocalMaximum<PointT>::applyFilterIndices (std::vector<int> &indices)
     // If query point is alone, we retain it regardless
     if (radius_indices.size () == 1)
     {
-        point_is_max[(*indices_)[iii]] = false;
+        point_is_max[iii] = false;
     }
 
     // Check to see if a neighbor is higher than the query point
-    float query_z = (*input_)[(*indices_)[iii]].z;
+    float query_z = (*input_)[iii].z;
     for (std::size_t k = 1; k < radius_indices.size (); ++k)  // k = 1 is the first neighbor
     {
       if ((*input_)[radius_indices[k]].z > query_z)
       {
         // Query point is not the local max, no need to check others
-        point_is_max[(*indices_)[iii]] = false;
+        point_is_max[iii] = false;
         break;
       }
     }
 
     // If the query point was a local max, all neighbors can be marked as
     // visited, excluding them from future consideration as local maxima
-    if (point_is_max[(*indices_)[iii]])
+    if (point_is_max[iii])
     {
       for (std::size_t k = 1; k < radius_indices.size (); ++k)  // k = 1 is the first neighbor
       {
@@ -166,18 +168,18 @@ pcl::LocalMaximum<PointT>::applyFilterIndices (std::vector<int> &indices)
 
     // Points that are local maxima are passed to removed indices
     // Unless negative was set, then it's the opposite condition
-    if ((!negative_ && point_is_max[(*indices_)[iii]]) || (negative_ && !point_is_max[(*indices_)[iii]]))
+    if ((!negative_ && point_is_max[iii]) || (negative_ && !point_is_max[iii]))
     {
       if (extract_removed_indices_)
       {
-        (*removed_indices_)[rii++] = (*indices_)[iii];
+        (*removed_indices_)[rii++] = iii;
       }
 
       continue;
     }
 
     // Otherwise it was a normal point for output (inlier)
-    indices[oii++] = (*indices_)[iii];
+    indices[oii++] = iii;
   }
 
   // Resize the output arrays
diff --git a/filters/include/pcl/filters/impl/median_filter.hpp b/filters/include/pcl/filters/impl/median_filter.hpp
index a29ed545..8f72ab84 100644
--- a/filters/include/pcl/filters/impl/median_filter.hpp
+++ b/filters/include/pcl/filters/impl/median_filter.hpp
@@ -76,8 +76,9 @@ pcl::MedianFilter<PointT>::applyFilter (PointCloud &output)
           continue;
 
         // The output depth will be the median of all the depths in the window
-        partial_sort (vals.begin (), vals.begin () + vals.size () / 2 + 1, vals.end ());
-        float new_depth = vals[vals.size () / 2];
+        auto middle_it = vals.begin () + vals.size () / 2;
+        std::nth_element (vals.begin (), middle_it, vals.end ());
+        float new_depth = *middle_it;
         // Do not allow points to move more than the set max_allowed_movement_
         if (std::abs (new_depth - (*input_)(x, y).z) < max_allowed_movement_)
           output (x, y).z = new_depth;
diff --git a/filters/include/pcl/filters/impl/model_outlier_removal.hpp b/filters/include/pcl/filters/impl/model_outlier_removal.hpp
index 96003da5..dcded28e 100644
--- a/filters/include/pcl/filters/impl/model_outlier_removal.hpp
+++ b/filters/include/pcl/filters/impl/model_outlier_removal.hpp
@@ -142,7 +142,7 @@ pcl::ModelOutlierRemoval<PointT>::initSACModel (pcl::SacModel model_type)
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
-pcl::ModelOutlierRemoval<PointT>::applyFilterIndices (std::vector<int> &indices)
+pcl::ModelOutlierRemoval<PointT>::applyFilterIndices (Indices &indices)
 {
   //The arrays to be used
   indices.resize (indices_->size ());
@@ -174,16 +174,16 @@ pcl::ModelOutlierRemoval<PointT>::applyFilterIndices (std::vector<int> &indices)
   //if the filter setup is invalid filter for nan and return;
   if (!valid_setup)
   {
-    for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)  // iii = input indices iterator
+    for (const auto& iii : (*indices_)) // iii = input indices iterator
     {
       // Non-finite entries are always passed to removed indices
-      if (!isFinite ((*input_)[ (*indices_)[iii]]))
+      if (!isFinite ((*input_)[iii]))
       {
         if (extract_removed_indices_)
-          (*removed_indices_)[rii++] = (*indices_)[iii];
+          (*removed_indices_)[rii++] = iii;
         continue;
       }
-      indices[oii++] = (*indices_)[iii];
+      indices[oii++] = iii;
     }
     return;
   }
diff --git a/filters/include/pcl/filters/impl/morphological_filter.hpp b/filters/include/pcl/filters/impl/morphological_filter.hpp
index 8f79943f..5452bd80 100644
--- a/filters/include/pcl/filters/impl/morphological_filter.hpp
+++ b/filters/include/pcl/filters/impl/morphological_filter.hpp
@@ -43,7 +43,6 @@
 #define PCL_FILTERS_IMPL_MORPHOLOGICAL_FILTER_H_
 
 #include <limits>
-#include <vector>
 
 #include <Eigen/Core>
 
@@ -79,7 +78,7 @@ applyMorphologicalOperator (const typename pcl::PointCloud<PointT>::ConstPtr &cl
       for (std::size_t p_idx = 0; p_idx < cloud_in->size (); ++p_idx)
       {
         Eigen::Vector3f bbox_min, bbox_max;
-        std::vector<int> pt_indices;
+        Indices pt_indices;
         float minx = (*cloud_in)[p_idx].x - half_res;
         float miny = (*cloud_in)[p_idx].y - half_res;
         float minz = -std::numeric_limits<float>::max ();
@@ -122,7 +121,7 @@ applyMorphologicalOperator (const typename pcl::PointCloud<PointT>::ConstPtr &cl
       for (std::size_t p_idx = 0; p_idx < cloud_temp.size (); ++p_idx)
       {
         Eigen::Vector3f bbox_min, bbox_max;
-        std::vector<int> pt_indices;
+        Indices pt_indices;
         float minx = cloud_temp[p_idx].x - half_res;
         float miny = cloud_temp[p_idx].y - half_res;
         float minz = -std::numeric_limits<float>::max ();
@@ -159,7 +158,7 @@ applyMorphologicalOperator (const typename pcl::PointCloud<PointT>::ConstPtr &cl
       for (std::size_t p_idx = 0; p_idx < cloud_temp.size (); ++p_idx)
       {
         Eigen::Vector3f bbox_min, bbox_max;
-        std::vector<int> pt_indices;
+        Indices pt_indices;
         float minx = cloud_temp[p_idx].x - half_res;
         float miny = cloud_temp[p_idx].y - half_res;
         float minz = -std::numeric_limits<float>::max ();
diff --git a/filters/include/pcl/filters/impl/normal_space.hpp b/filters/include/pcl/filters/impl/normal_space.hpp
index 193adea4..29c1ccae 100644
--- a/filters/include/pcl/filters/impl/normal_space.hpp
+++ b/filters/include/pcl/filters/impl/normal_space.hpp
@@ -88,7 +88,7 @@ pcl::NormalSpaceSampling<PointT, NormalT>::findBin (const float *normal)
 
 ///////////////////////////////////////////////////////////////////////////////
 template<typename PointT, typename NormalT> void
-pcl::NormalSpaceSampling<PointT, NormalT>::applyFilter (std::vector<int> &indices)
+pcl::NormalSpaceSampling<PointT, NormalT>::applyFilter (Indices &indices)
 {
   if (!initCompute ())
   {
@@ -180,10 +180,10 @@ pcl::NormalSpaceSampling<PointT, NormalT>::applyFilter (std::vector<int> &indice
   // If we need to return the indices that we haven't sampled
   if (extract_removed_indices_)
   {
-    std::vector<int> indices_temp = indices;
+    Indices indices_temp = indices;
     std::sort (indices_temp.begin (), indices_temp.end ());
 
-    std::vector<int> all_indices_temp = *indices_;
+    Indices all_indices_temp = *indices_;
     std::sort (all_indices_temp.begin (), all_indices_temp.end ());
     set_difference (all_indices_temp.begin (), all_indices_temp.end (), 
                     indices_temp.begin (), indices_temp.end (), 
diff --git a/filters/include/pcl/filters/impl/passthrough.hpp b/filters/include/pcl/filters/impl/passthrough.hpp
index 78222f45..d630c843 100644
--- a/filters/include/pcl/filters/impl/passthrough.hpp
+++ b/filters/include/pcl/filters/impl/passthrough.hpp
@@ -44,7 +44,7 @@
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
-pcl::PassThrough<PointT>::applyFilterIndices (std::vector<int> &indices)
+pcl::PassThrough<PointT>::applyFilterIndices (Indices &indices)
 {
   // The arrays to be used
   indices.resize (indices_->size ());
diff --git a/filters/include/pcl/filters/impl/plane_clipper3D.hpp b/filters/include/pcl/filters/impl/plane_clipper3D.hpp
index 1ccf9607..39417f22 100644
--- a/filters/include/pcl/filters/impl/plane_clipper3D.hpp
+++ b/filters/include/pcl/filters/impl/plane_clipper3D.hpp
@@ -177,7 +177,7 @@ pcl::PlaneClipper3D<PointT>::clipPlanarPolygon3D (std::vector<PointT, Eigen::ali
 
 // /ToDo: write fast version using eigen map and single matrix vector multiplication, that uses advantages of eigens SSE operations.
 template<typename PointT> void
-pcl::PlaneClipper3D<PointT>::clipPointCloud3D (const pcl::PointCloud<PointT>& cloud_in, std::vector<int>& clipped, const std::vector<int>& indices) const
+pcl::PlaneClipper3D<PointT>::clipPointCloud3D (const pcl::PointCloud<PointT>& cloud_in, Indices& clipped, const Indices& indices) const
 {
   if (indices.empty ())
   {
@@ -219,9 +219,9 @@ pcl::PlaneClipper3D<PointT>::clipPointCloud3D (const pcl::PointCloud<PointT>& cl
   }
   else
   {
-    for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
-      if (clipPoint3D (cloud_in[*iIt]))
-        clipped.push_back (*iIt);
+    for (const auto& index : indices)
+      if (clipPoint3D (cloud_in[index]))
+        clipped.push_back (index);
   }
 }
 #endif //PCL_FILTERS_IMPL_PLANE_CLIPPER3D_HPP
diff --git a/filters/include/pcl/filters/impl/project_inliers.hpp b/filters/include/pcl/filters/impl/project_inliers.hpp
index 8b4386c5..4e6e12fc 100644
--- a/filters/include/pcl/filters/impl/project_inliers.hpp
+++ b/filters/include/pcl/filters/impl/project_inliers.hpp
@@ -39,6 +39,18 @@
 #define PCL_FILTERS_IMPL_PROJECT_INLIERS_H_
 
 #include <pcl/filters/project_inliers.h>
+#include <pcl/sample_consensus/sac_model_circle.h>
+#include <pcl/sample_consensus/sac_model_cylinder.h>
+#include <pcl/sample_consensus/sac_model_cone.h>
+#include <pcl/sample_consensus/sac_model_line.h>
+#include <pcl/sample_consensus/sac_model_normal_plane.h>
+#include <pcl/sample_consensus/sac_model_normal_sphere.h>
+#include <pcl/sample_consensus/sac_model_parallel_plane.h>
+#include <pcl/sample_consensus/sac_model_normal_parallel_plane.h>
+#include <pcl/sample_consensus/sac_model_parallel_line.h>
+#include <pcl/sample_consensus/sac_model_perpendicular_plane.h>
+#include <pcl/sample_consensus/sac_model_plane.h>
+#include <pcl/sample_consensus/sac_model_sphere.h>
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
@@ -48,7 +60,7 @@ pcl::ProjectInliers<PointT>::applyFilter (PointCloud &output)
   {
     PCL_WARN ("[pcl::%s::applyFilter] No indices given or empty indices!\n", getClassName ().c_str ());
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
@@ -63,7 +75,7 @@ pcl::ProjectInliers<PointT>::applyFilter (PointCloud &output)
   {
     PCL_ERROR ("[pcl::%s::applyFilter] Error initializing the SAC model!\n", getClassName ().c_str ());
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
   if (copy_all_data_)
diff --git a/filters/include/pcl/filters/impl/pyramid.hpp b/filters/include/pcl/filters/impl/pyramid.hpp
index 2ad5b449..c3c77fe8 100644
--- a/filters/include/pcl/filters/impl/pyramid.hpp
+++ b/filters/include/pcl/filters/impl/pyramid.hpp
@@ -53,13 +53,13 @@ Pyramid<PointT>::initCompute ()
 {
   if (!input_->isOrganized ())
   {
-    PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should be at least 2!", getClassName ().c_str ());
+    PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should be at least 2!\n", 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 ());
+    PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should be at least 2!\n", getClassName ().c_str ());
     return (false);
   }
 
@@ -69,7 +69,7 @@ Pyramid<PointT>::initCompute ()
 
   if (levels_ > 4)
   {
-    PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should not exceed 4!", getClassName ().c_str ());
+    PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should not exceed 4!\n", getClassName ().c_str ());
     return (false);
   }
 
diff --git a/filters/include/pcl/filters/impl/radius_outlier_removal.hpp b/filters/include/pcl/filters/impl/radius_outlier_removal.hpp
index ae1a27de..15926d8f 100644
--- a/filters/include/pcl/filters/impl/radius_outlier_removal.hpp
+++ b/filters/include/pcl/filters/impl/radius_outlier_removal.hpp
@@ -41,10 +41,12 @@
 #define PCL_FILTERS_IMPL_RADIUS_OUTLIER_REMOVAL_H_
 
 #include <pcl/filters/radius_outlier_removal.h>
+#include <pcl/search/organized.h> // for OrganizedNeighbor
+#include <pcl/search/kdtree.h> // for KdTree
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
-pcl::RadiusOutlierRemoval<PointT>::applyFilterIndices (std::vector<int> &indices)
+pcl::RadiusOutlierRemoval<PointT>::applyFilterIndices (Indices &indices)
 {
   if (search_radius_ == 0.0)
   {
@@ -65,7 +67,7 @@ pcl::RadiusOutlierRemoval<PointT>::applyFilterIndices (std::vector<int> &indices
   searcher_->setInputCloud (input_);
 
   // The arrays to be used
-  std::vector<int> nn_indices (indices_->size ());
+  Indices nn_indices (indices_->size ());
   std::vector<float> nn_dists (indices_->size ());
   indices.resize (indices_->size ());
   removed_indices_->resize (indices_->size ());
@@ -78,10 +80,10 @@ pcl::RadiusOutlierRemoval<PointT>::applyFilterIndices (std::vector<int> &indices
     int mean_k = min_pts_radius_ + 1;
     double nn_dists_max = search_radius_ * search_radius_;
 
-    for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
+    for (const auto& index : (*indices_))
     {
       // Perform the nearest-k search
-      int k = searcher_->nearestKSearch (*it, mean_k, nn_indices, nn_dists);
+      int k = searcher_->nearestKSearch (index, mean_k, nn_indices, nn_dists);
 
       // Check the number of neighbors
       // Note: nn_dists is sorted, so check the last item
@@ -118,34 +120,34 @@ pcl::RadiusOutlierRemoval<PointT>::applyFilterIndices (std::vector<int> &indices
       if (!chk_neighbors)
       {
         if (extract_removed_indices_)
-          (*removed_indices_)[rii++] = *it;
+          (*removed_indices_)[rii++] = index;
         continue;
       }
 
       // Otherwise it was a normal point for output (inlier)
-      indices[oii++] = *it;
+      indices[oii++] = index;
     }
   }
   // NaN or Inf values could exist => use radius search
   else
   {
-    for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
+    for (const auto& index : (*indices_))
     {
       // Perform the radius search
       // Note: k includes the query point, so is always at least 1
-      int k = searcher_->radiusSearch (*it, search_radius_, nn_indices, nn_dists);
+      int k = searcher_->radiusSearch (index, search_radius_, nn_indices, nn_dists);
 
       // Points having too few neighbors are outliers and are passed to removed indices
       // Unless negative was set, then it's the opposite condition
       if ((!negative_ && k <= min_pts_radius_) || (negative_ && k > min_pts_radius_))
       {
         if (extract_removed_indices_)
-          (*removed_indices_)[rii++] = *it;
+          (*removed_indices_)[rii++] = index;
         continue;
       }
 
       // Otherwise it was a normal point for output (inlier)
-      indices[oii++] = *it;
+      indices[oii++] = index;
     }
   }
 
diff --git a/filters/include/pcl/filters/impl/random_sample.hpp b/filters/include/pcl/filters/impl/random_sample.hpp
index 7738c33a..24a0c571 100644
--- a/filters/include/pcl/filters/impl/random_sample.hpp
+++ b/filters/include/pcl/filters/impl/random_sample.hpp
@@ -39,13 +39,12 @@
 #define PCL_FILTERS_IMPL_RANDOM_SAMPLE_H_
 
 #include <pcl/filters/random_sample.h>
-#include <pcl/type_traits.h>
 
 
 ///////////////////////////////////////////////////////////////////////////////
 template<typename PointT>
 void
-pcl::RandomSample<PointT>::applyFilter (std::vector<int> &indices)
+pcl::RandomSample<PointT>::applyFilter (Indices &indices)
 {
   std::size_t N = indices_->size ();  
   std::size_t sample_size = negative_ ? N - sample_ : sample_;
diff --git a/filters/include/pcl/filters/impl/sampling_surface_normal.hpp b/filters/include/pcl/filters/impl/sampling_surface_normal.hpp
index 4fc00d42..fd3f221e 100644
--- a/filters/include/pcl/filters/impl/sampling_surface_normal.hpp
+++ b/filters/include/pcl/filters/impl/sampling_surface_normal.hpp
@@ -48,7 +48,7 @@
 template<typename PointT> void
 pcl::SamplingSurfaceNormal<PointT>::applyFilter (PointCloud &output)
 {
-  std::vector <int> indices;
+  Indices indices;
   std::size_t npts = input_->size ();
   for (std::size_t i = 0; i < npts; i++)
     indices.push_back (i);
@@ -86,7 +86,7 @@ template<typename PointT> void
 pcl::SamplingSurfaceNormal<PointT>::partition (
     const PointCloud& cloud, const int first, const int last,
     const Vector min_values, const Vector max_values, 
-    std::vector<int>& indices, PointCloud&  output)
+    Indices& indices, PointCloud& output)
 {
 	const int count (last - first);
   if (count <= static_cast<int> (sample_))
@@ -124,7 +124,7 @@ pcl::SamplingSurfaceNormal<PointT>::partition (
 template<typename PointT> void 
 pcl::SamplingSurfaceNormal<PointT>::samplePartition (
     const PointCloud& data, const int first, const int last,
-    std::vector <int>& indices, PointCloud& output)
+    Indices& indices, PointCloud& output)
 {
   pcl::PointCloud <PointT> cloud;
   
diff --git a/filters/include/pcl/filters/impl/shadowpoints.hpp b/filters/include/pcl/filters/impl/shadowpoints.hpp
index 5608406b..eedd66fc 100644
--- a/filters/include/pcl/filters/impl/shadowpoints.hpp
+++ b/filters/include/pcl/filters/impl/shadowpoints.hpp
@@ -47,7 +47,7 @@ template<typename PointT, typename NormalT> void
 pcl::ShadowPoints<PointT, NormalT>::applyFilter (PointCloud &output)
 {
   assert (input_normals_ != NULL);
-  output.points.resize (input_->size ());
+  output.resize (input_->size ());
   if (extract_removed_indices_)
     removed_indices_->resize (input_->size ());
 
@@ -73,7 +73,7 @@ pcl::ShadowPoints<PointT, NormalT>::applyFilter (PointCloud &output)
 
     }  
   }
-  output.points.resize (cp);
+  output.resize (cp);
   removed_indices_->resize (ri);
   output.height = 1;
   output.width = output.size ();
@@ -81,7 +81,7 @@ pcl::ShadowPoints<PointT, NormalT>::applyFilter (PointCloud &output)
 
 ///////////////////////////////////////////////////////////////////////////////
 template<typename PointT, typename NormalT> void
-pcl::ShadowPoints<PointT, NormalT>::applyFilter (std::vector<int> &indices)
+pcl::ShadowPoints<PointT, NormalT>::applyFilter (Indices &indices)
 {
   assert (input_normals_ != NULL);
   indices.resize (input_->size ());
@@ -90,17 +90,17 @@ pcl::ShadowPoints<PointT, NormalT>::applyFilter (std::vector<int> &indices)
 
   unsigned int k = 0;
   unsigned int z = 0;
-  for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
+  for (const auto& idx : (*indices_))
   {
-    const NormalT &normal = (*input_normals_)[*idx];
-    const PointT &pt = (*input_)[*idx];
+    const NormalT &normal = (*input_normals_)[idx];
+    const PointT &pt = (*input_)[idx];
     
     float val = std::abs (normal.normal_x * pt.x + normal.normal_y * pt.y + normal.normal_z * pt.z);
 
     if ( (val >= threshold_) ^ negative_)
-      indices[k++] = *idx;
+      indices[k++] = idx;
     else if (extract_removed_indices_)
-      (*removed_indices_)[z++] = *idx;
+      (*removed_indices_)[z++] = idx;
   }
   indices.resize (k);
   removed_indices_->resize (z);
diff --git a/filters/include/pcl/filters/impl/statistical_outlier_removal.hpp b/filters/include/pcl/filters/impl/statistical_outlier_removal.hpp
index 25323018..c1178633 100644
--- a/filters/include/pcl/filters/impl/statistical_outlier_removal.hpp
+++ b/filters/include/pcl/filters/impl/statistical_outlier_removal.hpp
@@ -41,10 +41,12 @@
 #define PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_
 
 #include <pcl/filters/statistical_outlier_removal.h>
+#include <pcl/search/organized.h> // for OrganizedNeighbor
+#include <pcl/search/kdtree.h> // for KdTree
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
-pcl::StatisticalOutlierRemoval<PointT>::applyFilterIndices (std::vector<int> &indices)
+pcl::StatisticalOutlierRemoval<PointT>::applyFilterIndices (Indices &indices)
 {
   // Initialize the search class
   if (!searcher_)
@@ -57,7 +59,7 @@ pcl::StatisticalOutlierRemoval<PointT>::applyFilterIndices (std::vector<int> &in
   searcher_->setInputCloud (input_);
 
   // The arrays to be used
-  std::vector<int> nn_indices (mean_k_);
+  Indices nn_indices (mean_k_);
   std::vector<float> nn_dists (mean_k_);
   std::vector<float> distances (indices_->size ());
   indices.resize (indices_->size ());
diff --git a/filters/include/pcl/filters/impl/voxel_grid.hpp b/filters/include/pcl/filters/impl/voxel_grid.hpp
index 601251e7..d23c9e6b 100644
--- a/filters/include/pcl/filters/impl/voxel_grid.hpp
+++ b/filters/include/pcl/filters/impl/voxel_grid.hpp
@@ -123,7 +123,7 @@ pcl::getMinMax3D (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
 ///////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
 pcl::getMinMax3D (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
-                  const std::vector<int> &indices,
+                  const Indices &indices,
                   const std::string &distance_field_name, float min_distance, float max_distance,
                   Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative)
 {
@@ -139,7 +139,7 @@ pcl::getMinMax3D (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
   // If dense, no need to check for NaNs
   if (cloud->is_dense)
   {
-    for (const int &index : indices)
+    for (const auto &index : indices)
     {
       // Get the distance value
       const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&(*cloud)[index]);
@@ -165,7 +165,7 @@ pcl::getMinMax3D (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
   }
   else
   {
-    for (const int &index : indices)
+    for (const auto &index : indices)
     {
       // Get the distance value
       const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&(*cloud)[index]);
@@ -218,7 +218,7 @@ pcl::VoxelGrid<PointT>::applyFilter (PointCloud &output)
   {
     PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
@@ -240,7 +240,7 @@ pcl::VoxelGrid<PointT>::applyFilter (PointCloud &output)
 
   if ((dx*dy*dz) > static_cast<std::int64_t>(std::numeric_limits<std::int32_t>::max()))
   {
-    PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName().c_str());
+    PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.\n", getClassName().c_str());
     output = *input_;
     return;
   }
@@ -276,17 +276,15 @@ pcl::VoxelGrid<PointT>::applyFilter (PointCloud &output)
     // First pass: go over all points and insert them into the index_vector vector
     // with calculated idx. Points with the same idx value will contribute to the
     // same point of resulting CloudPoint
-    for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
+    for (const auto& index : (*indices_))
     {
       if (!input_->is_dense)
         // Check if the point is invalid
-        if (!std::isfinite ((*input_)[*it].x) || 
-            !std::isfinite ((*input_)[*it].y) || 
-            !std::isfinite ((*input_)[*it].z))
+        if (!isXYZFinite ((*input_)[index]))
           continue;
 
       // Get the distance value
-      const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&(*input_)[*it]);
+      const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&(*input_)[index]);
       float distance_value = 0;
       memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
 
@@ -303,13 +301,13 @@ pcl::VoxelGrid<PointT>::applyFilter (PointCloud &output)
           continue;
       }
       
-      int ijk0 = static_cast<int> (std::floor ((*input_)[*it].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
-      int ijk1 = static_cast<int> (std::floor ((*input_)[*it].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
-      int ijk2 = static_cast<int> (std::floor ((*input_)[*it].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
+      int ijk0 = static_cast<int> (std::floor ((*input_)[index].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
+      int ijk1 = static_cast<int> (std::floor ((*input_)[index].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
+      int ijk2 = static_cast<int> (std::floor ((*input_)[index].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
 
       // Compute the centroid leaf index
       int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
-      index_vector.emplace_back(static_cast<unsigned int> (idx), *it);
+      index_vector.emplace_back(static_cast<unsigned int> (idx), index);
     }
   }
   // No distance filtering, process all data
@@ -318,22 +316,20 @@ pcl::VoxelGrid<PointT>::applyFilter (PointCloud &output)
     // First pass: go over all points and insert them into the index_vector vector
     // with calculated idx. Points with the same idx value will contribute to the
     // same point of resulting CloudPoint
-    for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
+    for (const auto& index : (*indices_))
     {
       if (!input_->is_dense)
         // Check if the point is invalid
-        if (!std::isfinite ((*input_)[*it].x) || 
-            !std::isfinite ((*input_)[*it].y) || 
-            !std::isfinite ((*input_)[*it].z))
+        if (!isXYZFinite ((*input_)[index]))
           continue;
 
-      int ijk0 = static_cast<int> (std::floor ((*input_)[*it].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
-      int ijk1 = static_cast<int> (std::floor ((*input_)[*it].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
-      int ijk2 = static_cast<int> (std::floor ((*input_)[*it].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
+      int ijk0 = static_cast<int> (std::floor ((*input_)[index].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
+      int ijk1 = static_cast<int> (std::floor ((*input_)[index].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
+      int ijk2 = static_cast<int> (std::floor ((*input_)[index].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
 
       // Compute the centroid leaf index
       int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
-      index_vector.emplace_back(static_cast<unsigned int> (idx), *it);
+      index_vector.emplace_back(static_cast<unsigned int> (idx), index);
     }
   }
 
@@ -366,7 +362,7 @@ pcl::VoxelGrid<PointT>::applyFilter (PointCloud &output)
   }
 
   // Fourth pass: compute centroids, insert them into their final position
-  output.points.resize (total);
+  output.resize (total);
   if (save_leaf_layout_)
   {
     try
diff --git a/filters/include/pcl/filters/impl/voxel_grid_covariance.hpp b/filters/include/pcl/filters/impl/voxel_grid_covariance.hpp
index f6822266..8b5a30df 100644
--- a/filters/include/pcl/filters/impl/voxel_grid_covariance.hpp
+++ b/filters/include/pcl/filters/impl/voxel_grid_covariance.hpp
@@ -39,10 +39,14 @@
 #define PCL_VOXEL_GRID_COVARIANCE_IMPL_H_
 
 #include <pcl/common/common.h>
-#include <pcl/filters/boost.h>
+#include <pcl/common/point_tests.h> // for isXYZFinite
 #include <pcl/filters/voxel_grid_covariance.h>
-#include <Eigen/Dense>
 #include <Eigen/Cholesky>
+#include <Eigen/Eigenvalues> // for SelfAdjointEigenSolver
+#include <boost/mpl/size.hpp> // for size
+#include <boost/random/mersenne_twister.hpp> // for mt19937
+#include <boost/random/normal_distribution.hpp> // for normal_distribution
+#include <boost/random/variate_generator.hpp> // for variate_generator
 
 //////////////////////////////////////////////////////////////////////////////////////////
 template<typename PointT> void
@@ -55,14 +59,14 @@ pcl::VoxelGridCovariance<PointT>::applyFilter (PointCloud &output)
   {
     PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
   // Copy the header (and thus the frame_id) + allocate enough space for points
   output.height = 1;                          // downsampling breaks the organized structure
   output.is_dense = true;                     // we filter out invalid points
-  output.points.clear ();
+  output.clear ();
 
   Eigen::Vector4f min_p, max_p;
   // Get the minimum and maximum dimensions
@@ -78,7 +82,7 @@ pcl::VoxelGridCovariance<PointT>::applyFilter (PointCloud &output)
 
   if((dx*dy*dz) > std::numeric_limits<std::int32_t>::max())
   {
-    PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName().c_str());
+    PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.\n", getClassName().c_str());
     output.clear();
     return;
   }
@@ -256,7 +260,7 @@ pcl::VoxelGridCovariance<PointT>::applyFilter (PointCloud &output)
   }
 
   // Second pass: go over all leaves and compute centroids and covariance matrices
-  output.points.reserve (leaves_.size ());
+  output.reserve (leaves_.size ());
   if (searchable_)
     voxel_centroids_leaf_indices_.reserve (leaves_.size ());
   int cp = 0;
@@ -296,9 +300,9 @@ pcl::VoxelGridCovariance<PointT>::applyFilter (PointCloud &output)
       // Do we need to process all the fields?
       if (!downsample_all_data_)
       {
-        output.points.back ().x = leaf.centroid[0];
-        output.points.back ().y = leaf.centroid[1];
-        output.points.back ().z = leaf.centroid[2];
+        output.back ().x = leaf.centroid[0];
+        output.back ().y = leaf.centroid[1];
+        output.back ().z = leaf.centroid[2];
       }
       else
       {
@@ -306,7 +310,7 @@ pcl::VoxelGridCovariance<PointT>::applyFilter (PointCloud &output)
         // ---[ RGB special case
         if (rgba_index >= 0)
         {
-          pcl::RGB& rgb = *reinterpret_cast<RGB*> (reinterpret_cast<char*> (&output.points.back ()) + rgba_index);
+          pcl::RGB& rgb = *reinterpret_cast<RGB*> (reinterpret_cast<char*> (&output.back ()) + rgba_index);
           rgb.a = leaf.centroid[centroid_size - 4];
           rgb.r = leaf.centroid[centroid_size - 3];
           rgb.g = leaf.centroid[centroid_size - 2];
@@ -319,8 +323,7 @@ pcl::VoxelGridCovariance<PointT>::applyFilter (PointCloud &output)
         voxel_centroids_leaf_indices_.push_back (static_cast<int> (it->first));
 
       // Single pass covariance calculation
-      leaf.cov_ = (leaf.cov_ - 2 * (pt_sum * leaf.mean_.transpose ())) / leaf.nr_points + leaf.mean_ * leaf.mean_.transpose ();
-      leaf.cov_ *= (leaf.nr_points - 1.0) / leaf.nr_points;
+      leaf.cov_ = (leaf.cov_ - pt_sum * leaf.mean_.transpose()) / (leaf.nr_points - 1.0);
 
       //Normalize Eigen Val such that max no more than 100x min.
       eigensolver.compute (leaf.cov_);
@@ -444,7 +447,7 @@ pcl::VoxelGridCovariance<PointT>::getDisplayCloud (pcl::PointCloud<PointXYZ>& ce
 
   int pnt_per_cell = 1000;
   boost::mt19937 rng;
-  boost::normal_distribution<> nd (0.0, leaf_size_.head (3).norm ());
+  boost::normal_distribution<> nd (0.0, 1.0);
   boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd);
 
   Eigen::LLT<Eigen::Matrix3d> llt_of_cov;
diff --git a/filters/include/pcl/filters/impl/voxel_grid_occlusion_estimation.hpp b/filters/include/pcl/filters/impl/voxel_grid_occlusion_estimation.hpp
index 689f592c..5a6d3974 100644
--- a/filters/include/pcl/filters/impl/voxel_grid_occlusion_estimation.hpp
+++ b/filters/include/pcl/filters/impl/voxel_grid_occlusion_estimation.hpp
@@ -39,7 +39,6 @@
 #ifndef PCL_FILTERS_IMPL_VOXEL_GRID_OCCLUSION_ESTIMATION_H_
 #define PCL_FILTERS_IMPL_VOXEL_GRID_OCCLUSION_ESTIMATION_H_
 
-#include <pcl/common/common.h>
 #include <pcl/filters/voxel_grid_occlusion_estimation.h>
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/filters/include/pcl/filters/local_maximum.h b/filters/include/pcl/filters/local_maximum.h
index 75e87308..741bf3b8 100644
--- a/filters/include/pcl/filters/local_maximum.h
+++ b/filters/include/pcl/filters/local_maximum.h
@@ -42,7 +42,7 @@
 #pragma once
 
 #include <pcl/filters/filter_indices.h>
-#include <pcl/search/pcl_search.h>
+#include <pcl/search/search.h> // for Search
 
 namespace pcl
 {
@@ -104,7 +104,7 @@ namespace pcl
         * \param[out] indices The resultant indices.
         */
       void
-      applyFilter (std::vector<int> &indices) override
+      applyFilter (Indices &indices) override
       {
         applyFilterIndices (indices);
       }
@@ -113,7 +113,7 @@ namespace pcl
         * \param[out] indices The resultant indices.
         */
       void
-      applyFilterIndices (std::vector<int> &indices);
+      applyFilterIndices (Indices &indices);
 
     private:
       /** \brief A pointer to the spatial search object. */
diff --git a/filters/include/pcl/filters/model_outlier_removal.h b/filters/include/pcl/filters/model_outlier_removal.h
index edba6ab3..56a3d093 100644
--- a/filters/include/pcl/filters/model_outlier_removal.h
+++ b/filters/include/pcl/filters/model_outlier_removal.h
@@ -203,7 +203,7 @@ namespace pcl
        * \param[out] indices The resultant indices.
        */
       void
-      applyFilter (std::vector<int> &indices) override
+      applyFilter (Indices &indices) override
       {
         applyFilterIndices (indices);
       }
@@ -212,7 +212,7 @@ namespace pcl
        * \param[out] indices The resultant indices.
        */
       void
-      applyFilterIndices (std::vector<int> &indices);
+      applyFilterIndices (Indices &indices);
 
     protected:
       double normals_distance_weight_;
diff --git a/filters/include/pcl/filters/morphological_filter.h b/filters/include/pcl/filters/morphological_filter.h
index eb18f28a..28ed5a03 100644
--- a/filters/include/pcl/filters/morphological_filter.h
+++ b/filters/include/pcl/filters/morphological_filter.h
@@ -41,11 +41,8 @@
 
 #pragma once
 
-#include <string>
-#include <pcl/pcl_base.h>
-#include <pcl/PointIndices.h>
-#include <pcl/conversions.h>
-#include <locale>
+#include <pcl/point_cloud.h> // for PointCloud
+#include <pcl/pcl_exports.h> // for PCL_EXPORTS
 
 namespace pcl
 {
diff --git a/filters/include/pcl/filters/normal_refinement.h b/filters/include/pcl/filters/normal_refinement.h
index 9c30f171..144ec0ad 100644
--- a/filters/include/pcl/filters/normal_refinement.h
+++ b/filters/include/pcl/filters/normal_refinement.h
@@ -54,8 +54,8 @@ namespace pcl
    */
   template <typename NormalT> inline std::vector<float>
   assignNormalWeights (const PointCloud<NormalT>& cloud,
-                       int index,
-                       const std::vector<int>& k_indices,
+                       index_t index,
+                       const Indices& k_indices,
                        const std::vector<float>& k_sqr_distances)
   {
     pcl::utils::ignore(cloud, index);
@@ -82,7 +82,7 @@ namespace pcl
   template <typename NormalT> inline bool
   refineNormal (const PointCloud<NormalT>& cloud,
                 int index,
-                const std::vector<int>& k_indices,
+                const Indices& k_indices,
                 const std::vector<float>& k_sqr_distances,
                 NormalT& point)
   {
@@ -157,13 +157,13 @@ namespace pcl
     * 
     * // Search parameters
     * const int k = 5;
-    * std::vector<std::vector<int> > k_indices;
+    * std::vector<Indices > k_indices;
     * std::vector<std::vector<float> > k_sqr_distances;
     * 
     * // Run search
     * pcl::search::KdTree<pcl::PointXYZRGB> search;
     * search.setInputCloud (cloud.makeShared ());
-    * search.nearestKSearch (cloud, std::vector<int> (), k, k_indices, k_sqr_distances);
+    * search.nearestKSearch (cloud, Indices (), k, k_indices, k_sqr_distances);
     * 
     * // Use search results for normal estimation
     * pcl::NormalEstimation<PointT, NormalT> ne;
@@ -212,7 +212,7 @@ namespace pcl
        * @param k_indices indices of neighboring points
        * @param k_sqr_distances squared distances to the neighboring points
        */
-      NormalRefinement (const std::vector< std::vector<int> >& k_indices, const std::vector< std::vector<float> >& k_sqr_distances) :
+      NormalRefinement (const std::vector< Indices >& k_indices, const std::vector< std::vector<float> >& k_sqr_distances) :
         Filter<NormalT>::Filter ()
       {
         filter_name_ = "NormalRefinement";
@@ -226,7 +226,7 @@ namespace pcl
        * @param k_sqr_distances squared distances to the neighboring points
        */
       inline void
-      setCorrespondences (const std::vector< std::vector<int> >& k_indices, const std::vector< std::vector<float> >& k_sqr_distances)
+      setCorrespondences (const std::vector< Indices >& k_indices, const std::vector< std::vector<float> >& k_sqr_distances)
       {
         k_indices_ = k_indices;
         k_sqr_distances_ = k_sqr_distances;
@@ -237,7 +237,7 @@ namespace pcl
        * @param k_sqr_distances squared distances to the neighboring points
        */
       inline void
-      getCorrespondences (std::vector< std::vector<int> >& k_indices, std::vector< std::vector<float> >& k_sqr_distances)
+      getCorrespondences (std::vector< Indices >& k_indices, std::vector< std::vector<float> >& k_sqr_distances)
       {
         k_indices.assign (k_indices_.begin (), k_indices_.end ());
         k_sqr_distances.assign (k_sqr_distances_.begin (), k_sqr_distances_.end ());
@@ -288,7 +288,7 @@ namespace pcl
       
     private:
       /** \brief indices of neighboring points */
-      std::vector< std::vector<int> > k_indices_;
+      std::vector< Indices > k_indices_;
       
       /** \brief squared distances to the neighboring points */
       std::vector< std::vector<float> > k_sqr_distances_;
diff --git a/filters/include/pcl/filters/normal_space.h b/filters/include/pcl/filters/normal_space.h
index 522db676..b34021a2 100644
--- a/filters/include/pcl/filters/normal_space.h
+++ b/filters/include/pcl/filters/normal_space.h
@@ -37,11 +37,9 @@
 
 #pragma once
 
-#include <pcl/filters/boost.h>
 #include <pcl/filters/filter_indices.h>
-
+#include <boost/dynamic_bitset.hpp> // for dynamic_bitset
 #include <ctime>
-#include <climits>
 #include <random> // std::mt19937
 
 namespace pcl
@@ -163,7 +161,7 @@ namespace pcl
         * \param[out] indices the resultant point cloud indices
         */
       void
-      applyFilter (std::vector<int> &indices) override;
+      applyFilter (Indices &indices) override;
 
       bool
       initCompute ();
diff --git a/filters/include/pcl/filters/passthrough.h b/filters/include/pcl/filters/passthrough.h
index d0f541d4..92f82d32 100644
--- a/filters/include/pcl/filters/passthrough.h
+++ b/filters/include/pcl/filters/passthrough.h
@@ -39,6 +39,7 @@
 
 #pragma once
 
+#include <cfloat> // for FLT_MIN, FLT_MAX
 #include <pcl/pcl_macros.h>
 #include <pcl/filters/filter_indices.h>
 
@@ -193,7 +194,7 @@ namespace pcl
         * \param[out] indices The resultant indices.
         */
       void
-      applyFilter (std::vector<int> &indices) override
+      applyFilter (Indices &indices) override
       {
         applyFilterIndices (indices);
       }
@@ -202,7 +203,7 @@ namespace pcl
         * \param[out] indices The resultant indices.
         */
       void
-      applyFilterIndices (std::vector<int> &indices);
+      applyFilterIndices (Indices &indices);
 
     private:
       /** \brief The name of the field that will be used for filtering. */
@@ -279,43 +280,12 @@ namespace pcl
         limit_max = filter_limit_max_;
       }
 
-      /** \brief Set to true if we want to return the data outside the interval specified by setFilterLimits (min, max).
-        * Default: false.
-        * \param[in] limit_negative return data inside the interval (false) or outside (true)
-        */
-      PCL_DEPRECATED(1, 12, "use inherited FilterIndices::setNegative() instead")
-      inline void
-      setFilterLimitsNegative (const bool limit_negative)
-      {
-        negative_ = limit_negative;
-      }
-
-      /** \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(1, 12, "use inherited FilterIndices::getNegative() instead")
-      inline void
-      getFilterLimitsNegative (bool &limit_negative) const
-      {
-        limit_negative = negative_;
-      }
-
-      /** \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(1, 12, "use inherited FilterIndices::getNegative() instead")
-      inline bool
-      getFilterLimitsNegative () const
-      {
-        return (negative_);
-      }
-
     protected:
       void
       applyFilter (PCLPointCloud2 &output) override;
 
       void
-      applyFilter (std::vector<int> &indices) override;
+      applyFilter (Indices &indices) override;
 
     private:
       /** \brief The desired user filter field name. */
diff --git a/filters/include/pcl/filters/plane_clipper3D.h b/filters/include/pcl/filters/plane_clipper3D.h
index eb73227b..f48aa92c 100644
--- a/filters/include/pcl/filters/plane_clipper3D.h
+++ b/filters/include/pcl/filters/plane_clipper3D.h
@@ -88,7 +88,7 @@ namespace pcl
       clipPlanarPolygon3D (const std::vector<PointT, Eigen::aligned_allocator<PointT> >& polygon, std::vector<PointT, Eigen::aligned_allocator<PointT> >& clipped_polygon) const;
 
       virtual void
-      clipPointCloud3D (const pcl::PointCloud<PointT> &cloud_in, std::vector<int>& clipped, const std::vector<int>& indices = std::vector<int> ()) const;
+      clipPointCloud3D (const pcl::PointCloud<PointT> &cloud_in, Indices& clipped, const Indices& indices = Indices ()) const;
 
       virtual Clipper3D<PointT>*
       clone () const;
diff --git a/filters/include/pcl/filters/project_inliers.h b/filters/include/pcl/filters/project_inliers.h
index ef026606..78c1601a 100644
--- a/filters/include/pcl/filters/project_inliers.h
+++ b/filters/include/pcl/filters/project_inliers.h
@@ -41,20 +41,7 @@
 #include <pcl/filters/filter.h>
 #include <pcl/ModelCoefficients.h>
 // Sample Consensus models
-#include <pcl/sample_consensus/model_types.h>
 #include <pcl/sample_consensus/sac_model.h>
-#include <pcl/sample_consensus/sac_model_circle.h>
-#include <pcl/sample_consensus/sac_model_cylinder.h>
-#include <pcl/sample_consensus/sac_model_cone.h>
-#include <pcl/sample_consensus/sac_model_line.h>
-#include <pcl/sample_consensus/sac_model_normal_plane.h>
-#include <pcl/sample_consensus/sac_model_normal_sphere.h>
-#include <pcl/sample_consensus/sac_model_parallel_plane.h>
-#include <pcl/sample_consensus/sac_model_normal_parallel_plane.h>
-#include <pcl/sample_consensus/sac_model_parallel_line.h>
-#include <pcl/sample_consensus/sac_model_perpendicular_plane.h>
-#include <pcl/sample_consensus/sac_model_plane.h>
-#include <pcl/sample_consensus/sac_model_sphere.h>
 
 namespace pcl
 {
@@ -170,7 +157,6 @@ namespace pcl
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   /** \brief @b ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a
     * separate PointCloud.
-    * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
     * \author Radu Bogdan Rusu
     * \ingroup filters
     */
diff --git a/filters/include/pcl/filters/radius_outlier_removal.h b/filters/include/pcl/filters/radius_outlier_removal.h
index 99a69029..62a65031 100644
--- a/filters/include/pcl/filters/radius_outlier_removal.h
+++ b/filters/include/pcl/filters/radius_outlier_removal.h
@@ -40,7 +40,7 @@
 #pragma once
 
 #include <pcl/filters/filter_indices.h>
-#include <pcl/search/pcl_search.h>
+#include <pcl/search/search.h> // for Search, Search<>::Ptr
 
 namespace pcl
 {
@@ -153,7 +153,7 @@ namespace pcl
         * \param[out] indices The resultant indices.
         */
       void
-      applyFilter (std::vector<int> &indices) override
+      applyFilter (Indices &indices) override
       {
         applyFilterIndices (indices);
       }
@@ -162,7 +162,7 @@ namespace pcl
         * \param[out] indices The resultant indices.
         */
       void
-      applyFilterIndices (std::vector<int> &indices);
+      applyFilterIndices (Indices &indices);
 
     private:
       /** \brief A pointer to the spatial search object. */
@@ -178,7 +178,6 @@ namespace pcl
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   /** \brief @b RadiusOutlierRemoval is a simple filter that removes outliers if the number of neighbors in a certain
     * search radius is smaller than a given K.
-    * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
     * \author Radu Bogdan Rusu
     * \ingroup filters
     */
@@ -258,7 +257,7 @@ namespace pcl
       applyFilter (PCLPointCloud2 &output) override;
 
       void
-      applyFilter (std::vector<int> &indices) override;
+      applyFilter (Indices &indices) override;
   };
 }
 
diff --git a/filters/include/pcl/filters/random_sample.h b/filters/include/pcl/filters/random_sample.h
index e29f4c8c..f2066df7 100644
--- a/filters/include/pcl/filters/random_sample.h
+++ b/filters/include/pcl/filters/random_sample.h
@@ -127,7 +127,7 @@ namespace pcl
         * \param indices the resultant point cloud indices
         */
       void
-      applyFilter (std::vector<int> &indices) override;
+      applyFilter (Indices &indices) override;
 
       /** \brief Return a random number fast using a LCG (Linear Congruential Generator) algorithm.
         * See http://software.intel.com/en-us/articles/fast-random-number-generator-on-the-intel-pentiumr-4-processor/ for more information.
@@ -216,7 +216,7 @@ namespace pcl
         * \param indices the resultant point cloud indices
         */
       void
-      applyFilter (std::vector<int> &indices) override;
+      applyFilter (Indices &indices) override;
 
       /** \brief Return a random number fast using a LCG (Linear Congruential Generator) algorithm.
         * See http://software.intel.com/en-us/articles/fast-random-number-generator-on-the-intel-pentiumr-4-processor/ for more information.
diff --git a/filters/include/pcl/filters/sampling_surface_normal.h b/filters/include/pcl/filters/sampling_surface_normal.h
index d39ef1c8..ea0a68bc 100644
--- a/filters/include/pcl/filters/sampling_surface_normal.h
+++ b/filters/include/pcl/filters/sampling_surface_normal.h
@@ -39,7 +39,6 @@
 
 #include <pcl/filters/filter.h>
 #include <ctime>
-#include <climits>
 
 namespace pcl
 {
@@ -192,7 +191,7 @@ namespace pcl
       void 
       partition (const PointCloud& cloud, const int first, const int last, 
                  const Vector min_values, const Vector max_values, 
-                 std::vector<int>& indices, PointCloud& outcloud);
+                 Indices& indices, PointCloud& outcloud);
 
       /** \brief Randomly sample the points in each grid.
         * \param[in] data 
@@ -203,7 +202,7 @@ namespace pcl
         */
       void 
       samplePartition (const PointCloud& data, const int first, const int last, 
-                       std::vector<int>& indices, PointCloud& outcloud);
+                       Indices& indices, PointCloud& outcloud);
 
       /** \brief Returns the threshold for splitting in a given dimension.
         * \param[in] cloud the input cloud
diff --git a/filters/include/pcl/filters/shadowpoints.h b/filters/include/pcl/filters/shadowpoints.h
index f3cce2ee..5e2fe6ad 100644
--- a/filters/include/pcl/filters/shadowpoints.h
+++ b/filters/include/pcl/filters/shadowpoints.h
@@ -38,8 +38,6 @@
 #pragma once
 
 #include <pcl/filters/filter_indices.h>
-#include <ctime>
-#include <climits>
 
 namespace pcl
 {
@@ -115,7 +113,7 @@ namespace pcl
         * \param[out] indices the resultant point cloud indices
         */
       void
-      applyFilter (std::vector<int> &indices) override;
+      applyFilter (Indices &indices) override;
 
     private:
 
diff --git a/filters/include/pcl/filters/statistical_outlier_removal.h b/filters/include/pcl/filters/statistical_outlier_removal.h
index bcc8a465..70b450de 100644
--- a/filters/include/pcl/filters/statistical_outlier_removal.h
+++ b/filters/include/pcl/filters/statistical_outlier_removal.h
@@ -40,7 +40,7 @@
 #pragma once
 
 #include <pcl/filters/filter_indices.h>
-#include <pcl/search/pcl_search.h>
+#include <pcl/search/search.h> // for Search
 
 namespace pcl
 {
@@ -157,7 +157,7 @@ namespace pcl
         * \param[out] indices The resultant indices.
         */
       void
-      applyFilter (std::vector<int> &indices) override
+      applyFilter (Indices &indices) override
       {
         applyFilterIndices (indices);
       }
@@ -166,7 +166,7 @@ namespace pcl
         * \param[out] indices The resultant indices.
         */
       void
-      applyFilterIndices (std::vector<int> &indices);
+      applyFilterIndices (Indices &indices);
 
     private:
       /** \brief A pointer to the spatial search object. */
@@ -186,7 +186,6 @@ namespace pcl
     *     Towards 3D Point Cloud Based Object Maps for Household Environments
     *     Robotics and Autonomous Systems Journal (Special Issue on Semantic Knowledge), 2008.
     *
-    * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
     * \author Radu Bogdan Rusu
     * \ingroup filters
     */
@@ -263,7 +262,7 @@ namespace pcl
       KdTreePtr tree_;
 
       void
-      applyFilter (std::vector<int> &indices) override;
+      applyFilter (Indices &indices) override;
 
       void
       applyFilter (PCLPointCloud2 &output) override;
diff --git a/filters/include/pcl/filters/uniform_sampling.h b/filters/include/pcl/filters/uniform_sampling.h
index a32f6692..d8ec1ce6 100644
--- a/filters/include/pcl/filters/uniform_sampling.h
+++ b/filters/include/pcl/filters/uniform_sampling.h
@@ -50,13 +50,11 @@ namespace pcl
     * The @b UniformSampling class creates a *3D voxel grid* (think about a voxel
     * grid as a set of tiny 3D boxes in space) over the input point cloud data.
     * Then, in each *voxel* (i.e., 3D box), all the points present will be
-    * approximated (i.e., *downsampled*) with their centroid. This approach is
-    * a bit slower than approximating them with the center of the voxel, but it
-    * represents the underlying surface more accurately.
+    * approximated (i.e., *downsampled*) with the closest point to the center of the voxel.
     *
     * \author Radu Bogdan Rusu
     * \ingroup filters
-    */
+    */ 
   template <typename PointT>
   class UniformSampling: public Filter<PointT>
   {
diff --git a/filters/include/pcl/filters/voxel_grid.h b/filters/include/pcl/filters/voxel_grid.h
index 0356bdb0..72ee252f 100644
--- a/filters/include/pcl/filters/voxel_grid.h
+++ b/filters/include/pcl/filters/voxel_grid.h
@@ -39,9 +39,8 @@
 
 #pragma once
 
-#include <pcl/filters/boost.h>
 #include <pcl/filters/filter.h>
-#include <map>
+#include <cfloat> // for FLT_MAX
 
 namespace pcl
 {
@@ -157,7 +156,7 @@ namespace pcl
     */
   template <typename PointT> void
   getMinMax3D (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
-               const std::vector<int> &indices,
+               const Indices &indices,
                const std::string &distance_field_name, float min_distance, float max_distance,
                Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative = false);
 
diff --git a/filters/include/pcl/filters/voxel_grid_covariance.h b/filters/include/pcl/filters/voxel_grid_covariance.h
index b5624f12..325c85d1 100644
--- a/filters/include/pcl/filters/voxel_grid_covariance.h
+++ b/filters/include/pcl/filters/voxel_grid_covariance.h
@@ -37,7 +37,6 @@
 
 #pragma once
 
-#include <pcl/filters/boost.h>
 #include <pcl/filters/voxel_grid.h>
 #include <map>
 #include <pcl/point_types.h>
@@ -92,12 +91,12 @@ namespace pcl
       struct Leaf
       {
         /** \brief Constructor.
-         * Sets \ref nr_points, \ref icov_, \ref mean_ and \ref evals_ to 0 and \ref cov_ and \ref evecs_ to the identity matrix
+         * Sets \ref nr_points, \ref cov_, \ref icov_, \ref mean_ and \ref evals_ to 0 and \ref evecs_ to the identity matrix
          */
         Leaf () :
           nr_points (0),
           mean_ (Eigen::Vector3d::Zero ()),
-          cov_ (Eigen::Matrix3d::Identity ()),
+          cov_ (Eigen::Matrix3d::Zero ()),
           icov_ (Eigen::Matrix3d::Zero ()),
           evecs_ (Eigen::Matrix3d::Identity ()),
           evals_ (Eigen::Vector3d::Zero ())
@@ -455,12 +454,12 @@ namespace pcl
         }
 
         // Find k-nearest neighbors in the occupied voxel centroid cloud
-        std::vector<int> k_indices;
+        Indices k_indices;
         k = kdtree_.nearestKSearch (point, k, k_indices, k_sqr_distances);
 
         // Find leaves corresponding to neighbors
         k_leaves.reserve (k);
-        for (const int &k_index : k_indices)
+        for (const auto &k_index : k_indices)
         {
           auto voxel = leaves_.find(voxel_centroids_leaf_indices_[k_index]);
           if (voxel == leaves_.end()) {
@@ -514,12 +513,12 @@ namespace pcl
         }
 
         // Find neighbors within radius in the occupied voxel centroid cloud
-        std::vector<int> k_indices;
+        Indices k_indices;
         const int k = kdtree_.radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
 
         // Find leaves corresponding to neighbors
         k_leaves.reserve (k);
-        for (const int &k_index : k_indices)
+        for (const auto &k_index : k_indices)
         {
           const auto voxel = leaves_.find(voxel_centroids_leaf_indices_[k_index]);
           if(voxel == leaves_.end()) {
diff --git a/filters/src/crop_box.cpp b/filters/src/crop_box.cpp
index 7d9ccf84..b4fa3f5b 100644
--- a/filters/src/crop_box.cpp
+++ b/filters/src/crop_box.cpp
@@ -43,7 +43,7 @@
 void
 pcl::CropBox<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
 {
-  std::vector<int> indices;
+  Indices indices;
   if (keep_organized_)
   {
     bool temp = extract_removed_indices_;
@@ -96,7 +96,7 @@ pcl::CropBox<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
 
 ///////////////////////////////////////////////////////////////////////////////
 void
-pcl::CropBox<pcl::PCLPointCloud2>::applyFilter (std::vector<int> &indices)
+pcl::CropBox<pcl::PCLPointCloud2>::applyFilter (Indices &indices)
 {
   indices.resize (input_->width * input_->height);
   removed_indices_->resize (input_->width * input_->height);
@@ -154,7 +154,7 @@ pcl::CropBox<pcl::PCLPointCloud2>::applyFilter (std::vector<int> &indices)
       }
       else if (extract_removed_indices_)
       {
-        (*removed_indices_)[removed_indices_count++] = static_cast<int> (index);
+        (*removed_indices_)[removed_indices_count++] = index;
       }
     }
     // If inside the cropbox
@@ -162,7 +162,7 @@ pcl::CropBox<pcl::PCLPointCloud2>::applyFilter (std::vector<int> &indices)
     {
       if (negative_ && extract_removed_indices_)
       {
-        (*removed_indices_)[removed_indices_count++] = static_cast<int> (index);
+        (*removed_indices_)[removed_indices_count++] = index;
       }
       else if (!negative_) {
         indices[indices_count++] = index;
diff --git a/filters/src/extract_indices.cpp b/filters/src/extract_indices.cpp
index dc26bcb2..6ac3544d 100644
--- a/filters/src/extract_indices.cpp
+++ b/filters/src/extract_indices.cpp
@@ -58,20 +58,20 @@ pcl::ExtractIndices<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
     else
     {
       // Prepare a vector holding all indices
-      std::vector<int> all_indices (input_->width * input_->height);
-      for (int i = 0; i < static_cast<int>(all_indices.size ()); ++i)
+      Indices all_indices (input_->width * input_->height);
+      for (index_t i = 0; i < static_cast<index_t>(all_indices.size ()); ++i)
         all_indices[i] = i;
 
-      std::vector<int> indices = *indices_;
+      Indices indices = *indices_;
       std::sort (indices.begin (), indices.end ());
 
       // Get the diference
-      std::vector<int> remaining_indices;
+      Indices remaining_indices;
       set_difference (all_indices.begin (), all_indices.end (), indices.begin (), indices.end (),
                       inserter (remaining_indices, remaining_indices.begin ()));
 
       // Prepare the output and copy the data
-      for (const int &remaining_index : remaining_indices)
+      for (const auto &remaining_index : remaining_indices)
         for (std::size_t j = 0; j < output.fields.size(); ++j)
           memcpy (&output.data[remaining_index * output.point_step + output.fields[j].offset],
                   &user_filter_value_, sizeof(float));
@@ -113,15 +113,15 @@ pcl::ExtractIndices<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
   if (negative_)
   {
     // Prepare a vector holding all indices
-    std::vector<int> all_indices (input_->width * input_->height);
-    for (int i = 0; i < static_cast<int>(all_indices.size ()); ++i)
+    Indices all_indices (input_->width * input_->height);
+    for (index_t i = 0; i < static_cast<index_t>(all_indices.size ()); ++i)
       all_indices[i] = i;
 
-    std::vector<int> indices = *indices_;
+    Indices indices = *indices_;
     std::sort (indices.begin (), indices.end ());
 
     // Get the diference
-    std::vector<int> remaining_indices;
+    Indices remaining_indices;
     set_difference (all_indices.begin (), all_indices.end (), indices.begin (), indices.end (),
                     inserter (remaining_indices, remaining_indices.begin ()));
 
@@ -144,7 +144,7 @@ pcl::ExtractIndices<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl::ExtractIndices<pcl::PCLPointCloud2>::applyFilter (std::vector<int> &indices)
+pcl::ExtractIndices<pcl::PCLPointCloud2>::applyFilter (Indices &indices)
 {
   if (indices_->size () > (input_->width * input_->height))
   {
@@ -161,12 +161,12 @@ pcl::ExtractIndices<pcl::PCLPointCloud2>::applyFilter (std::vector<int> &indices
     if (extract_removed_indices_)
     {
       // Set up the full indices set
-      std::vector<int> full_indices (input_->width * input_->height);
-      for (int fii = 0; fii < static_cast<int> (full_indices.size ()); ++fii)  // fii = full indices iterator
+      Indices full_indices (input_->width * input_->height);
+      for (index_t fii = 0; fii < static_cast<index_t> (full_indices.size ()); ++fii)  // fii = full indices iterator
         full_indices[fii] = fii;
 
       // Set up the sorted input indices
-      std::vector<int> sorted_input_indices = *indices_;
+      Indices sorted_input_indices = *indices_;
       std::sort (sorted_input_indices.begin (), sorted_input_indices.end ());
 
       // Store the difference in removed_indices
@@ -177,12 +177,12 @@ pcl::ExtractIndices<pcl::PCLPointCloud2>::applyFilter (std::vector<int> &indices
   else  // Inverted functionality
   {
     // Set up the full indices set
-    std::vector<int> full_indices (input_->width * input_->height);
-    for (int fii = 0; fii < static_cast<int> (full_indices.size ()); ++fii)  // fii = full indices iterator
+    Indices full_indices (input_->width * input_->height);
+    for (index_t fii = 0; fii < static_cast<index_t> (full_indices.size ()); ++fii)  // fii = full indices iterator
       full_indices[fii] = fii;
 
     // Set up the sorted input indices
-    std::vector<int> sorted_input_indices = *indices_;
+    Indices sorted_input_indices = *indices_;
     std::sort (sorted_input_indices.begin (), sorted_input_indices.end ());
 
     // Store the difference in indices
diff --git a/filters/src/filter_indices.cpp b/filters/src/filter_indices.cpp
index b30f6065..06eba155 100644
--- a/filters/src/filter_indices.cpp
+++ b/filters/src/filter_indices.cpp
@@ -45,7 +45,7 @@ namespace pcl { struct PCLPointCloud2; }
  * \param output the resultant filtered point cloud dataset
  */
 void
-pcl::FilterIndices<pcl::PCLPointCloud2>::filter (std::vector<int> &indices)
+pcl::FilterIndices<pcl::PCLPointCloud2>::filter (Indices &indices)
 {
   if (!initCompute ())
     return;
diff --git a/filters/src/passthrough.cpp b/filters/src/passthrough.cpp
index a5123ea6..77964e9f 100644
--- a/filters/src/passthrough.cpp
+++ b/filters/src/passthrough.cpp
@@ -274,7 +274,7 @@ pcl::PassThrough<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
 }
 
 void
-pcl::PassThrough<pcl::PCLPointCloud2>::applyFilter (std::vector<int> &indices)
+pcl::PassThrough<pcl::PCLPointCloud2>::applyFilter (Indices &indices)
 {
   // If input is not present, we cannot filter
   if (!input_)
@@ -297,8 +297,11 @@ pcl::PassThrough<pcl::PCLPointCloud2>::applyFilter (std::vector<int> &indices)
   indices.resize (indices_->size ());
   removed_indices_->resize (indices_->size ());
   int oii = 0, rii = 0;  // oii = output indices iterator, rii = removed indices iterator
-  const std::uint32_t xyz_offset[3] = {input_->fields[x_idx_].offset, input_->fields[y_idx_].offset, input_->fields[z_idx_].offset};
-  PCL_DEBUG ("[pcl::%s<pcl::PCLPointCloud2>::applyFilter] Field offsets: x: %u, y: %u, z: %u.\n", filter_name_.c_str (), xyz_offset[0], xyz_offset[1], xyz_offset[2]);
+  const auto x_offset = input_->fields[x_idx_].offset,
+             y_offset = input_->fields[y_idx_].offset,
+             z_offset = input_->fields[z_idx_].offset;
+  PCL_DEBUG ("[pcl::%s<pcl::PCLPointCloud2>::applyFilter] Field offsets: x: %zu, y: %zu, z: %zu.\n", filter_name_.c_str (),
+             static_cast<std::size_t>(x_offset), static_cast<std::size_t>(y_offset), static_cast<std::size_t>(z_offset));
 
   // Has a field name been specified?
   if (filter_field_name_.empty ())
@@ -307,9 +310,9 @@ pcl::PassThrough<pcl::PCLPointCloud2>::applyFilter (std::vector<int> &indices)
     for (const auto ii : indices)  // ii = input index
     {
       float pt[3];
-      memcpy (&pt[0], &input_->data[ii * input_->point_step + xyz_offset[0]], sizeof(float));
-      memcpy (&pt[1], &input_->data[ii * input_->point_step + xyz_offset[1]], sizeof(float));
-      memcpy (&pt[2], &input_->data[ii * input_->point_step + xyz_offset[2]], sizeof(float));
+      memcpy (&pt[0], &input_->data[ii * input_->point_step + x_offset], sizeof(float));
+      memcpy (&pt[1], &input_->data[ii * input_->point_step + y_offset], sizeof(float));
+      memcpy (&pt[2], &input_->data[ii * input_->point_step + z_offset], sizeof(float));
       // Non-finite entries are always passed to removed indices
       if (!std::isfinite (pt[0]) ||
           !std::isfinite (pt[1]) ||
@@ -338,9 +341,9 @@ pcl::PassThrough<pcl::PCLPointCloud2>::applyFilter (std::vector<int> &indices)
     for (const auto ii : indices)  // ii = input index
     {
       float pt[3];
-      memcpy (&pt[0], &input_->data[ii * input_->point_step + xyz_offset[0]], sizeof(float));
-      memcpy (&pt[1], &input_->data[ii * input_->point_step + xyz_offset[1]], sizeof(float));
-      memcpy (&pt[2], &input_->data[ii * input_->point_step + xyz_offset[2]], sizeof(float));
+      memcpy (&pt[0], &input_->data[ii * input_->point_step + x_offset], sizeof(float));
+      memcpy (&pt[1], &input_->data[ii * input_->point_step + y_offset], sizeof(float));
+      memcpy (&pt[2], &input_->data[ii * input_->point_step + z_offset], sizeof(float));
       // Non-finite entries are always passed to removed indices
       if (!std::isfinite (pt[0]) ||
           !std::isfinite (pt[1]) ||
diff --git a/filters/src/radius_outlier_removal.cpp b/filters/src/radius_outlier_removal.cpp
index a7565606..dacc26ff 100644
--- a/filters/src/radius_outlier_removal.cpp
+++ b/filters/src/radius_outlier_removal.cpp
@@ -83,7 +83,7 @@ pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &out
   searcher_->setInputCloud (cloud);
 
   // Allocate enough space to hold the results
-  std::vector<int> nn_indices (indices_->size ());
+  Indices nn_indices (indices_->size ());
   std::vector<float> nn_dists (indices_->size ());
 
   // Copy the common fields
@@ -154,7 +154,7 @@ pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &out
 }
 
 void
-pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2>::applyFilter (std::vector<int> &indices)
+pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2>::applyFilter (Indices &indices)
 {
   if (search_radius_ == 0.0)
   {
@@ -178,7 +178,7 @@ pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2>::applyFilter (std::vector<int> &i
   searcher_->setInputCloud (cloud);
 
   // The arrays to be used
-  std::vector<int> nn_indices (indices_->size ());
+  Indices nn_indices (indices_->size ());
   std::vector<float> nn_dists (indices_->size ());
   indices.resize (indices_->size ());
   removed_indices_->resize (indices_->size ());
diff --git a/filters/src/random_sample.cpp b/filters/src/random_sample.cpp
index 5bc2d556..1142ed38 100644
--- a/filters/src/random_sample.cpp
+++ b/filters/src/random_sample.cpp
@@ -44,7 +44,7 @@
 void
 pcl::RandomSample<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
 {
-  std::vector<int> indices;
+  Indices indices;
   if (keep_organized_)
   {
     bool temp = extract_removed_indices_;
@@ -97,7 +97,7 @@ pcl::RandomSample<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
 
 ///////////////////////////////////////////////////////////////////////////////
 void
-pcl::RandomSample<pcl::PCLPointCloud2>::applyFilter (std::vector<int> &indices)
+pcl::RandomSample<pcl::PCLPointCloud2>::applyFilter (Indices &indices)
 {
   // Note: this function does not have to access input_ at all
   std::size_t N = indices_->size ();
diff --git a/filters/src/statistical_outlier_removal.cpp b/filters/src/statistical_outlier_removal.cpp
index 739f397b..baaf90b9 100644
--- a/filters/src/statistical_outlier_removal.cpp
+++ b/filters/src/statistical_outlier_removal.cpp
@@ -135,7 +135,7 @@ pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2
 
 ///////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2>::applyFilter (std::vector<int>& indices)
+pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2>::applyFilter (Indices& indices)
 {
   // If fields x/y/z are not present, we cannot filter
   if (x_idx_ == UNAVAILABLE || y_idx_ == UNAVAILABLE || z_idx_ == UNAVAILABLE)
@@ -204,7 +204,7 @@ pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2>::generateStatistics (double&
   tree_->setInputCloud (cloud);
 
   // Allocate enough space to hold the results
-  std::vector<int> nn_indices (mean_k_);
+  Indices nn_indices (mean_k_);
   std::vector<float> nn_dists (mean_k_);
 
   distances.resize (indices_->size ());
diff --git a/filters/src/voxel_grid.cpp b/filters/src/voxel_grid.cpp
index 7ad23fa7..749fec98 100644
--- a/filters/src/voxel_grid.cpp
+++ b/filters/src/voxel_grid.cpp
@@ -42,6 +42,7 @@
 #include <pcl/common/io.h>
 #include <pcl/filters/impl/voxel_grid.hpp>
 #include <boost/sort/spreadsort/integer_sort.hpp>
+#include <array>
 
 using Array4size_t = Eigen::Array<std::size_t, 4, 1>;
 
@@ -229,7 +230,7 @@ pcl::VoxelGrid<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
 
   if( (dx*dy*dz) > static_cast<std::int64_t>(std::numeric_limits<std::int32_t>::max()) )
   {
-    PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName().c_str());
+    PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.\n", getClassName().c_str());
     //output.width = output.height = 0;
     //output.data.clear();
     //return;
@@ -385,12 +386,22 @@ pcl::VoxelGrid<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
   // we need to skip all the same, adjacenent idx values
   std::size_t total = 0;
   std::size_t index = 0;
+  // first_and_last_indices_vector[i] represents the index in index_vector of the first point in
+  // index_vector belonging to the voxel which corresponds to the i-th output point,
+  // and of the first point not belonging to.
+  std::vector<std::pair<index_t, index_t> > first_and_last_indices_vector;
+  // Worst case size
+  first_and_last_indices_vector.reserve (index_vector.size ());
   while (index < index_vector.size ()) 
   {
     std::size_t i = index + 1;
     while (i < index_vector.size () && index_vector[i].idx == index_vector[index].idx) 
       ++i;
-    ++total;
+    if ((i - index) >= min_points_per_voxel_)
+    {
+      ++total;
+      first_and_last_indices_vector.emplace_back(index, i);
+    }
     index = i;
   }
 
@@ -436,93 +447,66 @@ pcl::VoxelGrid<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
     xyz_offset = Array4size_t (0, 4, 8, 12);
 
   index=0;
-  Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
   Eigen::VectorXf temporary = Eigen::VectorXf::Zero (centroid_size);
+  const std::array<index_t, 3> field_indices {x_idx_, y_idx_, z_idx_};
 
-  for (std::size_t cp = 0; cp < index_vector.size ();)
+  for (const auto &cp : first_and_last_indices_vector)
   {
-    std::size_t point_offset = index_vector[cp].cloud_point_index * input_->point_step;
-    // Do we need to process all the fields?
-    if (!downsample_all_data_) 
-    {
-      memcpy (&pt[0], &input_->data[point_offset+input_->fields[x_idx_].offset], sizeof (float));
-      memcpy (&pt[1], &input_->data[point_offset+input_->fields[y_idx_].offset], sizeof (float));
-      memcpy (&pt[2], &input_->data[point_offset+input_->fields[z_idx_].offset], sizeof (float));
-      centroid[0] = pt[0];
-      centroid[1] = pt[1];
-      centroid[2] = pt[2];
-      centroid[3] = 0;
-    }
-    else
+    // calculate centroid - sum values from all input points, that have the same idx value in index_vector array
+    index_t first_index = cp.first;
+    index_t last_index = cp.second;
+
+    // index is centroid final position in resulting PointCloud
+    if (save_leaf_layout_)
+      leaf_layout_[index_vector[first_index].idx] = index;
+
+    //Limit downsampling to coords
+    if (!downsample_all_data_)
     {
-      // ---[ RGB special case
-      // fill extra r/g/b centroid field
-      if (rgba_index >= 0)
+      Eigen::Vector4f centroid (Eigen::Vector4f::Zero ());
+
+      for (index_t li = first_index; li < last_index; ++li)
       {
-        pcl::RGB rgb;
-        memcpy (&rgb, &input_->data[point_offset + input_->fields[rgba_index].offset], sizeof (RGB));
-        centroid[centroid_size-4] = rgb.r;
-        centroid[centroid_size-3] = rgb.g;
-        centroid[centroid_size-2] = rgb.b;
-        centroid[centroid_size-1] = rgb.a;
+        std::size_t point_offset = index_vector[li].cloud_point_index * input_->point_step;
+        for (uint8_t ind=0; ind<3; ++ind)
+        {
+          memcpy (&pt[ind], &input_->data[point_offset+input_->fields[field_indices[ind]].offset], sizeof (float));
+        }
+        centroid += pt;
       }
-      // Copy all the fields
-      for (std::size_t d = 0; d < input_->fields.size (); ++d)
-        memcpy (&centroid[d], &input_->data[point_offset + input_->fields[d].offset], field_sizes_[d]);
-    }
 
-    std::size_t i = cp + 1;
-    while (i < index_vector.size () && index_vector[i].idx == index_vector[cp].idx) 
-    {
-      std::size_t point_offset = index_vector[i].cloud_point_index * input_->point_step;
-      if (!downsample_all_data_) 
+      centroid /= static_cast<float> (last_index - first_index);
+      for (uint8_t ind=0; ind<3; ++ind)
       {
-        memcpy (&pt[0], &input_->data[point_offset+input_->fields[x_idx_].offset], sizeof (float));
-        memcpy (&pt[1], &input_->data[point_offset+input_->fields[y_idx_].offset], sizeof (float));
-        memcpy (&pt[2], &input_->data[point_offset+input_->fields[z_idx_].offset], sizeof (float));
-        centroid[0] += pt[0];
-        centroid[1] += pt[1];
-        centroid[2] += pt[2];
+        memcpy (&output.data[xyz_offset[ind]], &centroid[ind], sizeof (float));
       }
-      else
-      {
+      xyz_offset += output.point_step;
+    }
+    else
+    {
+
+      Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
+      // fill in the accumulator with leaf points
+      for (index_t li = first_index; li < last_index; ++li) {
+        std::size_t point_offset = index_vector[li].cloud_point_index * input_->point_step;
         // ---[ RGB special case
         // fill extra r/g/b centroid field
         if (rgba_index >= 0)
         {
-          pcl::RGB rgb;
-          memcpy (&rgb, &input_->data[point_offset + input_->fields[rgba_index].offset], sizeof (RGB));
-          temporary[centroid_size-4] = rgb.r;
-          temporary[centroid_size-3] = rgb.g;
-          temporary[centroid_size-2] = rgb.b;
-          temporary[centroid_size-1] = rgb.a;
+            pcl::RGB rgb;
+            memcpy (&rgb, &input_->data[point_offset + input_->fields[rgba_index].offset], sizeof (RGB));
+            temporary[centroid_size-4] = rgb.r;
+            temporary[centroid_size-3] = rgb.g;
+            temporary[centroid_size-2] = rgb.b;
+            temporary[centroid_size-1] = rgb.a;
         }
         // Copy all the fields
         for (std::size_t d = 0; d < input_->fields.size (); ++d)
           memcpy (&temporary[d], &input_->data[point_offset + input_->fields[d].offset], field_sizes_[d]);
         centroid += temporary;
       }
-      ++i;
-    }
-
-	  // Save leaf layout information for fast access to cells relative to current position
-    if (save_leaf_layout_)
-      leaf_layout_[index_vector[cp].idx] = static_cast<int> (index);
-
-    // Normalize the centroid
-    centroid /= static_cast<float> (i - cp);
-
-    // Do we need to process all the fields?
-    if (!downsample_all_data_)
-    {
-      // Copy the data
-      memcpy (&output.data[xyz_offset[0]], &centroid[0], sizeof (float));
-      memcpy (&output.data[xyz_offset[1]], &centroid[1], sizeof (float));
-      memcpy (&output.data[xyz_offset[2]], &centroid[2], sizeof (float));
-      xyz_offset += output.point_step;
-    }
-    else
-    {
+      centroid /= static_cast<float> (last_index - first_index);
+      
       std::size_t point_offset = index * output.point_step;
       // Copy all the fields
       for (std::size_t d = 0; d < output.fields.size (); ++d)
@@ -537,7 +521,7 @@ pcl::VoxelGrid<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
         memcpy (&output.data[point_offset + output.fields[rgba_index].offset], &rgb, sizeof (float));
       }
     }
-    cp = i;
+     
     ++index;
   }
 }
diff --git a/filters/src/voxel_grid_covariance.cpp b/filters/src/voxel_grid_covariance.cpp
index 0423e7f7..2d6c1621 100644
--- a/filters/src/voxel_grid_covariance.cpp
+++ b/filters/src/voxel_grid_covariance.cpp
@@ -37,7 +37,6 @@
  */
 
 #include <pcl/filters/impl/voxel_grid_covariance.hpp>
-#include <pcl/filters/impl/voxel_grid.hpp>
 
 #ifndef PCL_NO_PRECOMPILE
 #include <pcl/point_types.h>
diff --git a/filters/src/voxel_grid_label.cpp b/filters/src/voxel_grid_label.cpp
index e73a9813..c0eacb3a 100644
--- a/filters/src/voxel_grid_label.cpp
+++ b/filters/src/voxel_grid_label.cpp
@@ -49,7 +49,7 @@ pcl::VoxelGridLabel::applyFilter (PointCloud &output)
   {
     PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
@@ -71,7 +71,7 @@ pcl::VoxelGridLabel::applyFilter (PointCloud &output)
 
   if( (dx*dy*dz) > static_cast<std::int64_t>(std::numeric_limits<std::int32_t>::max()) )
   {
-    PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName().c_str());
+    PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.\n", getClassName().c_str());
     output.clear();
     return;
   }
@@ -205,7 +205,7 @@ pcl::VoxelGridLabel::applyFilter (PointCloud &output)
   }
 
   // Fourth pass: compute centroids, insert them into their final position
-  output.points.resize (total);
+  output.resize (total);
   if (save_leaf_layout_)
   {
     try
diff --git a/geometry/include/pcl/geometry/boost.h b/geometry/include/pcl/geometry/boost.h
index 75b6a6b5..0a4dfc63 100644
--- a/geometry/include/pcl/geometry/boost.h
+++ b/geometry/include/pcl/geometry/boost.h
@@ -41,7 +41,7 @@
 #pragma once
 
 #ifdef __GNUC__
-#  pragma GCC system_header
+#pragma GCC system_header
 #endif
 
 #include <boost/operators.hpp>
diff --git a/geometry/include/pcl/geometry/eigen.h b/geometry/include/pcl/geometry/eigen.h
index 8975e64a..2f250e29 100644
--- a/geometry/include/pcl/geometry/eigen.h
+++ b/geometry/include/pcl/geometry/eigen.h
@@ -41,8 +41,9 @@
 #pragma once
 
 #ifdef __GNUC__
-#  pragma GCC system_header
+#pragma GCC system_header
 #endif
+PCL_DEPRECATED_HEADER(1, 15, "Please include the needed eigen headers directly.")
 
 #include <Eigen/Core>
 #include <Eigen/StdVector>
diff --git a/geometry/include/pcl/geometry/get_boundary.h b/geometry/include/pcl/geometry/get_boundary.h
index 47d211bc..c27cb6a0 100644
--- a/geometry/include/pcl/geometry/get_boundary.h
+++ b/geometry/include/pcl/geometry/get_boundary.h
@@ -42,51 +42,53 @@
 
 #include <vector>
 
-namespace pcl
+namespace pcl {
+namespace geometry {
+/**
+ * \brief Get a collection of boundary half-edges for the input mesh.
+ * \param[in] mesh The input mesh.
+ * \param[out] boundary_he_collection Collection of boundary half-edges. Each element in
+ * the vector is one connected boundary. The whole boundary is the union of all
+ * elements.
+ * \param [in] expected_size If you already know the size of the longest
+ * boundary you can tell this here. Defaults to 3 (minimum possible boundary).
+ * \author Martin Saelzle
+ * \ingroup geometry
+ */
+template <class MeshT>
+void
+getBoundBoundaryHalfEdges(
+    const MeshT& mesh,
+    std::vector<typename MeshT::HalfEdgeIndices>& boundary_he_collection,
+    const std::size_t expected_size = 3)
 {
-  namespace geometry
-  {
-    /** \brief Get a collection of boundary half-edges for the input mesh.
-      * \param[in] mesh The input mesh.
-      * \param[out] boundary_he_collection Collection of boundary half-edges. Each element in the vector is one connected boundary. The whole boundary is the union of all elements.
-      * \param [in] expected_size If you already know the size of the longest boundary you can tell this here. Defaults to 3 (minimum possible boundary).
-      * \author Martin Saelzle
-      * \ingroup geometry
-      */
-    template <class MeshT> void
-    getBoundBoundaryHalfEdges (const MeshT&                                   mesh,
-                               std::vector <typename MeshT::HalfEdgeIndices>& boundary_he_collection,
-                               const std::size_t                                   expected_size = 3)
-    {
-      using Mesh = MeshT;
-      using HalfEdgeIndex = typename Mesh::HalfEdgeIndex;
-      using HalfEdgeIndices = typename Mesh::HalfEdgeIndices;
-      using IHEAFC = typename Mesh::InnerHalfEdgeAroundFaceCirculator;
+  using Mesh = MeshT;
+  using HalfEdgeIndex = typename Mesh::HalfEdgeIndex;
+  using HalfEdgeIndices = typename Mesh::HalfEdgeIndices;
+  using IHEAFC = typename Mesh::InnerHalfEdgeAroundFaceCirculator;
 
-      boundary_he_collection.clear ();
+  boundary_he_collection.clear();
 
-      HalfEdgeIndices boundary_he; boundary_he.reserve (expected_size);
-      std::vector <bool> visited (mesh.sizeEdges (), false);
-      IHEAFC circ, circ_end;
+  HalfEdgeIndices boundary_he;
+  boundary_he.reserve(expected_size);
+  std::vector<bool> visited(mesh.sizeEdges(), false);
+  IHEAFC circ, circ_end;
 
-      for (HalfEdgeIndex i (0); i<HalfEdgeIndex (mesh.sizeHalfEdges ()); ++i)
-      {
-        if (mesh.isBoundary (i) && !visited [pcl::geometry::toEdgeIndex (i).get ()])
-        {
-          boundary_he.clear ();
+  for (HalfEdgeIndex i(0); i < HalfEdgeIndex(mesh.sizeHalfEdges()); ++i) {
+    if (mesh.isBoundary(i) && !visited[pcl::geometry::toEdgeIndex(i).get()]) {
+      boundary_he.clear();
 
-          circ     = mesh.getInnerHalfEdgeAroundFaceCirculator (i);
-          circ_end = circ;
-          do
-          {
-            visited [pcl::geometry::toEdgeIndex (circ.getTargetIndex ()).get ()] = true;
-            boundary_he.push_back (circ.getTargetIndex ());
-          } while (++circ != circ_end);
+      circ = mesh.getInnerHalfEdgeAroundFaceCirculator(i);
+      circ_end = circ;
+      do {
+        visited[pcl::geometry::toEdgeIndex(circ.getTargetIndex()).get()] = true;
+        boundary_he.push_back(circ.getTargetIndex());
+      } while (++circ != circ_end);
 
-          boundary_he_collection.push_back (boundary_he);
-        }
-      }
+      boundary_he_collection.push_back(boundary_he);
     }
+  }
+}
 
-  } // End namespace geometry
+} // End namespace geometry
 } // End namespace pcl
diff --git a/geometry/include/pcl/geometry/impl/polygon_operations.hpp b/geometry/include/pcl/geometry/impl/polygon_operations.hpp
index 7ef17708..585ba6ae 100644
--- a/geometry/include/pcl/geometry/impl/polygon_operations.hpp
+++ b/geometry/include/pcl/geometry/impl/polygon_operations.hpp
@@ -39,68 +39,77 @@
 #include <pcl/geometry/polygon_operations.h>
 
 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT> void
-pcl::approximatePolygon (const PlanarPolygon<PointT>& polygon, PlanarPolygon<PointT>& approx_polygon, float threshold, bool refine, bool closed)
+template <typename PointT>
+void
+pcl::approximatePolygon(const PlanarPolygon<PointT>& polygon,
+                        PlanarPolygon<PointT>& approx_polygon,
+                        float threshold,
+                        bool refine,
+                        bool closed)
 {
-  const Eigen::Vector4f& coefficients = polygon.getCoefficients ();
-  const typename pcl::PointCloud<PointT>::VectorType &contour = polygon.getContour ();
-  
-  Eigen::Vector3f rotation_axis (coefficients[1], -coefficients[0], 0.0f);
-  rotation_axis.normalize ();
+  const Eigen::Vector4f& coefficients = polygon.getCoefficients();
+  const typename pcl::PointCloud<PointT>::VectorType& contour = polygon.getContour();
 
-  float rotation_angle = acosf (coefficients [2]);
-  Eigen::Affine3f transformation = Eigen::Translation3f (0, 0, coefficients [3]) * Eigen::AngleAxisf (rotation_angle, rotation_axis);
+  Eigen::Vector3f rotation_axis(coefficients[1], -coefficients[0], 0.0f);
+  rotation_axis.normalize();
 
-  typename pcl::PointCloud<PointT>::VectorType polygon2D (contour.size ());
-  for (std::size_t pIdx = 0; pIdx < polygon2D.size (); ++pIdx)
-    polygon2D [pIdx].getVector3fMap () = transformation * contour [pIdx].getVector3fMap ();
+  float rotation_angle = acosf(coefficients[2]);
+  Eigen::Affine3f transformation = Eigen::Translation3f(0, 0, coefficients[3]) *
+                                   Eigen::AngleAxisf(rotation_angle, rotation_axis);
+
+  typename pcl::PointCloud<PointT>::VectorType polygon2D(contour.size());
+  for (std::size_t pIdx = 0; pIdx < polygon2D.size(); ++pIdx)
+    polygon2D[pIdx].getVector3fMap() = transformation * contour[pIdx].getVector3fMap();
 
   typename pcl::PointCloud<PointT>::VectorType approx_polygon2D;
-  approximatePolygon2D<PointT> (polygon2D, approx_polygon2D, threshold, refine, closed);
-  
-  typename pcl::PointCloud<PointT>::VectorType &approx_contour = approx_polygon.getContour ();
-  approx_contour.resize (approx_polygon2D.size ());
-  
-  Eigen::Affine3f inv_transformation = transformation.inverse ();
-  for (std::size_t pIdx = 0; pIdx < approx_polygon2D.size (); ++pIdx)
-    approx_contour [pIdx].getVector3fMap () = inv_transformation * approx_polygon2D [pIdx].getVector3fMap ();
+  approximatePolygon2D<PointT>(polygon2D, approx_polygon2D, threshold, refine, closed);
+
+  typename pcl::PointCloud<PointT>::VectorType& approx_contour =
+      approx_polygon.getContour();
+  approx_contour.resize(approx_polygon2D.size());
+
+  Eigen::Affine3f inv_transformation = transformation.inverse();
+  for (std::size_t pIdx = 0; pIdx < approx_polygon2D.size(); ++pIdx)
+    approx_contour[pIdx].getVector3fMap() =
+        inv_transformation * approx_polygon2D[pIdx].getVector3fMap();
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> void
-pcl::approximatePolygon2D (const typename pcl::PointCloud<PointT>::VectorType &polygon, 
-                           typename pcl::PointCloud<PointT>::VectorType &approx_polygon, 
-                           float threshold, bool refine, bool closed)
+template <typename PointT>
+void
+pcl::approximatePolygon2D(const typename pcl::PointCloud<PointT>::VectorType& polygon,
+                          typename pcl::PointCloud<PointT>::VectorType& approx_polygon,
+                          float threshold,
+                          bool refine,
+                          bool closed)
 {
-  approx_polygon.clear ();
-  if (polygon.size () < 3)
+  approx_polygon.clear();
+  if (polygon.size() < 3)
     return;
-  
-  std::vector<std::pair<unsigned, unsigned> > intervals;
-  std::pair<unsigned,unsigned> interval (0, 0);
-  
-  if (closed)
-  {
+
+  std::vector<std::pair<unsigned, unsigned>> intervals;
+  std::pair<unsigned, unsigned> interval(0, 0);
+
+  if (closed) {
     float max_distance = .0f;
-    for (std::size_t idx = 1; idx < polygon.size (); ++idx)
-    {
-      float distance = (polygon [0].x - polygon [idx].x) * (polygon [0].x - polygon [idx].x) + 
-                       (polygon [0].y - polygon [idx].y) * (polygon [0].y - polygon [idx].y);
+    for (std::size_t idx = 1; idx < polygon.size(); ++idx) {
+      float distance =
+          (polygon[0].x - polygon[idx].x) * (polygon[0].x - polygon[idx].x) +
+          (polygon[0].y - polygon[idx].y) * (polygon[0].y - polygon[idx].y);
 
-      if (distance > max_distance)
-      {
+      if (distance > max_distance) {
         max_distance = distance;
         interval.second = idx;
       }
     }
 
-    for (std::size_t idx = 1; idx < polygon.size (); ++idx)
-    {
-      float distance = (polygon [interval.second].x - polygon [idx].x) * (polygon [interval.second].x - polygon [idx].x) + 
-                       (polygon [interval.second].y - polygon [idx].y) * (polygon [interval.second].y - polygon [idx].y);
+    for (std::size_t idx = 1; idx < polygon.size(); ++idx) {
+      float distance = (polygon[interval.second].x - polygon[idx].x) *
+                           (polygon[interval.second].x - polygon[idx].x) +
+                       (polygon[interval.second].y - polygon[idx].y) *
+                           (polygon[interval.second].y - polygon[idx].y);
 
-      if (distance > max_distance)
-      {
+      if (distance > max_distance) {
         max_distance = distance;
         interval.first = idx;
       }
@@ -109,190 +118,181 @@ pcl::approximatePolygon2D (const typename pcl::PointCloud<PointT>::VectorType &p
     if (max_distance < threshold * threshold)
       return;
 
-    intervals.push_back (interval);
-    std::swap (interval.first, interval.second);
-    intervals.push_back (interval);
+    intervals.push_back(interval);
+    std::swap(interval.first, interval.second);
+    intervals.push_back(interval);
   }
-  else
-  {
+  else {
     interval.first = 0;
-    interval.second = static_cast<unsigned int> (polygon.size ()) - 1;
-    intervals.push_back (interval);
+    interval.second = static_cast<unsigned int>(polygon.size()) - 1;
+    intervals.push_back(interval);
   }
-  
+
   std::vector<unsigned> result;
   // recursively refine
-  while (!intervals.empty ())
-  {
-    std::pair<unsigned, unsigned>& currentInterval = intervals.back ();
-    float line_x = polygon [currentInterval.first].y - polygon [currentInterval.second].y;
-    float line_y = polygon [currentInterval.second].x - polygon [currentInterval.first].x;
-    float line_d = polygon [currentInterval.first].x * polygon [currentInterval.second].y - polygon [currentInterval.first].y * polygon [currentInterval.second].x;
-    
-    float linelen = 1.0f / std::sqrt (line_x * line_x + line_y * line_y);
-    
+  while (!intervals.empty()) {
+    std::pair<unsigned, unsigned>& currentInterval = intervals.back();
+    float line_x = polygon[currentInterval.first].y - polygon[currentInterval.second].y;
+    float line_y = polygon[currentInterval.second].x - polygon[currentInterval.first].x;
+    float line_d =
+        polygon[currentInterval.first].x * polygon[currentInterval.second].y -
+        polygon[currentInterval.first].y * polygon[currentInterval.second].x;
+
+    float linelen = 1.0f / std::sqrt(line_x * line_x + line_y * line_y);
+
     line_x *= linelen;
     line_y *= linelen;
     line_d *= linelen;
-    
+
     float max_distance = 0.0;
     unsigned first_index = currentInterval.first + 1;
     unsigned max_index = 0;
 
     // => 0-crossing
-    if (currentInterval.first > currentInterval.second)
-    {
-      for (std::size_t idx = first_index; idx < polygon.size(); idx++)
-      {
-        float distance = std::abs (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
-        if (distance > max_distance)
-        {
+    if (currentInterval.first > currentInterval.second) {
+      for (std::size_t idx = first_index; idx < polygon.size(); idx++) {
+        float distance =
+            std::abs(line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
+        if (distance > max_distance) {
           max_distance = distance;
-          max_index  = idx;
+          max_index = idx;
         }
       }
       first_index = 0;
     }
 
-    for (unsigned int idx = first_index; idx < currentInterval.second; idx++)
-    {
-      float distance = std::abs (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
-      if (distance > max_distance)
-      {
+    for (unsigned int idx = first_index; idx < currentInterval.second; idx++) {
+      float distance =
+          std::abs(line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
+      if (distance > max_distance) {
         max_distance = distance;
-        max_index  = idx;
+        max_index = idx;
       }
     }
 
-    if (max_distance > threshold)
-    {
-      std::pair<unsigned, unsigned> interval (max_index, currentInterval.second);
+    if (max_distance > threshold) {
+      std::pair<unsigned, unsigned> interval(max_index, currentInterval.second);
       currentInterval.second = max_index;
-      intervals.push_back (interval);
+      intervals.push_back(interval);
     }
-    else
-    {
-      result.push_back (currentInterval.second);
-      intervals.pop_back ();
+    else {
+      result.push_back(currentInterval.second);
+      intervals.pop_back();
     }
   }
-  
-  approx_polygon.reserve (result.size ());
-  if (refine)
-  {
-    std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > lines (result.size ());
-    std::reverse (result.begin (), result.end ());
-    for (std::size_t rIdx = 0; rIdx < result.size (); ++rIdx)
-    {
+
+  approx_polygon.reserve(result.size());
+  if (refine) {
+    std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> lines(
+        result.size());
+    std::reverse(result.begin(), result.end());
+    for (std::size_t rIdx = 0; rIdx < result.size(); ++rIdx) {
       std::size_t nIdx = rIdx + 1;
-      if (nIdx == result.size ())
+      if (nIdx == result.size())
         nIdx = 0;
-      
-      Eigen::Vector2f centroid = Eigen::Vector2f::Zero ();
-      Eigen::Matrix2f covariance = Eigen::Matrix2f::Zero ();
+
+      Eigen::Vector2f centroid = Eigen::Vector2f::Zero();
+      Eigen::Matrix2f covariance = Eigen::Matrix2f::Zero();
       std::size_t pIdx = result[rIdx];
       unsigned num_points = 0;
-      if (pIdx > result[nIdx])
-      {
-        num_points = static_cast<unsigned> (polygon.size ()) - pIdx;
-        for (; pIdx < polygon.size (); ++pIdx)
-        {
-          covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x;
-          covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y;
-          covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y;
-          centroid [0] += polygon [pIdx].x;
-          centroid [1] += polygon [pIdx].y;
+      if (pIdx > result[nIdx]) {
+        num_points = static_cast<unsigned>(polygon.size()) - pIdx;
+        for (; pIdx < polygon.size(); ++pIdx) {
+          covariance.coeffRef(0) += polygon[pIdx].x * polygon[pIdx].x;
+          covariance.coeffRef(1) += polygon[pIdx].x * polygon[pIdx].y;
+          covariance.coeffRef(3) += polygon[pIdx].y * polygon[pIdx].y;
+          centroid[0] += polygon[pIdx].x;
+          centroid[1] += polygon[pIdx].y;
         }
         pIdx = 0;
       }
-      
+
       num_points += result[nIdx] - pIdx;
-      for (; pIdx < result[nIdx]; ++pIdx)
-      {
-        covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x;
-        covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y;
-        covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y;
-        centroid [0] += polygon [pIdx].x;
-        centroid [1] += polygon [pIdx].y;
+      for (; pIdx < result[nIdx]; ++pIdx) {
+        covariance.coeffRef(0) += polygon[pIdx].x * polygon[pIdx].x;
+        covariance.coeffRef(1) += polygon[pIdx].x * polygon[pIdx].y;
+        covariance.coeffRef(3) += polygon[pIdx].y * polygon[pIdx].y;
+        centroid[0] += polygon[pIdx].x;
+        centroid[1] += polygon[pIdx].y;
       }
-      
-      covariance.coeffRef (2) = covariance.coeff (1);
-      
-      float norm = 1.0f / float (num_points);
+
+      covariance.coeffRef(2) = covariance.coeff(1);
+
+      float norm = 1.0f / float(num_points);
       centroid *= norm;
       covariance *= norm;
-      covariance.coeffRef (0) -= centroid [0] * centroid [0];
-      covariance.coeffRef (1) -= centroid [0] * centroid [1];
-      covariance.coeffRef (3) -= centroid [1] * centroid [1];
-      
+      covariance.coeffRef(0) -= centroid[0] * centroid[0];
+      covariance.coeffRef(1) -= centroid[0] * centroid[1];
+      covariance.coeffRef(3) -= centroid[1] * centroid[1];
+
       float eval;
       Eigen::Vector2f normal;
-      eigen22 (covariance, eval, normal);
+      eigen22(covariance, eval, normal);
 
       // select the one which is more "parallel" to the original line
       Eigen::Vector2f direction;
-      direction [0] = polygon[result[nIdx]].x - polygon[result[rIdx]].x;
-      direction [1] = polygon[result[nIdx]].y - polygon[result[rIdx]].y;
-      direction.normalize ();
-      
-      if (std::abs (direction.dot (normal)) > float(M_SQRT1_2))
-      {
-        std::swap (normal [0], normal [1]);
-        normal [0] = -normal [0];
+      direction[0] = polygon[result[nIdx]].x - polygon[result[rIdx]].x;
+      direction[1] = polygon[result[nIdx]].y - polygon[result[rIdx]].y;
+      direction.normalize();
+
+      if (std::abs(direction.dot(normal)) > float(M_SQRT1_2)) {
+        std::swap(normal[0], normal[1]);
+        normal[0] = -normal[0];
       }
-      
+
       // needs to be on the left side of the edge
-      if (direction [0] * normal [1] < direction [1] * normal [0])
+      if (direction[0] * normal[1] < direction[1] * normal[0])
         normal *= -1.0;
-      
-      lines [rIdx].head<2> ().matrix () = normal;
-      lines [rIdx] [2] = -normal.dot (centroid);
+
+      lines[rIdx].head<2>().matrix() = normal;
+      lines[rIdx][2] = -normal.dot(centroid);
     }
-    
+
     float threshold2 = threshold * threshold;
-    for (std::size_t rIdx = 0; rIdx < lines.size (); ++rIdx)
-    {
+    for (std::size_t rIdx = 0; rIdx < lines.size(); ++rIdx) {
       std::size_t nIdx = rIdx + 1;
-      if (nIdx == result.size ())
-        nIdx = 0;      
-      
-      Eigen::Vector3f vertex = lines [rIdx].cross (lines [nIdx]);
-      vertex /= vertex [2];
-      vertex [2] = 0.0;
-
-      PointT point;      
-      // test whether we need another edge since the intersection point is too far away from the original vertex
-      Eigen::Vector3f pq = polygon [result[nIdx]].getVector3fMap () - vertex;
-      pq [2] = 0.0;
-      
-      float distance = pq.squaredNorm ();
-      if (distance > threshold2)
-      {
+      if (nIdx == result.size())
+        nIdx = 0;
+
+      Eigen::Vector3f vertex = lines[rIdx].cross(lines[nIdx]);
+      vertex /= vertex[2];
+      vertex[2] = 0.0;
+
+      PointT point;
+      // test whether we need another edge since the intersection point is too far away
+      // from the original vertex
+      Eigen::Vector3f pq = polygon[result[nIdx]].getVector3fMap() - vertex;
+      pq[2] = 0.0;
+
+      float distance = pq.squaredNorm();
+      if (distance > threshold2) {
         // test whether the old point is inside the new polygon or outside
-        if ((pq [0] * lines [rIdx] [0] + pq [1] * lines [rIdx] [1] < 0.0) &&
-            (pq [0] * lines [nIdx] [0] + pq [1] * lines [nIdx] [1] < 0.0) )
-        {
-          float distance1 = lines [rIdx] [0] * polygon[result[nIdx]].x + lines [rIdx] [1] * polygon[result[nIdx]].y + lines [rIdx] [2];
-          float distance2 = lines [nIdx] [0] * polygon[result[nIdx]].x + lines [nIdx] [1] * polygon[result[nIdx]].y + lines [nIdx] [2];
+        if ((pq[0] * lines[rIdx][0] + pq[1] * lines[rIdx][1] < 0.0) &&
+            (pq[0] * lines[nIdx][0] + pq[1] * lines[nIdx][1] < 0.0)) {
+          float distance1 = lines[rIdx][0] * polygon[result[nIdx]].x +
+                            lines[rIdx][1] * polygon[result[nIdx]].y + lines[rIdx][2];
+          float distance2 = lines[nIdx][0] * polygon[result[nIdx]].x +
+                            lines[nIdx][1] * polygon[result[nIdx]].y + lines[nIdx][2];
 
-          point.x = polygon[result[nIdx]].x - distance1 * lines [rIdx] [0];
-          point.y = polygon[result[nIdx]].y - distance1 * lines [rIdx] [1];
+          point.x = polygon[result[nIdx]].x - distance1 * lines[rIdx][0];
+          point.y = polygon[result[nIdx]].y - distance1 * lines[rIdx][1];
 
-          approx_polygon.push_back (point);
+          approx_polygon.push_back(point);
 
-          vertex [0] = polygon[result[nIdx]].x - distance2 * lines [nIdx] [0];
-          vertex [1] = polygon[result[nIdx]].y - distance2 * lines [nIdx] [1];
+          vertex[0] = polygon[result[nIdx]].x - distance2 * lines[nIdx][0];
+          vertex[1] = polygon[result[nIdx]].y - distance2 * lines[nIdx][1];
         }
       }
-      point.getVector3fMap () = vertex;
-      approx_polygon.push_back (point);
+      point.getVector3fMap() = vertex;
+      approx_polygon.push_back(point);
     }
   }
-  else
-  {
-    // we have a new polygon in results, but inverted (clockwise <-> counter-clockwise)    
-    for (std::vector<unsigned>::reverse_iterator it = result.rbegin (); it != result.rend (); ++it)
-      approx_polygon.push_back (polygon [*it]);
+  else {
+    // we have a new polygon in results, but inverted (clockwise <-> counter-clockwise)
+    for (std::vector<unsigned>::reverse_iterator it = result.rbegin();
+         it != result.rend();
+         ++it)
+      approx_polygon.push_back(polygon[*it]);
   }
 }
 
diff --git a/geometry/include/pcl/geometry/line_iterator.h b/geometry/include/pcl/geometry/line_iterator.h
index a8223d11..32e70c1d 100644
--- a/geometry/include/pcl/geometry/line_iterator.h
+++ b/geometry/include/pcl/geometry/line_iterator.h
@@ -37,118 +37,135 @@
 
 #include "organized_index_iterator.h"
 
-namespace pcl
-{
-/** \brief Organized Index Iterator for iterating over the "pixels" for a given line using the Bresenham algorithm.
-  * Supports 4 and 8 neighborhood connectivity
-  * \note iterator does not visit the given end-point (on purpose).
-  * \author Suat Gedikli <gedikli@willowgarage.com>
-  * \ingroup  geometry
-  */
-class LineIterator : public OrganizedIndexIterator
-{
-  public:
-    /** \brief Neighborhood connectivity  */
-    enum Neighborhood {Neighbor4 = 4, Neighbor8 = 8};
-  public:
-    /** \brief Constructor
-      * \param x_start column of the start pixel of the line
-      * \param y_start row of the start pixel of the line
-      * \param x_end column of the end pixel of the line
-      * \param y_end row of the end pixel of the line
-      * \param width width of the organized structure e.g. image/cloud/map etc..
-      * \param neighborhood connectivity of the neighborhood
-      */
-    LineIterator (unsigned x_start, unsigned y_start, unsigned x_end, unsigned y_end, unsigned width, const Neighborhood& neighborhood = Neighbor8);
-    
-    /** \brief Destructor*/
-    ~LineIterator ();
-    
-    void operator ++ () override;
-    unsigned getRowIndex () const override;
-    unsigned getColumnIndex () const override;
-    bool isValid () const override;
-    void reset () override;
-  protected:
-    /** \brief initializes the variables for the Bresenham algorithm
-      * \param[in] neighborhood connectivity to the neighborhood. Either 4 or 8
-      */
-    void init (const Neighborhood& neighborhood);
-    
-    /** \brief current column index*/
-    unsigned x_;
-    
-    /** \brief current row index*/
-    unsigned y_;
-    
-    /** \brief column index of first pixel/point*/
-    unsigned x_start_;
-    
-    /** \brief row index of first pixel/point*/
-    unsigned y_start_;
-    
-    /** \brief column index of end pixel/point*/
-    unsigned x_end_;
-    
-    /** \brief row index of end pixel/point*/
-    unsigned y_end_;
-    
-    // calculated values
-    /** \brief current distance to the line*/
-    int error_;
-    
-    /** \brief error threshold*/
-    int error_max_;
-    
-    /** \brief increment of error (distance) value in case of an y-step (if dx > dy)*/
-    int error_minus_;
-    
-    /** \brief increment of error (distance) value in case of just an x-step (if dx > dy)*/
-    int error_plus_;
-    
-    /** \brief increment of column index in case of just an x-step (if dx > dy)*/
-    int x_plus_;
-
-    /** \brief increment of row index in case of just an x-step (if dx > dy)*/
-    int y_plus_;
-    
-    /** \brief increment of column index in case of just an y-step (if dx > dy)*/
-    int x_minus_;
-
-    /** \brief increment of row index in case of just an y-step (if dx > dy)*/
-    int y_minus_;
-    
-    /** \brief increment pixel/point index in case of just an x-step (if dx > dy)*/
-    int index_plus_;
-
-    /** \brief increment pixel/point index in case of just an y-step (if dx > dy)*/
-    int index_minus_;
+namespace pcl {
+/**
+ * \brief Organized Index Iterator for iterating over the "pixels" for a given line
+ * using the Bresenham algorithm. Supports 4 and 8 neighborhood connectivity
+ * \note iterator does not visit the given end-point (on purpose).
+ * \author Suat Gedikli <gedikli@willowgarage.com>
+ * \ingroup  geometry
+ */
+class LineIterator : public OrganizedIndexIterator {
+public:
+  /** \brief Neighborhood connectivity  */
+  enum Neighborhood { Neighbor4 = 4, Neighbor8 = 8 };
+
+public:
+  /**
+   * \brief Constructor
+   * \param x_start column of the start pixel of the line
+   * \param y_start row of the start pixel of the line
+   * \param x_end column of the end pixel of the line
+   * \param y_end row of the end pixel of the line
+   * \param width width of the organized structure e.g. image/cloud/map etc..
+   * \param neighborhood connectivity of the neighborhood
+   */
+  LineIterator(unsigned x_start,
+               unsigned y_start,
+               unsigned x_end,
+               unsigned y_end,
+               unsigned width,
+               const Neighborhood& neighborhood = Neighbor8);
+
+  /** \brief Destructor*/
+  ~LineIterator();
+
+  void
+  operator++() override;
+  unsigned
+  getRowIndex() const override;
+  unsigned
+  getColumnIndex() const override;
+  bool
+  isValid() const override;
+  void
+  reset() override;
+
+protected:
+  /**
+   * \brief initializes the variables for the Bresenham algorithm
+   * \param[in] neighborhood connectivity to the neighborhood. Either 4 or 8
+   */
+  void
+  init(const Neighborhood& neighborhood);
+
+  /** \brief current column index*/
+  unsigned x_;
+
+  /** \brief current row index*/
+  unsigned y_;
+
+  /** \brief column index of first pixel/point*/
+  unsigned x_start_;
+
+  /** \brief row index of first pixel/point*/
+  unsigned y_start_;
+
+  /** \brief column index of end pixel/point*/
+  unsigned x_end_;
+
+  /** \brief row index of end pixel/point*/
+  unsigned y_end_;
+
+  // calculated values
+  /** \brief current distance to the line*/
+  int error_;
+
+  /** \brief error threshold*/
+  int error_max_;
+
+  /** \brief increment of error (distance) value in case of an y-step (if dx > dy)*/
+  int error_minus_;
+
+  /** \brief increment of error (distance) value in case of just an x-step (if dx >
+   * dy)*/
+  int error_plus_;
+
+  /** \brief increment of column index in case of just an x-step (if dx > dy)*/
+  int x_plus_;
+
+  /** \brief increment of row index in case of just an x-step (if dx > dy)*/
+  int y_plus_;
+
+  /** \brief increment of column index in case of just an y-step (if dx > dy)*/
+  int x_minus_;
+
+  /** \brief increment of row index in case of just an y-step (if dx > dy)*/
+  int y_minus_;
+
+  /** \brief increment pixel/point index in case of just an x-step (if dx > dy)*/
+  int index_plus_;
+
+  /** \brief increment pixel/point index in case of just an y-step (if dx > dy)*/
+  int index_minus_;
 };
 
 ////////////////////////////////////////////////////////////////////////////////
 ////////////////////////////////////////////////////////////////////////////////
 ////////////////////////////////////////////////////////////////////////////////
 
-
 ////////////////////////////////////////////////////////////////////////////////
-inline LineIterator::LineIterator (unsigned x_start, unsigned y_start, unsigned x_end, unsigned y_end, unsigned width, const Neighborhood& neighborhood)
-: OrganizedIndexIterator (width)
-, x_start_ (x_start)
-, y_start_ (y_start)
-, x_end_ (x_end)
-, y_end_ (y_end)
+inline LineIterator::LineIterator(unsigned x_start,
+                                  unsigned y_start,
+                                  unsigned x_end,
+                                  unsigned y_end,
+                                  unsigned width,
+                                  const Neighborhood& neighborhood)
+: OrganizedIndexIterator(width)
+, x_start_(x_start)
+, y_start_(y_start)
+, x_end_(x_end)
+, y_end_(y_end)
 {
-  init (neighborhood);
+  init(neighborhood);
 }
 
 ////////////////////////////////////////////////////////////////////////////////
-inline LineIterator::~LineIterator ()
-{  
-}
+inline LineIterator::~LineIterator() {}
 
 ////////////////////////////////////////////////////////////////////////////////
 inline void
-LineIterator::init (const Neighborhood& neighborhood)
+LineIterator::init(const Neighborhood& neighborhood)
 {
   x_ = x_start_;
   y_ = y_start_;
@@ -157,112 +174,104 @@ LineIterator::init (const Neighborhood& neighborhood)
 
   int delta_x = x_end_ - x_start_;
   int delta_y = y_end_ - y_start_;
-  
-  int x_dir = ( (delta_x > 0) ? 1 : -1 ) ;
-  int y_dir = ( (delta_y > 0) ? 1 : -1 ) ;
+
+  int x_dir = ((delta_x > 0) ? 1 : -1);
+  int y_dir = ((delta_y > 0) ? 1 : -1);
 
   delta_x *= x_dir;
   delta_y *= y_dir;
-  
-  if(delta_x >= delta_y)
-  {
-    if( neighborhood == Neighbor4 )
-    {
+
+  if (delta_x >= delta_y) {
+    if (neighborhood == Neighbor4) {
       error_max_ = delta_x - delta_y;
       x_minus_ = 0;
       y_minus_ = y_dir;
-      x_plus_  = x_dir;
-      y_plus_  = 0;
+      x_plus_ = x_dir;
+      y_plus_ = 0;
 
       error_minus_ = -(delta_x * 2);
-      error_plus_  = (delta_y * 2);
+      error_plus_ = (delta_y * 2);
     }
-    else
-    {
+    else {
       error_max_ = delta_x - (delta_y * 2);
       x_minus_ = x_dir;
       y_minus_ = y_dir;
-      x_plus_  = x_dir;
-      y_plus_  = 0;
-        
+      x_plus_ = x_dir;
+      y_plus_ = 0;
+
       error_minus_ = (delta_y - delta_x) * 2;
-      error_plus_  = (delta_y * 2);
+      error_plus_ = (delta_y * 2);
     }
   }
-  else
-  {
-    if( neighborhood == Neighbor4 )
-    {
+  else {
+    if (neighborhood == Neighbor4) {
       error_max_ = delta_y - delta_x;
       x_minus_ = x_dir;
       y_minus_ = 0;
-      x_plus_  = 0;
-      y_plus_  = y_dir;
+      x_plus_ = 0;
+      y_plus_ = y_dir;
 
       error_minus_ = -(delta_y * 2);
-      error_plus_  = (delta_x * 2);
+      error_plus_ = (delta_x * 2);
     }
-    else
-    {
+    else {
       error_max_ = delta_y - (delta_x * 2);
       x_minus_ = x_dir;
       y_minus_ = y_dir;
-      x_plus_  = 0;
-      y_plus_  = y_dir;
+      x_plus_ = 0;
+      y_plus_ = y_dir;
 
       error_minus_ = (delta_x - delta_y) * 2;
-      error_plus_  = (delta_x * 2);
+      error_plus_ = (delta_x * 2);
     }
   }
 
   index_minus_ = x_minus_ + y_minus_ * width_;
-  index_plus_ = x_plus_ + y_plus_ * width_;  
+  index_plus_ = x_plus_ + y_plus_ * width_;
 }
 
 ////////////////////////////////////////////////////////////////////////////////
 inline void
-LineIterator::operator ++ ()
+LineIterator::operator++()
 {
-  if (error_ >= error_max_ )
-  {
+  if (error_ >= error_max_) {
     x_ += x_minus_;
     y_ += y_minus_;
     error_ += error_minus_;
     index_ += index_minus_;
   }
-  else
-  {
+  else {
     x_ += x_plus_;
     y_ += y_plus_;
     error_ += error_plus_;
     index_ += index_plus_;
-  }  
+  }
 }
 
 ////////////////////////////////////////////////////////////////////////////////
 inline unsigned
-LineIterator::getRowIndex () const
+LineIterator::getRowIndex() const
 {
   return y_;
 }
 
 ////////////////////////////////////////////////////////////////////////////////
 inline unsigned
-LineIterator::getColumnIndex () const
+LineIterator::getColumnIndex() const
 {
   return x_;
 }
 
 ////////////////////////////////////////////////////////////////////////////////
 inline bool
-LineIterator::isValid () const
+LineIterator::isValid() const
 {
   return (x_ != x_end_ || y_ != y_end_);
 }
 
 ////////////////////////////////////////////////////////////////////////////////
 inline void
-LineIterator::reset ()
+LineIterator::reset()
 {
   x_ = x_start_;
   y_ = y_start_;
diff --git a/geometry/include/pcl/geometry/mesh_base.h b/geometry/include/pcl/geometry/mesh_base.h
index 1f96b121..32223baa 100644
--- a/geometry/include/pcl/geometry/mesh_base.h
+++ b/geometry/include/pcl/geometry/mesh_base.h
@@ -40,30 +40,27 @@
 
 #pragma once
 
-#include <pcl/geometry/boost.h>
-#include <pcl/geometry/eigen.h>
 #include <pcl/geometry/mesh_circulators.h>
-#include <pcl/geometry/mesh_indices.h>
 #include <pcl/geometry/mesh_elements.h>
+#include <pcl/geometry/mesh_indices.h>
 #include <pcl/geometry/mesh_traits.h>
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
 #include <pcl/point_cloud.h>
 
-#include <vector>
 #include <type_traits>
 
+#include <vector>
+
 ////////////////////////////////////////////////////////////////////////////////
 // Global variables used during testing
 ////////////////////////////////////////////////////////////////////////////////
 
 #ifdef PCL_GEOMETRY_MESH_BASE_TEST_DELETE_FACE_MANIFOLD_2
-namespace pcl
-{
-  namespace geometry
-  {
-    bool g_pcl_geometry_mesh_base_test_delete_face_manifold_2_success;
-  } // End namespace geometry
+namespace pcl {
+namespace geometry {
+bool g_pcl_geometry_mesh_base_test_delete_face_manifold_2_success;
+} // End namespace geometry
 } // End namespace pcl
 #endif
 
@@ -71,2045 +68,2131 @@ namespace pcl
 // Forward declarations
 ////////////////////////////////////////////////////////////////////////////////
 
-namespace pcl
-{
-  namespace geometry
-  {
-    template <class MeshT>
-    class MeshIO;
-  } // End namespace geometry
+namespace pcl {
+namespace geometry {
+template <class MeshT>
+class MeshIO;
+} // End namespace geometry
 } // End namespace pcl
 
 ////////////////////////////////////////////////////////////////////////////////
 // MeshBase
 ////////////////////////////////////////////////////////////////////////////////
 
-namespace pcl
-{
-  namespace geometry
-  {
-    /** \brief Base class for the half-edge mesh.
-      * \tparam DerivedT Has to implement the method 'addFaceImpl'. Please have a look at pcl::geometry::TriangleMesh, pcl::geometry::QuadMesh and pcl::geometry::PolygonMesh.
-      * \tparam MeshTraitsT Please have a look at pcl::geometry::DefaultMeshTraits.
-      * \tparam MeshTagT Tag describing the type of the mesh, e.g. TriangleMeshTag, QuadMeshTag, PolygonMeshTag.
-      * \author Martin Saelzle
-      * \ingroup geometry
-      * \todo Add documentation
-      */
-    template <class DerivedT, class MeshTraitsT, class MeshTagT>
-    class MeshBase
-    {
-      public:
-
-        using Self = MeshBase <DerivedT, MeshTraitsT, MeshTagT>;
-        using Ptr = shared_ptr<Self>;
-        using ConstPtr = shared_ptr<const Self>;
-
-        using Derived = DerivedT;
-
-        // These have to be defined in the traits class.
-        using VertexData = typename MeshTraitsT::VertexData;
-        using HalfEdgeData = typename MeshTraitsT::HalfEdgeData;
-        using EdgeData = typename MeshTraitsT::EdgeData;
-        using FaceData = typename MeshTraitsT::FaceData;
-        using IsManifold = typename MeshTraitsT::IsManifold;
-
-        // Check if the mesh traits are defined correctly.
-        static_assert (std::is_convertible<IsManifold, bool>::value, "MeshTraitsT::IsManifold is not convertible to bool");
-
-        using MeshTag = MeshTagT;
-
-        // Data
-        using HasVertexData = std::integral_constant <bool, !std::is_same <VertexData  , pcl::geometry::NoData>::value>;
-        using HasHalfEdgeData = std::integral_constant <bool, !std::is_same <HalfEdgeData, pcl::geometry::NoData>::value>;
-        using HasEdgeData = std::integral_constant <bool, !std::is_same <EdgeData    , pcl::geometry::NoData>::value>;
-        using HasFaceData = std::integral_constant <bool, !std::is_same <FaceData    , pcl::geometry::NoData>::value>;
-
-        using VertexDataCloud = pcl::PointCloud<VertexData>;
-        using HalfEdgeDataCloud = pcl::PointCloud<HalfEdgeData>;
-        using EdgeDataCloud = pcl::PointCloud<EdgeData>;
-        using FaceDataCloud = pcl::PointCloud<FaceData>;
-
-        // Indices
-        using VertexIndex = pcl::geometry::VertexIndex;
-        using HalfEdgeIndex = pcl::geometry::HalfEdgeIndex;
-        using EdgeIndex = pcl::geometry::EdgeIndex;
-        using FaceIndex = pcl::geometry::FaceIndex;
-
-        using VertexIndices = std::vector<VertexIndex>;
-        using HalfEdgeIndices = std::vector<HalfEdgeIndex>;
-        using EdgeIndices = std::vector<EdgeIndex>;
-        using FaceIndices = std::vector<FaceIndex>;
-
-        // Circulators
-        using VertexAroundVertexCirculator = pcl::geometry::VertexAroundVertexCirculator<const Self>;
-        using OutgoingHalfEdgeAroundVertexCirculator = pcl::geometry::OutgoingHalfEdgeAroundVertexCirculator<const Self>;
-        using IncomingHalfEdgeAroundVertexCirculator = pcl::geometry::IncomingHalfEdgeAroundVertexCirculator<const Self>;
-        using FaceAroundVertexCirculator = pcl::geometry::FaceAroundVertexCirculator<const Self>;
-        using VertexAroundFaceCirculator = pcl::geometry::VertexAroundFaceCirculator<const Self>;
-        using InnerHalfEdgeAroundFaceCirculator = pcl::geometry::InnerHalfEdgeAroundFaceCirculator<const Self>;
-        using OuterHalfEdgeAroundFaceCirculator = pcl::geometry::OuterHalfEdgeAroundFaceCirculator<const Self>;
-        using FaceAroundFaceCirculator = pcl::geometry::FaceAroundFaceCirculator<const Self>;
-
-        /** \brief Constructor. */
-        MeshBase ()
-          : vertex_data_cloud_ (),
-            half_edge_data_cloud_ (),
-            edge_data_cloud_ (),
-            face_data_cloud_ ()
-        {
-        }
+namespace pcl {
+namespace geometry {
+/**
+ * \brief Base class for the half-edge mesh.
+ * \tparam DerivedT Has to implement the method 'addFaceImpl'. Please have a look at
+ * pcl::geometry::TriangleMesh, pcl::geometry::QuadMesh and pcl::geometry::PolygonMesh.
+ * \tparam MeshTraitsT Please have a look at pcl::geometry::DefaultMeshTraits.
+ * \tparam MeshTagT Tag describing the type of the mesh, e.g. TriangleMeshTag,
+ * QuadMeshTag, PolygonMeshTag.
+ * \author Martin Saelzle
+ * \ingroup geometry
+ * \todo Add documentation
+ */
+template <class DerivedT, class MeshTraitsT, class MeshTagT>
+class MeshBase {
+public:
+  using Self = MeshBase<DerivedT, MeshTraitsT, MeshTagT>;
+  using Ptr = shared_ptr<Self>;
+  using ConstPtr = shared_ptr<const Self>;
+
+  using Derived = DerivedT;
+
+  // These have to be defined in the traits class.
+  using VertexData = typename MeshTraitsT::VertexData;
+  using HalfEdgeData = typename MeshTraitsT::HalfEdgeData;
+  using EdgeData = typename MeshTraitsT::EdgeData;
+  using FaceData = typename MeshTraitsT::FaceData;
+  using IsManifold = typename MeshTraitsT::IsManifold;
+
+  // Check if the mesh traits are defined correctly.
+  static_assert(std::is_convertible<IsManifold, bool>::value,
+                "MeshTraitsT::IsManifold is not convertible to bool");
+
+  using MeshTag = MeshTagT;
+
+  // Data
+  using HasVertexData =
+      std::integral_constant<bool,
+                             !std::is_same<VertexData, pcl::geometry::NoData>::value>;
+  using HasHalfEdgeData =
+      std::integral_constant<bool,
+                             !std::is_same<HalfEdgeData, pcl::geometry::NoData>::value>;
+  using HasEdgeData =
+      std::integral_constant<bool,
+                             !std::is_same<EdgeData, pcl::geometry::NoData>::value>;
+  using HasFaceData =
+      std::integral_constant<bool,
+                             !std::is_same<FaceData, pcl::geometry::NoData>::value>;
+
+  using VertexDataCloud = pcl::PointCloud<VertexData>;
+  using HalfEdgeDataCloud = pcl::PointCloud<HalfEdgeData>;
+  using EdgeDataCloud = pcl::PointCloud<EdgeData>;
+  using FaceDataCloud = pcl::PointCloud<FaceData>;
+
+  // Indices
+  using VertexIndex = pcl::geometry::VertexIndex;
+  using HalfEdgeIndex = pcl::geometry::HalfEdgeIndex;
+  using EdgeIndex = pcl::geometry::EdgeIndex;
+  using FaceIndex = pcl::geometry::FaceIndex;
+
+  using VertexIndices = std::vector<VertexIndex>;
+  using HalfEdgeIndices = std::vector<HalfEdgeIndex>;
+  using EdgeIndices = std::vector<EdgeIndex>;
+  using FaceIndices = std::vector<FaceIndex>;
+
+  // Circulators
+  using VertexAroundVertexCirculator =
+      pcl::geometry::VertexAroundVertexCirculator<const Self>;
+  using OutgoingHalfEdgeAroundVertexCirculator =
+      pcl::geometry::OutgoingHalfEdgeAroundVertexCirculator<const Self>;
+  using IncomingHalfEdgeAroundVertexCirculator =
+      pcl::geometry::IncomingHalfEdgeAroundVertexCirculator<const Self>;
+  using FaceAroundVertexCirculator =
+      pcl::geometry::FaceAroundVertexCirculator<const Self>;
+  using VertexAroundFaceCirculator =
+      pcl::geometry::VertexAroundFaceCirculator<const Self>;
+  using InnerHalfEdgeAroundFaceCirculator =
+      pcl::geometry::InnerHalfEdgeAroundFaceCirculator<const Self>;
+  using OuterHalfEdgeAroundFaceCirculator =
+      pcl::geometry::OuterHalfEdgeAroundFaceCirculator<const Self>;
+  using FaceAroundFaceCirculator = pcl::geometry::FaceAroundFaceCirculator<const Self>;
+
+  /** \brief Constructor. */
+  MeshBase()
+  : vertex_data_cloud_()
+  , half_edge_data_cloud_()
+  , edge_data_cloud_()
+  , face_data_cloud_()
+  {}
+
+  ////////////////////////////////////////////////////////////////////////
+  // addVertex / addFace / deleteVertex / deleteEdge / deleteFace / cleanUp
+  ////////////////////////////////////////////////////////////////////////
+
+  /**
+   * \brief Add a vertex to the mesh.
+   * \param[in] vertex_data Data that is stored in the vertex. This is only added if the
+   * mesh has data associated with the vertices.
+   * \return Index to the new vertex.
+   */
+  inline VertexIndex
+  addVertex(const VertexData& vertex_data = VertexData())
+  {
+    vertices_.push_back(Vertex());
+    this->addData(vertex_data_cloud_, vertex_data, HasVertexData());
+    return (VertexIndex(static_cast<int>(this->sizeVertices() - 1)));
+  }
+
+  /**
+   * \brief Add a face to the mesh. Data is only added if it is associated with the
+   * elements. The last vertex is connected with the first one.
+   * \param[in] vertices Indices to the vertices of the new face.
+   * \param[in] face_data      Data that is set for the face.
+   * \param[in] half_edge_data Data that is set for all added half-edges.
+   * \param[in] edge_data      Data that is set for all added edges.
+   * \return Index to the new face. Failure is signaled by returning an invalid face
+   * index.
+   * \warning The vertices must be valid and unique (each vertex may be contained
+   * only once). Not complying with this requirement results in undefined behavior!
+   */
+  inline FaceIndex
+  addFace(const VertexIndices& vertices,
+          const FaceData& face_data = FaceData(),
+          const EdgeData& edge_data = EdgeData(),
+          const HalfEdgeData& half_edge_data = HalfEdgeData())
+  {
+    // NOTE: The derived class has to implement addFaceImpl. If needed it can use the
+    // general method addFaceImplBase.
+    return (static_cast<Derived*>(this)->addFaceImpl(
+        vertices, face_data, edge_data, half_edge_data));
+  }
+
+  /**
+   * \brief Mark the given vertex and all connected half-edges and faces as deleted.
+   * \note Call cleanUp () to finally delete all mesh-elements.
+   */
+  void
+  deleteVertex(const VertexIndex& idx_vertex)
+  {
+    assert(this->isValid(idx_vertex));
+    if (this->isDeleted(idx_vertex))
+      return;
+
+    delete_faces_vertex_.clear();
+    FaceAroundVertexCirculator circ = this->getFaceAroundVertexCirculator(idx_vertex);
+    const FaceAroundVertexCirculator circ_end = circ;
+    do {
+      if (circ.getTargetIndex().isValid()) // Check for boundary.
+      {
+        delete_faces_vertex_.push_back(circ.getTargetIndex());
+      }
+    } while (++circ != circ_end);
+
+    for (FaceIndices::const_iterator it = delete_faces_vertex_.begin();
+         it != delete_faces_vertex_.end();
+         ++it) {
+      this->deleteFace(*it);
+    }
+  }
+
+  /**
+   * \brief Mark the given half-edge, the opposite half-edge and the associated faces
+   * as deleted.
+   * \note Call cleanUp () to finally delete all mesh-elements.
+   */
+  void
+  deleteEdge(const HalfEdgeIndex& idx_he)
+  {
+    assert(this->isValid(idx_he));
+    if (this->isDeleted(idx_he))
+      return;
+
+    HalfEdgeIndex opposite = this->getOppositeHalfEdgeIndex(idx_he);
+
+    if (this->isBoundary(idx_he))
+      this->markDeleted(idx_he);
+    else
+      this->deleteFace(this->getFaceIndex(idx_he));
+    if (this->isBoundary(opposite))
+      this->markDeleted(opposite);
+    else
+      this->deleteFace(this->getFaceIndex(opposite));
+  }
+
+  /**
+   * \brief Mark the given edge (both half-edges) and the associated faces as deleted.
+   * \note Call cleanUp () to finally delete all mesh-elements.
+   */
+  inline void
+  deleteEdge(const EdgeIndex& idx_edge)
+  {
+    assert(this->isValid(idx_edge));
+    this->deleteEdge(pcl::geometry::toHalfEdgeIndex(idx_edge));
+    assert(this->isDeleted(
+        pcl::geometry::toHalfEdgeIndex(idx_edge, false))); // Bug in this class!
+  }
+
+  /**
+   * \brief Mark the given face as deleted. More faces are deleted if the manifold mesh
+   * would become non-manifold.
+   * \note Call cleanUp () to finally delete all mesh-elements.
+   */
+  inline void
+  deleteFace(const FaceIndex& idx_face)
+  {
+    assert(this->isValid(idx_face));
+    if (this->isDeleted(idx_face))
+      return;
+
+    this->deleteFace(idx_face, IsManifold());
+  }
+
+  /**
+   * \brief Removes all mesh elements and data that are marked as deleted.
+   * \note This removes all isolated vertices as well.
+   */
+  void
+  cleanUp()
+  {
+    // Copy the non-deleted mesh elements and store the index to their new position
+    const VertexIndices new_vertex_indices =
+        this->remove<Vertices, VertexDataCloud, VertexIndices, HasVertexData>(
+            vertices_, vertex_data_cloud_);
+    const HalfEdgeIndices new_half_edge_indices =
+        this->remove<HalfEdges, HalfEdgeDataCloud, HalfEdgeIndices, HasHalfEdgeData>(
+            half_edges_, half_edge_data_cloud_);
+    const FaceIndices new_face_indices =
+        this->remove<Faces, FaceDataCloud, FaceIndices, HasFaceData>(faces_,
+                                                                     face_data_cloud_);
+
+    // Remove deleted edge data
+    if (HasEdgeData::value) {
+      auto it_ed_old = edge_data_cloud_.begin();
+      auto it_ed_new = edge_data_cloud_.begin();
+
+      for (auto it_ind = new_half_edge_indices.cbegin(),
+                it_ind_end = new_half_edge_indices.cend();
+           it_ind != it_ind_end;
+           it_ind += 2, ++it_ed_old) {
+        if (it_ind->isValid()) {
+          *it_ed_new++ = *it_ed_old;
+        }
+      }
+      edge_data_cloud_.resize(this->sizeEdges());
+    }
+
+    // Adjust the indices
+    for (VertexIterator it = vertices_.begin(); it != vertices_.end(); ++it) {
+      if (it->idx_outgoing_half_edge_.isValid()) {
+        it->idx_outgoing_half_edge_ =
+            new_half_edge_indices[it->idx_outgoing_half_edge_.get()];
+      }
+    }
+
+    for (HalfEdgeIterator it = half_edges_.begin(); it != half_edges_.end(); ++it) {
+      it->idx_terminating_vertex_ =
+          new_vertex_indices[it->idx_terminating_vertex_.get()];
+      it->idx_next_half_edge_ = new_half_edge_indices[it->idx_next_half_edge_.get()];
+      it->idx_prev_half_edge_ = new_half_edge_indices[it->idx_prev_half_edge_.get()];
+      if (it->idx_face_.isValid()) {
+        it->idx_face_ = new_face_indices[it->idx_face_.get()];
+      }
+    }
+
+    for (FaceIterator it = faces_.begin(); it != faces_.end(); ++it) {
+      it->idx_inner_half_edge_ = new_half_edge_indices[it->idx_inner_half_edge_.get()];
+    }
+  }
+
+  ////////////////////////////////////////////////////////////////////////
+  // Vertex connectivity
+  ////////////////////////////////////////////////////////////////////////
+
+  /** \brief Get the outgoing half-edge index to a given vertex. */
+  inline HalfEdgeIndex
+  getOutgoingHalfEdgeIndex(const VertexIndex& idx_vertex) const
+  {
+    assert(this->isValid(idx_vertex));
+    return (this->getVertex(idx_vertex).idx_outgoing_half_edge_);
+  }
 
-        ////////////////////////////////////////////////////////////////////////
-        // addVertex / addFace / deleteVertex / deleteEdge / deleteFace / cleanUp
-        ////////////////////////////////////////////////////////////////////////
+  /** \brief Get the incoming half-edge index to a given vertex. */
+  inline HalfEdgeIndex
+  getIncomingHalfEdgeIndex(const VertexIndex& idx_vertex) const
+  {
+    assert(this->isValid(idx_vertex));
+    return (this->getOppositeHalfEdgeIndex(this->getOutgoingHalfEdgeIndex(idx_vertex)));
+  }
 
-        /** \brief Add a vertex to the mesh.
-          * \param[in] vertex_data Data that is stored in the vertex. This is only added if the mesh has data associated with the vertices.
-          * \return Index to the new vertex.
-          */
-        inline VertexIndex
-        addVertex (const VertexData& vertex_data=VertexData ())
-        {
-          vertices_.push_back (Vertex ());
-          this->addData (vertex_data_cloud_, vertex_data, HasVertexData ());
-          return (VertexIndex (static_cast <int> (this->sizeVertices () - 1)));
-        }
+  ////////////////////////////////////////////////////////////////////////
+  // Half-edge connectivity
+  ////////////////////////////////////////////////////////////////////////
 
-        /** \brief Add a face to the mesh. Data is only added if it is associated with the elements. The last vertex is connected with the first one.
-          * \param[in] vertices       Indices to the vertices of the new face.
-          * \param[in] face_data      Data that is set for the face.
-          * \param[in] half_edge_data Data that is set for all added half-edges.
-          * \param[in] edge_data      Data that is set for all added edges.
-          * \return Index to the new face. Failure is signaled by returning an invalid face index.
-          * \warning The vertices must be valid and unique (each vertex may be contained only once). Not complying with this requirement results in undefined behavior!
-          */
-        inline FaceIndex
-        addFace (const VertexIndices& vertices,
-                 const FaceData&      face_data      = FaceData (),
-                 const EdgeData&      edge_data      = EdgeData (),
-                 const HalfEdgeData&  half_edge_data = HalfEdgeData ())
-        {
-          // NOTE: The derived class has to implement addFaceImpl. If needed it can use the general method addFaceImplBase.
-          return (static_cast <Derived*> (this)->addFaceImpl (vertices, face_data, edge_data, half_edge_data));
-        }
+  /** \brief Get the terminating vertex index to a given half-edge. */
+  inline VertexIndex
+  getTerminatingVertexIndex(const HalfEdgeIndex& idx_half_edge) const
+  {
+    assert(this->isValid(idx_half_edge));
+    return (this->getHalfEdge(idx_half_edge).idx_terminating_vertex_);
+  }
 
-        /** \brief Mark the given vertex and all connected half-edges and faces as deleted.
-          * \note Call cleanUp () to finally delete all mesh-elements.
-          */
-        void
-        deleteVertex (const VertexIndex& idx_vertex)
-        {
-          assert (this->isValid (idx_vertex));
-          if (this->isDeleted (idx_vertex)) return;
-
-          delete_faces_vertex_.clear ();
-          FaceAroundVertexCirculator       circ     = this->getFaceAroundVertexCirculator (idx_vertex);
-          const FaceAroundVertexCirculator circ_end = circ;
-          do
-          {
-            if (circ.getTargetIndex ().isValid ()) // Check for boundary.
-            {
-              delete_faces_vertex_.push_back (circ.getTargetIndex ());
-            }
-          } while (++circ!=circ_end);
-
-          for (FaceIndices::const_iterator it = delete_faces_vertex_.begin (); it!=delete_faces_vertex_.end (); ++it)
-          {
-            this->deleteFace (*it);
-          }
-        }
+  /** \brief Get the originating vertex index to a given half-edge. */
+  inline VertexIndex
+  getOriginatingVertexIndex(const HalfEdgeIndex& idx_half_edge) const
+  {
+    assert(this->isValid(idx_half_edge));
+    return (
+        this->getTerminatingVertexIndex(this->getOppositeHalfEdgeIndex(idx_half_edge)));
+  }
+
+  /** \brief Get the opposite half-edge index to a given half-edge. */
+  inline HalfEdgeIndex
+  getOppositeHalfEdgeIndex(const HalfEdgeIndex& idx_half_edge) const
+  {
+    assert(this->isValid(idx_half_edge));
+    // Check if the index is even or odd and return the other index.
+    return (HalfEdgeIndex(idx_half_edge.get() & 1 ? idx_half_edge.get() - 1
+                                                  : idx_half_edge.get() + 1));
+  }
+
+  /** \brief Get the next half-edge index to a given half-edge. */
+  inline HalfEdgeIndex
+  getNextHalfEdgeIndex(const HalfEdgeIndex& idx_half_edge) const
+  {
+    assert(this->isValid(idx_half_edge));
+    return (this->getHalfEdge(idx_half_edge).idx_next_half_edge_);
+  }
 
-        /** \brief Mark the given half-edge, the opposite half-edge and the associated faces as deleted.
-          * \note Call cleanUp () to finally delete all mesh-elements.
-          */
-        void
-        deleteEdge (const HalfEdgeIndex& idx_he)
-        {
-          assert (this->isValid (idx_he));
-          if (this->isDeleted (idx_he)) return;
+  /** \brief Get the previous half-edge index to a given half-edge. */
+  inline HalfEdgeIndex
+  getPrevHalfEdgeIndex(const HalfEdgeIndex& idx_half_edge) const
+  {
+    assert(this->isValid(idx_half_edge));
+    return (this->getHalfEdge(idx_half_edge).idx_prev_half_edge_);
+  }
 
-          HalfEdgeIndex opposite = this->getOppositeHalfEdgeIndex (idx_he);
+  /** \brief Get the face index to a given half-edge. */
+  inline FaceIndex
+  getFaceIndex(const HalfEdgeIndex& idx_half_edge) const
+  {
+    assert(this->isValid(idx_half_edge));
+    return (this->getHalfEdge(idx_half_edge).idx_face_);
+  }
 
-          if (this->isBoundary (idx_he))   this->markDeleted (idx_he);
-          else                             this->deleteFace (this->getFaceIndex (idx_he));
-          if (this->isBoundary (opposite)) this->markDeleted (opposite);
-          else                             this->deleteFace (this->getFaceIndex (opposite));
-        }
+  /** \brief Get the face index to a given half-edge. */
+  inline FaceIndex
+  getOppositeFaceIndex(const HalfEdgeIndex& idx_half_edge) const
+  {
+    assert(this->isValid(idx_half_edge));
+    return (this->getFaceIndex(this->getOppositeHalfEdgeIndex(idx_half_edge)));
+  }
 
-        /** \brief Mark the given edge (both half-edges) and the associated faces as deleted.
-          * \note Call cleanUp () to finally delete all mesh-elements.
-          */
-        inline void
-        deleteEdge (const EdgeIndex& idx_edge)
-        {
-          assert (this->isValid (idx_edge));
-          this->deleteEdge (pcl::geometry::toHalfEdgeIndex (idx_edge));
-          assert (this->isDeleted (pcl::geometry::toHalfEdgeIndex (idx_edge, false))); // Bug in this class!
-        }
+  ////////////////////////////////////////////////////////////////////////
+  // Face connectivity
+  ////////////////////////////////////////////////////////////////////////
 
-        /** \brief Mark the given face as deleted. More faces are deleted if the manifold mesh would become non-manifold.
-          * \note Call cleanUp () to finally delete all mesh-elements.
-          */
-        inline void
-        deleteFace (const FaceIndex& idx_face)
-        {
-          assert (this->isValid (idx_face));
-          if (this->isDeleted (idx_face)) return;
+  /** \brief Get the inner half-edge index to a given face. */
+  inline HalfEdgeIndex
+  getInnerHalfEdgeIndex(const FaceIndex& idx_face) const
+  {
+    assert(this->isValid(idx_face));
+    return (this->getFace(idx_face).idx_inner_half_edge_);
+  }
 
-          this->deleteFace (idx_face, IsManifold ());
-        }
+  /** \brief Get the outer half-edge inex to a given face. */
+  inline HalfEdgeIndex
+  getOuterHalfEdgeIndex(const FaceIndex& idx_face) const
+  {
+    assert(this->isValid(idx_face));
+    return (this->getOppositeHalfEdgeIndex(this->getInnerHalfEdgeIndex(idx_face)));
+  }
 
-        /** \brief Removes all mesh elements and data that are marked as deleted.
-          * \note This removes all isolated vertices as well.
-          */
-        void
-        cleanUp ()
-        {
-          // Copy the non-deleted mesh elements and store the index to their new position
-          const VertexIndices new_vertex_indices =
-              this->remove <Vertices, VertexDataCloud, VertexIndices, HasVertexData>
-              (vertices_, vertex_data_cloud_);
-          const HalfEdgeIndices new_half_edge_indices =
-              this->remove <HalfEdges, HalfEdgeDataCloud, HalfEdgeIndices, HasHalfEdgeData>
-              (half_edges_, half_edge_data_cloud_);
-          const FaceIndices new_face_indices =
-              this->remove <Faces, FaceDataCloud, FaceIndices, HasFaceData>
-              (faces_, face_data_cloud_);
-
-          // Remove deleted edge data
-          if (HasEdgeData::value)
-          {
-            auto it_ed_old = edge_data_cloud_.begin ();
-            auto it_ed_new = edge_data_cloud_.begin ();
-
-            for (auto it_ind = new_half_edge_indices.cbegin (), it_ind_end = new_half_edge_indices.cend (); it_ind!=it_ind_end; it_ind+=2, ++it_ed_old)
-            {
-              if (it_ind->isValid ())
-              {
-                *it_ed_new++ = *it_ed_old;
-              }
-            }
-            edge_data_cloud_.resize (this->sizeEdges ());
-          }
-
-          // Adjust the indices
-          for (VertexIterator it = vertices_.begin (); it!=vertices_.end (); ++it)
-          {
-            if (it->idx_outgoing_half_edge_.isValid ())
-            {
-              it->idx_outgoing_half_edge_ = new_half_edge_indices [it->idx_outgoing_half_edge_.get ()];
-            }
-          }
-
-          for (HalfEdgeIterator it = half_edges_.begin (); it!=half_edges_.end (); ++it)
-          {
-            it->idx_terminating_vertex_ = new_vertex_indices    [it->idx_terminating_vertex_.get ()];
-            it->idx_next_half_edge_     = new_half_edge_indices [it->idx_next_half_edge_.get ()];
-            it->idx_prev_half_edge_     = new_half_edge_indices [it->idx_prev_half_edge_.get ()];
-            if (it->idx_face_.isValid ())
-            {
-              it->idx_face_ = new_face_indices [it->idx_face_.get ()];
-            }
-          }
-
-          for (FaceIterator it = faces_.begin (); it!=faces_.end (); ++it)
-          {
-            it->idx_inner_half_edge_ = new_half_edge_indices [it->idx_inner_half_edge_.get ()];
-          }
-        }
+  ////////////////////////////////////////////////////////////////////////
+  // Circulators
+  ////////////////////////////////////////////////////////////////////////
 
-        ////////////////////////////////////////////////////////////////////////
-        // Vertex connectivity
-        ////////////////////////////////////////////////////////////////////////
+  /** \see pcl::geometry::VertexAroundVertexCirculator */
+  inline VertexAroundVertexCirculator
+  getVertexAroundVertexCirculator(const VertexIndex& idx_vertex) const
+  {
+    assert(this->isValid(idx_vertex));
+    return (VertexAroundVertexCirculator(idx_vertex, this));
+  }
 
-        /** \brief Get the outgoing half-edge index to a given vertex. */
-        inline HalfEdgeIndex
-        getOutgoingHalfEdgeIndex (const VertexIndex& idx_vertex) const
-        {
-          assert (this->isValid (idx_vertex));
-          return (this->getVertex (idx_vertex).idx_outgoing_half_edge_);
-        }
+  /** \see pcl::geometry::VertexAroundVertexCirculator */
+  inline VertexAroundVertexCirculator
+  getVertexAroundVertexCirculator(const HalfEdgeIndex& idx_outgoing_half_edge) const
+  {
+    assert(this->isValid(idx_outgoing_half_edge));
+    return (VertexAroundVertexCirculator(idx_outgoing_half_edge, this));
+  }
 
-        /** \brief Get the incoming half-edge index to a given vertex. */
-        inline HalfEdgeIndex
-        getIncomingHalfEdgeIndex (const VertexIndex& idx_vertex) const
-        {
-          assert (this->isValid (idx_vertex));
-          return (this->getOppositeHalfEdgeIndex (this->getOutgoingHalfEdgeIndex (idx_vertex)));
-        }
+  /** \see pcl::geometry::OutgoingHalfEdgeAroundVertexCirculator */
+  inline OutgoingHalfEdgeAroundVertexCirculator
+  getOutgoingHalfEdgeAroundVertexCirculator(const VertexIndex& idx_vertex) const
+  {
+    assert(this->isValid(idx_vertex));
+    return (OutgoingHalfEdgeAroundVertexCirculator(idx_vertex, this));
+  }
+
+  /** \see pcl::geometry::OutgoingHalfEdgeAroundVertexCirculator */
+  inline OutgoingHalfEdgeAroundVertexCirculator
+  getOutgoingHalfEdgeAroundVertexCirculator(
+      const HalfEdgeIndex& idx_outgoing_half_edge) const
+  {
+    assert(this->isValid(idx_outgoing_half_edge));
+    return (OutgoingHalfEdgeAroundVertexCirculator(idx_outgoing_half_edge, this));
+  }
 
-        ////////////////////////////////////////////////////////////////////////
-        // Half-edge connectivity
-        ////////////////////////////////////////////////////////////////////////
+  /** \see pcl::geometry::IncomingHalfEdgeAroundVertexCirculator */
+  inline IncomingHalfEdgeAroundVertexCirculator
+  getIncomingHalfEdgeAroundVertexCirculator(const VertexIndex& idx_vertex) const
+  {
+    assert(this->isValid(idx_vertex));
+    return (IncomingHalfEdgeAroundVertexCirculator(idx_vertex, this));
+  }
+
+  /** \see pcl::geometry::IncomingHalfEdgeAroundVertexCirculator */
+  inline IncomingHalfEdgeAroundVertexCirculator
+  getIncomingHalfEdgeAroundVertexCirculator(
+      const HalfEdgeIndex& idx_incoming_half_edge) const
+  {
+    assert(this->isValid(idx_incoming_half_edge));
+    return (IncomingHalfEdgeAroundVertexCirculator(idx_incoming_half_edge, this));
+  }
 
-        /** \brief Get the terminating vertex index to a given half-edge. */
-        inline VertexIndex
-        getTerminatingVertexIndex (const HalfEdgeIndex& idx_half_edge) const
-        {
-          assert (this->isValid (idx_half_edge));
-          return (this->getHalfEdge (idx_half_edge).idx_terminating_vertex_);
-        }
+  /** \see pcl::geometry::FaceAroundVertexCirculator */
+  inline FaceAroundVertexCirculator
+  getFaceAroundVertexCirculator(const VertexIndex& idx_vertex) const
+  {
+    assert(this->isValid(idx_vertex));
+    return (FaceAroundVertexCirculator(idx_vertex, this));
+  }
 
-        /** \brief Get the originating vertex index to a given half-edge. */
-        inline VertexIndex
-        getOriginatingVertexIndex (const HalfEdgeIndex& idx_half_edge) const
-        {
-          assert (this->isValid (idx_half_edge));
-          return (this->getTerminatingVertexIndex (this->getOppositeHalfEdgeIndex (idx_half_edge)));
-        }
+  /** \see pcl::geometry::FaceAroundVertexCirculator */
+  inline FaceAroundVertexCirculator
+  getFaceAroundVertexCirculator(const HalfEdgeIndex& idx_outgoing_half_edge) const
+  {
+    assert(this->isValid(idx_outgoing_half_edge));
+    return (FaceAroundVertexCirculator(idx_outgoing_half_edge, this));
+  }
 
-        /** \brief Get the opposite half-edge index to a given half-edge. */
-        inline HalfEdgeIndex
-        getOppositeHalfEdgeIndex (const HalfEdgeIndex& idx_half_edge) const
-        {
-          assert (this->isValid (idx_half_edge));
-          // Check if the index is even or odd and return the other index.
-          return (HalfEdgeIndex (idx_half_edge.get () & 1 ? idx_half_edge.get () - 1 : idx_half_edge.get () + 1));
-        }
+  /** \see pcl::geometry::VertexAroundFaceCirculator */
+  inline VertexAroundFaceCirculator
+  getVertexAroundFaceCirculator(const FaceIndex& idx_face) const
+  {
+    assert(this->isValid(idx_face));
+    return (VertexAroundFaceCirculator(idx_face, this));
+  }
 
-        /** \brief Get the next half-edge index to a given half-edge. */
-        inline HalfEdgeIndex
-        getNextHalfEdgeIndex (const HalfEdgeIndex& idx_half_edge) const
-        {
-          assert (this->isValid (idx_half_edge));
-          return (this->getHalfEdge (idx_half_edge).idx_next_half_edge_);
-        }
+  /** \see pcl::geometry::VertexAroundFaceCirculator */
+  inline VertexAroundFaceCirculator
+  getVertexAroundFaceCirculator(const HalfEdgeIndex& idx_inner_half_edge) const
+  {
+    assert(this->isValid(idx_inner_half_edge));
+    return (VertexAroundFaceCirculator(idx_inner_half_edge, this));
+  }
 
-        /** \brief Get the previous half-edge index to a given half-edge. */
-        inline HalfEdgeIndex
-        getPrevHalfEdgeIndex (const HalfEdgeIndex& idx_half_edge) const
-        {
-          assert (this->isValid (idx_half_edge));
-          return (this->getHalfEdge (idx_half_edge).idx_prev_half_edge_);
-        }
+  /** \see pcl::geometry::InnerHalfEdgeAroundFaceCirculator */
+  inline InnerHalfEdgeAroundFaceCirculator
+  getInnerHalfEdgeAroundFaceCirculator(const FaceIndex& idx_face) const
+  {
+    assert(this->isValid(idx_face));
+    return (InnerHalfEdgeAroundFaceCirculator(idx_face, this));
+  }
 
-        /** \brief Get the face index to a given half-edge. */
-        inline FaceIndex
-        getFaceIndex (const HalfEdgeIndex& idx_half_edge) const
-        {
-          assert (this->isValid (idx_half_edge));
-          return (this->getHalfEdge (idx_half_edge).idx_face_);
-        }
+  /** \see pcl::geometry::InnerHalfEdgeAroundFaceCirculator */
+  inline InnerHalfEdgeAroundFaceCirculator
+  getInnerHalfEdgeAroundFaceCirculator(const HalfEdgeIndex& idx_inner_half_edge) const
+  {
+    assert(this->isValid(idx_inner_half_edge));
+    return (InnerHalfEdgeAroundFaceCirculator(idx_inner_half_edge, this));
+  }
 
-        /** \brief Get the face index to a given half-edge. */
-        inline FaceIndex
-        getOppositeFaceIndex (const HalfEdgeIndex& idx_half_edge) const
-        {
-          assert (this->isValid (idx_half_edge));
-          return (this->getFaceIndex (this->getOppositeHalfEdgeIndex (idx_half_edge)));
-        }
+  /** \see pcl::geometry::OuterHalfEdgeAroundFaceCirculator */
+  inline OuterHalfEdgeAroundFaceCirculator
+  getOuterHalfEdgeAroundFaceCirculator(const FaceIndex& idx_face) const
+  {
+    assert(this->isValid(idx_face));
+    return (OuterHalfEdgeAroundFaceCirculator(idx_face, this));
+  }
 
-        ////////////////////////////////////////////////////////////////////////
-        // Face connectivity
-        ////////////////////////////////////////////////////////////////////////
+  /** \see pcl::geometry::OuterHalfEdgeAroundFaceCirculator */
+  inline OuterHalfEdgeAroundFaceCirculator
+  getOuterHalfEdgeAroundFaceCirculator(const HalfEdgeIndex& idx_inner_half_edge) const
+  {
+    assert(this->isValid(idx_inner_half_edge));
+    return (OuterHalfEdgeAroundFaceCirculator(idx_inner_half_edge, this));
+  }
 
-        /** \brief Get the inner half-edge index to a given face. */
-        inline HalfEdgeIndex
-        getInnerHalfEdgeIndex (const FaceIndex& idx_face) const
-        {
-          assert (this->isValid (idx_face));
-          return (this->getFace (idx_face).idx_inner_half_edge_);
-        }
+  /** \see pcl::geometry::FaceAroundFaceCirculator */
+  inline FaceAroundFaceCirculator
+  getFaceAroundFaceCirculator(const FaceIndex& idx_face) const
+  {
+    assert(this->isValid(idx_face));
+    return (FaceAroundFaceCirculator(idx_face, this));
+  }
 
-        /** \brief Get the outer half-edge inex to a given face. */
-        inline HalfEdgeIndex
-        getOuterHalfEdgeIndex (const FaceIndex& idx_face) const
-        {
-          assert (this->isValid (idx_face));
-          return (this->getOppositeHalfEdgeIndex (this->getInnerHalfEdgeIndex (idx_face)));
-        }
+  /** \see pcl::geometry::FaceAroundFaceCirculator */
+  inline FaceAroundFaceCirculator
+  getFaceAroundFaceCirculator(const HalfEdgeIndex& idx_inner_half_edge) const
+  {
+    assert(this->isValid(idx_inner_half_edge));
+    return (FaceAroundFaceCirculator(idx_inner_half_edge, this));
+  }
 
-        ////////////////////////////////////////////////////////////////////////
-        // Circulators
-        ////////////////////////////////////////////////////////////////////////
+  //////////////////////////////////////////////////////////////////////////
+  // isEqualTopology
+  //////////////////////////////////////////////////////////////////////////
 
-        /** \see pcl::geometry::VertexAroundVertexCirculator */
-        inline VertexAroundVertexCirculator
-        getVertexAroundVertexCirculator (const VertexIndex& idx_vertex) const
-        {
-          assert (this->isValid (idx_vertex));
-          return (VertexAroundVertexCirculator (idx_vertex, this));
-        }
+  /** \brief Check if the other mesh has the same topology as this mesh. */
+  bool
+  isEqualTopology(const Self& other) const
+  {
+    if (this->sizeVertices() != other.sizeVertices())
+      return (false);
+    if (this->sizeHalfEdges() != other.sizeHalfEdges())
+      return (false);
+    if (this->sizeFaces() != other.sizeFaces())
+      return (false);
+
+    for (std::size_t i = 0; i < this->sizeVertices(); ++i) {
+      if (this->getOutgoingHalfEdgeIndex(VertexIndex(i)) !=
+          other.getOutgoingHalfEdgeIndex(VertexIndex(i)))
+        return (false);
+    }
+
+    for (std::size_t i = 0; i < this->sizeHalfEdges(); ++i) {
+      if (this->getTerminatingVertexIndex(HalfEdgeIndex(i)) !=
+          other.getTerminatingVertexIndex(HalfEdgeIndex(i)))
+        return (false);
+
+      if (this->getNextHalfEdgeIndex(HalfEdgeIndex(i)) !=
+          other.getNextHalfEdgeIndex(HalfEdgeIndex(i)))
+        return (false);
+
+      if (this->getPrevHalfEdgeIndex(HalfEdgeIndex(i)) !=
+          other.getPrevHalfEdgeIndex(HalfEdgeIndex(i)))
+        return (false);
+
+      if (this->getFaceIndex(HalfEdgeIndex(i)) != other.getFaceIndex(HalfEdgeIndex(i)))
+        return (false);
+    }
+
+    for (std::size_t i = 0; i < this->sizeFaces(); ++i) {
+      if (this->getInnerHalfEdgeIndex(FaceIndex(i)) !=
+          other.getInnerHalfEdgeIndex(FaceIndex(i)))
+        return (false);
+    }
+
+    return (true);
+  }
+
+  ////////////////////////////////////////////////////////////////////////
+  // isValid
+  ////////////////////////////////////////////////////////////////////////
+
+  /** \brief Check if the given vertex index is a valid index into the mesh. */
+  inline bool
+  isValid(const VertexIndex& idx_vertex) const
+  {
+    return (idx_vertex >= VertexIndex(0) &&
+            idx_vertex < VertexIndex(int(vertices_.size())));
+  }
 
-        /** \see pcl::geometry::VertexAroundVertexCirculator */
-        inline VertexAroundVertexCirculator
-        getVertexAroundVertexCirculator (const HalfEdgeIndex& idx_outgoing_half_edge) const
-        {
-          assert (this->isValid (idx_outgoing_half_edge));
-          return (VertexAroundVertexCirculator (idx_outgoing_half_edge, this));
-        }
+  /** \brief Check if the given half-edge index is a valid index into the mesh.  */
+  inline bool
+  isValid(const HalfEdgeIndex& idx_he) const
+  {
+    return (idx_he >= HalfEdgeIndex(0) && idx_he < HalfEdgeIndex(half_edges_.size()));
+  }
 
-        /** \see pcl::geometry::OutgoingHalfEdgeAroundVertexCirculator */
-        inline OutgoingHalfEdgeAroundVertexCirculator
-        getOutgoingHalfEdgeAroundVertexCirculator (const VertexIndex& idx_vertex) const
-        {
-          assert (this->isValid (idx_vertex));
-          return (OutgoingHalfEdgeAroundVertexCirculator (idx_vertex, this));
-        }
+  /** \brief Check if the given edge index is a valid index into the mesh. */
+  inline bool
+  isValid(const EdgeIndex& idx_edge) const
+  {
+    return (idx_edge >= EdgeIndex(0) && idx_edge < EdgeIndex(half_edges_.size() / 2));
+  }
 
-        /** \see pcl::geometry::OutgoingHalfEdgeAroundVertexCirculator */
-        inline OutgoingHalfEdgeAroundVertexCirculator
-        getOutgoingHalfEdgeAroundVertexCirculator (const HalfEdgeIndex& idx_outgoing_half_edge) const
-        {
-          assert (this->isValid (idx_outgoing_half_edge));
-          return (OutgoingHalfEdgeAroundVertexCirculator (idx_outgoing_half_edge, this));
-        }
+  /** \brief Check if the given face index is a valid index into the mesh.  */
+  inline bool
+  isValid(const FaceIndex& idx_face) const
+  {
+    return (idx_face >= FaceIndex(0) && idx_face < FaceIndex(faces_.size()));
+  }
 
-        /** \see pcl::geometry::IncomingHalfEdgeAroundVertexCirculator */
-        inline IncomingHalfEdgeAroundVertexCirculator
-        getIncomingHalfEdgeAroundVertexCirculator (const VertexIndex& idx_vertex) const
-        {
-          assert (this->isValid (idx_vertex));
-          return (IncomingHalfEdgeAroundVertexCirculator (idx_vertex, this));
-        }
+  ////////////////////////////////////////////////////////////////////////
+  // isDeleted
+  ////////////////////////////////////////////////////////////////////////
 
-        /** \see pcl::geometry::IncomingHalfEdgeAroundVertexCirculator */
-        inline IncomingHalfEdgeAroundVertexCirculator
-        getIncomingHalfEdgeAroundVertexCirculator (const HalfEdgeIndex& idx_incoming_half_edge) const
-        {
-          assert (this->isValid (idx_incoming_half_edge));
-          return (IncomingHalfEdgeAroundVertexCirculator (idx_incoming_half_edge, this));
-        }
+  /** \brief Check if the given vertex is marked as deleted. */
+  inline bool
+  isDeleted(const VertexIndex& idx_vertex) const
+  {
+    assert(this->isValid(idx_vertex));
+    return (!this->getOutgoingHalfEdgeIndex(idx_vertex).isValid());
+  }
 
-        /** \see pcl::geometry::FaceAroundVertexCirculator */
-        inline FaceAroundVertexCirculator
-        getFaceAroundVertexCirculator (const VertexIndex& idx_vertex) const
-        {
-          assert (this->isValid (idx_vertex));
-          return (FaceAroundVertexCirculator (idx_vertex, this));
-        }
+  /** \brief Check if the given half-edge is marked as deleted. */
+  inline bool
+  isDeleted(const HalfEdgeIndex& idx_he) const
+  {
+    assert(this->isValid(idx_he));
+    return (!this->getTerminatingVertexIndex(idx_he).isValid());
+  }
+
+  /** \brief Check if the given edge (any of the two half-edges) is marked as deleted.
+   */
+  inline bool
+  isDeleted(const EdgeIndex& idx_edge) const
+  {
+    assert(this->isValid(idx_edge));
+    return (this->isDeleted(pcl::geometry::toHalfEdgeIndex(idx_edge, true)) ||
+            this->isDeleted(pcl::geometry::toHalfEdgeIndex(idx_edge, false)));
+  }
+
+  /** \brief Check if the given face is marked as deleted. */
+  inline bool
+  isDeleted(const FaceIndex& idx_face) const
+  {
+    assert(this->isValid(idx_face));
+    return (!this->getInnerHalfEdgeIndex(idx_face).isValid());
+  }
 
-        /** \see pcl::geometry::FaceAroundVertexCirculator */
-        inline FaceAroundVertexCirculator
-        getFaceAroundVertexCirculator (const HalfEdgeIndex& idx_outgoing_half_edge) const
-        {
-          assert (this->isValid (idx_outgoing_half_edge));
-          return (FaceAroundVertexCirculator (idx_outgoing_half_edge, this));
-        }
+  ////////////////////////////////////////////////////////////////////////
+  // isIsolated
+  ////////////////////////////////////////////////////////////////////////
 
-        /** \see pcl::geometry::VertexAroundFaceCirculator */
-        inline VertexAroundFaceCirculator
-        getVertexAroundFaceCirculator (const FaceIndex& idx_face) const
-        {
-          assert (this->isValid (idx_face));
-          return (VertexAroundFaceCirculator (idx_face, this));
-        }
+  /** \brief Check if the given vertex is isolated (not connected to other elements). */
+  inline bool
+  isIsolated(const VertexIndex& idx_vertex) const
+  {
+    assert(this->isValid(idx_vertex));
+    return (!this->getOutgoingHalfEdgeIndex(idx_vertex).isValid());
+  }
+
+  ////////////////////////////////////////////////////////////////////////
+  // isBoundary
+  ////////////////////////////////////////////////////////////////////////
+
+  /** \brief Check if the given vertex lies on the boundary. Isolated vertices are
+   * considered to be on the boundary. */
+  inline bool
+  isBoundary(const VertexIndex& idx_vertex) const
+  {
+    assert(this->isValid(idx_vertex));
+    if (this->isIsolated(idx_vertex))
+      return (true);
+    return (this->isBoundary(this->getOutgoingHalfEdgeIndex(idx_vertex)));
+  }
+
+  /** \brief Check if the given half-edge lies on the bounddary. */
+  inline bool
+  isBoundary(const HalfEdgeIndex& idx_he) const
+  {
+    assert(this->isValid(idx_he));
+    return (!this->getFaceIndex(idx_he).isValid());
+  }
+
+  /** \brief Check if the given edge lies on the boundary (any of the two half-edges
+   * lies on the boundary. */
+  inline bool
+  isBoundary(const EdgeIndex& idx_edge) const
+  {
+    assert(this->isValid(idx_edge));
+    const HalfEdgeIndex& idx = pcl::geometry::toHalfEdgeIndex(idx_edge);
+    return (this->isBoundary(idx) ||
+            this->isBoundary(this->getOppositeHalfEdgeIndex(idx)));
+  }
+
+  /**
+   * \brief Check if the given face lies on the boundary. There are two versions of
+   * this method, selected by the template parameter.
+   * \tparam CheckVerticesT Check if any vertex lies on the boundary (true) or
+   * check if any edge lies on the boundary (false).
+   */
+  template <bool CheckVerticesT>
+  inline bool
+  isBoundary(const FaceIndex& idx_face) const
+  {
+    assert(this->isValid(idx_face));
+    return (this->isBoundary(idx_face, std::integral_constant<bool, CheckVerticesT>()));
+  }
+
+  /** \brief Check if the given face lies on the boundary. This method uses isBoundary
+   * \c true which checks if any vertex lies on the boundary. */
+  inline bool
+  isBoundary(const FaceIndex& idx_face) const
+  {
+    assert(this->isValid(idx_face));
+    return (this->isBoundary(idx_face, std::true_type()));
+  }
 
-        /** \see pcl::geometry::VertexAroundFaceCirculator */
-        inline VertexAroundFaceCirculator
-        getVertexAroundFaceCirculator (const HalfEdgeIndex& idx_inner_half_edge) const
-        {
-          assert (this->isValid (idx_inner_half_edge));
-          return (VertexAroundFaceCirculator (idx_inner_half_edge, this));
-        }
+  ////////////////////////////////////////////////////////////////////////
+  // isManifold
+  ////////////////////////////////////////////////////////////////////////
 
-        /** \see pcl::geometry::InnerHalfEdgeAroundFaceCirculator */
-        inline InnerHalfEdgeAroundFaceCirculator
-        getInnerHalfEdgeAroundFaceCirculator (const FaceIndex& idx_face) const
-        {
-          assert (this->isValid (idx_face));
-          return (InnerHalfEdgeAroundFaceCirculator (idx_face, this));
-        }
+  /** \brief Check if the given vertex is manifold. Isolated vertices are manifold. */
+  inline bool
+  isManifold(const VertexIndex& idx_vertex) const
+  {
+    assert(this->isValid(idx_vertex));
+    if (this->isIsolated(idx_vertex))
+      return (true);
+    return (this->isManifold(idx_vertex, IsManifold()));
+  }
+
+  /** \brief Check if the mesh is manifold. */
+  inline bool
+  isManifold() const
+  {
+    return (this->isManifold(IsManifold()));
+  }
 
-        /** \see pcl::geometry::InnerHalfEdgeAroundFaceCirculator */
-        inline InnerHalfEdgeAroundFaceCirculator
-        getInnerHalfEdgeAroundFaceCirculator (const HalfEdgeIndex& idx_inner_half_edge) const
-        {
-          assert (this->isValid (idx_inner_half_edge));
-          return (InnerHalfEdgeAroundFaceCirculator (idx_inner_half_edge, this));
-        }
+  ////////////////////////////////////////////////////////////////////////
+  // size
+  ////////////////////////////////////////////////////////////////////////
 
-        /** \see pcl::geometry::OuterHalfEdgeAroundFaceCirculator */
-        inline OuterHalfEdgeAroundFaceCirculator
-        getOuterHalfEdgeAroundFaceCirculator (const FaceIndex& idx_face) const
-        {
-          assert (this->isValid (idx_face));
-          return (OuterHalfEdgeAroundFaceCirculator (idx_face, this));
-        }
+  /** \brief Get the number of the vertices. */
+  inline std::size_t
+  sizeVertices() const
+  {
+    return (vertices_.size());
+  }
 
-        /** \see pcl::geometry::OuterHalfEdgeAroundFaceCirculator */
-        inline OuterHalfEdgeAroundFaceCirculator
-        getOuterHalfEdgeAroundFaceCirculator (const HalfEdgeIndex& idx_inner_half_edge) const
-        {
-          assert (this->isValid (idx_inner_half_edge));
-          return (OuterHalfEdgeAroundFaceCirculator (idx_inner_half_edge, this));
-        }
+  /** \brief Get the number of the half-edges. */
+  inline std::size_t
+  sizeHalfEdges() const
+  {
+    assert(half_edges_.size() % 2 == 0); // This would be a bug in the mesh.
+    return (half_edges_.size());
+  }
 
-        /** \see pcl::geometry::FaceAroundFaceCirculator */
-        inline FaceAroundFaceCirculator
-        getFaceAroundFaceCirculator (const FaceIndex& idx_face) const
-        {
-          assert (this->isValid (idx_face));
-          return (FaceAroundFaceCirculator (idx_face, this));
-        }
+  /** \brief Get the number of the edges. */
+  inline std::size_t
+  sizeEdges() const
+  {
+    assert(half_edges_.size() % 2 == 0); // This would be a bug in the mesh.
+    return (half_edges_.size() / 2);
+  }
 
-        /** \see pcl::geometry::FaceAroundFaceCirculator */
-        inline FaceAroundFaceCirculator
-        getFaceAroundFaceCirculator (const HalfEdgeIndex& idx_inner_half_edge) const
-        {
-          assert (this->isValid (idx_inner_half_edge));
-          return (FaceAroundFaceCirculator (idx_inner_half_edge, this));
-        }
+  /** \brief Get the number of the faces. */
+  inline std::size_t
+  sizeFaces() const
+  {
+    return (faces_.size());
+  }
 
-        //////////////////////////////////////////////////////////////////////////
-        // isEqualTopology
-        //////////////////////////////////////////////////////////////////////////
+  ////////////////////////////////////////////////////////////////////////
+  // empty
+  ////////////////////////////////////////////////////////////////////////
 
-        /** \brief Check if the other mesh has the same topology as this mesh. */
-        bool
-        isEqualTopology (const Self& other) const
-        {
-          if (this->sizeVertices  () != other.sizeVertices  ()) return (false);
-          if (this->sizeHalfEdges () != other.sizeHalfEdges ()) return (false);
-          if (this->sizeFaces     () != other.sizeFaces     ()) return (false);
-
-          for (std::size_t i=0; i<this->sizeVertices (); ++i)
-          {
-            if (this->getOutgoingHalfEdgeIndex (VertexIndex (i)) !=
-                other.getOutgoingHalfEdgeIndex (VertexIndex (i))) return (false);
-          }
-
-          for (std::size_t i=0; i<this->sizeHalfEdges (); ++i)
-          {
-            if (this->getTerminatingVertexIndex (HalfEdgeIndex (i)) !=
-                other.getTerminatingVertexIndex (HalfEdgeIndex (i))) return (false);
-
-            if (this->getNextHalfEdgeIndex (HalfEdgeIndex (i)) !=
-                other.getNextHalfEdgeIndex (HalfEdgeIndex (i))) return (false);
-
-            if (this->getPrevHalfEdgeIndex (HalfEdgeIndex (i)) !=
-                other.getPrevHalfEdgeIndex (HalfEdgeIndex (i))) return (false);
-
-            if (this->getFaceIndex (HalfEdgeIndex (i)) !=
-                other.getFaceIndex (HalfEdgeIndex (i))) return (false);
-          }
-
-          for (std::size_t i=0; i<this->sizeFaces (); ++i)
-          {
-            if (this->getInnerHalfEdgeIndex (FaceIndex (i)) !=
-                other.getInnerHalfEdgeIndex (FaceIndex (i))) return (false);
-          }
-
-          return (true);
-        }
+  /** \brief Check if the mesh is empty. */
+  inline bool
+  empty() const
+  {
+    return (this->emptyVertices() && this->emptyEdges() && this->emptyFaces());
+  }
 
-        ////////////////////////////////////////////////////////////////////////
-        // isValid
-        ////////////////////////////////////////////////////////////////////////
+  /** \brief Check if the vertices are empty. */
+  inline bool
+  emptyVertices() const
+  {
+    return (vertices_.empty());
+  }
 
-        /** \brief Check if the given vertex index is a valid index into the mesh. */
-        inline bool
-        isValid (const VertexIndex& idx_vertex) const
-        {
-          return (idx_vertex >= VertexIndex (0) && idx_vertex < VertexIndex (int (vertices_.size ())));
-        }
+  /** \brief Check if the edges are empty. */
+  inline bool
+  emptyEdges() const
+  {
+    return (half_edges_.empty());
+  }
 
-        /** \brief Check if the given half-edge index is a valid index into the mesh.  */
-        inline bool
-        isValid (const HalfEdgeIndex& idx_he) const
-        {
-          return (idx_he >= HalfEdgeIndex (0) && idx_he < HalfEdgeIndex (half_edges_.size ()));
-        }
+  /** \brief Check if the faces are empty. */
+  inline bool
+  emptyFaces() const
+  {
+    return (faces_.empty());
+  }
 
-        /** \brief Check if the given edge index is a valid index into the mesh. */
-        inline bool
-        isValid (const EdgeIndex& idx_edge) const
-        {
-          return (idx_edge >= EdgeIndex (0) && idx_edge < EdgeIndex (half_edges_.size () / 2));
-        }
+  ////////////////////////////////////////////////////////////////////////
+  // reserve
+  ////////////////////////////////////////////////////////////////////////
 
-        /** \brief Check if the given face index is a valid index into the mesh.  */
-        inline bool
-        isValid (const FaceIndex& idx_face) const
-        {
-          return (idx_face >= FaceIndex (0) && idx_face < FaceIndex (faces_.size ()));
-        }
+  /** \brief Reserve storage space n vertices. */
+  inline void
+  reserveVertices(const std::size_t n)
+  {
+    vertices_.reserve(n);
+    this->reserveData(vertex_data_cloud_, n, HasVertexData());
+  }
+
+  /** \brief Reserve storage space for n edges (2*n storage space is reserved for the
+   * half-edges). */
+  inline void
+  reserveEdges(const std::size_t n)
+  {
+    half_edges_.reserve(2 * n);
+    this->reserveData(half_edge_data_cloud_, 2 * n, HasHalfEdgeData());
+    this->reserveData(edge_data_cloud_, n, HasEdgeData());
+  }
+
+  /** \brief Reserve storage space for n faces. */
+  inline void
+  reserveFaces(const std::size_t n)
+  {
+    faces_.reserve(n);
+    this->reserveData(face_data_cloud_, n, HasFaceData());
+  }
 
-        ////////////////////////////////////////////////////////////////////////
-        // isDeleted
-        ////////////////////////////////////////////////////////////////////////
+  ////////////////////////////////////////////////////////////////////////
+  // resize
+  ////////////////////////////////////////////////////////////////////////
 
-        /** \brief Check if the given vertex is marked as deleted. */
-        inline bool
-        isDeleted (const VertexIndex& idx_vertex) const
-        {
-          assert (this->isValid (idx_vertex));
-          return (!this->getOutgoingHalfEdgeIndex (idx_vertex).isValid ());
-        }
+  /** \brief Resize the the vertices to n elements. */
+  inline void
+  resizeVertices(const std::size_t n, const VertexData& data = VertexData())
+  {
+    vertices_.resize(n);
+    this->resizeData(vertex_data_cloud_, n, data, HasVertexData());
+  }
+
+  /** \brief Resize the edges to n elements (half-edges will hold 2*n elements). */
+  inline void
+  resizeEdges(const std::size_t n,
+              const EdgeData& edge_data = EdgeData(),
+              const HalfEdgeData he_data = HalfEdgeData())
+  {
+    half_edges_.resize(2 * n);
+    this->resizeData(half_edge_data_cloud_, 2 * n, he_data, HasHalfEdgeData());
+    this->resizeData(edge_data_cloud_, n, edge_data, HasEdgeData());
+  }
+
+  /** \brief Resize the faces to n elements. */
+  inline void
+  resizeFaces(const std::size_t n, const FaceData& data = FaceData())
+  {
+    faces_.resize(n);
+    this->resizeData(face_data_cloud_, n, data, HasFaceData());
+  }
 
-        /** \brief Check if the given half-edge is marked as deleted. */
-        inline bool
-        isDeleted (const HalfEdgeIndex& idx_he) const
-        {
-          assert (this->isValid (idx_he));
-          return (!this->getTerminatingVertexIndex (idx_he).isValid ());
-        }
+  ////////////////////////////////////////////////////////////////////////
+  // clear
+  ////////////////////////////////////////////////////////////////////////
 
-        /** \brief Check if the given edge (any of the two half-edges) is marked as deleted. */
-        inline bool
-        isDeleted (const EdgeIndex& idx_edge) const
-        {
-          assert (this->isValid (idx_edge));
-          return (this->isDeleted (pcl::geometry::toHalfEdgeIndex (idx_edge, true)) ||
-                  this->isDeleted (pcl::geometry::toHalfEdgeIndex (idx_edge, false)));
-        }
+  /** \brief Clear all mesh elements and data. */
+  void
+  clear()
+  {
+    vertices_.clear();
+    half_edges_.clear();
+    faces_.clear();
+
+    this->clearData(vertex_data_cloud_, HasVertexData());
+    this->clearData(half_edge_data_cloud_, HasHalfEdgeData());
+    this->clearData(edge_data_cloud_, HasEdgeData());
+    this->clearData(face_data_cloud_, HasFaceData());
+  }
+
+  ////////////////////////////////////////////////////////////////////////
+  // get / set the vertex data cloud
+  ////////////////////////////////////////////////////////////////////////
+
+  /** \brief Get access to the stored vertex data.
+   * \warning Please make sure to NOT add or remove elements from the cloud.
+   */
+  inline VertexDataCloud&
+  getVertexDataCloud()
+  {
+    return (vertex_data_cloud_);
+  }
 
-        /** \brief Check if the given face is marked as deleted. */
-        inline bool
-        isDeleted (const FaceIndex& idx_face) const
-        {
-          assert (this->isValid (idx_face));
-          return (!this->getInnerHalfEdgeIndex (idx_face).isValid ());
-        }
+  /** \brief Get the stored vertex data. */
+  inline VertexDataCloud
+  getVertexDataCloud() const
+  {
+    return (vertex_data_cloud_);
+  }
+
+  /** \brief Change the stored vertex data.
+   * \param[in] vertex_data_cloud The new vertex data. Must be the same as the current
+   * data.
+   * \return true if the cloud could be set.
+   */
+  inline bool
+  setVertexDataCloud(const VertexDataCloud& vertex_data_cloud)
+  {
+    if (vertex_data_cloud.size() == vertex_data_cloud_.size()) {
+      vertex_data_cloud_ = vertex_data_cloud;
+      return (true);
+    }
+    return (false);
+  }
+
+  ////////////////////////////////////////////////////////////////////////
+  // get / set the half-edge data cloud
+  ////////////////////////////////////////////////////////////////////////
+
+  /** \brief Get access to the stored half-edge data.
+   * \warning Please make sure to NOT add or remove elements from the cloud.
+   */
+  inline HalfEdgeDataCloud&
+  getHalfEdgeDataCloud()
+  {
+    return (half_edge_data_cloud_);
+  }
 
-        ////////////////////////////////////////////////////////////////////////
-        // isIsolated
-        ////////////////////////////////////////////////////////////////////////
+  /** \brief Get the stored half-edge data. */
+  inline HalfEdgeDataCloud
+  getHalfEdgeDataCloud() const
+  {
+    return (half_edge_data_cloud_);
+  }
+
+  /** \brief Change the stored half-edge data.
+   * \param[in] half_edge_data_cloud The new half-edge data. Must be the same as the
+   * current data.
+   * \return true if the cloud could be set.
+   */
+  inline bool
+  setHalfEdgeDataCloud(const HalfEdgeDataCloud& half_edge_data_cloud)
+  {
+    if (half_edge_data_cloud.size() == half_edge_data_cloud_.size()) {
+      half_edge_data_cloud_ = half_edge_data_cloud;
+      return (true);
+    }
+    return (false);
+  }
+
+  ////////////////////////////////////////////////////////////////////////
+  // get / set the edge data cloud
+  ////////////////////////////////////////////////////////////////////////
+
+  /** \brief Get access to the stored edge data.
+   * \warning Please make sure to NOT add or remove elements from the cloud.
+   */
+  inline EdgeDataCloud&
+  getEdgeDataCloud()
+  {
+    return (edge_data_cloud_);
+  }
 
-        /** \brief Check if the given vertex is isolated (not connected to other elements). */
-        inline bool
-        isIsolated (const VertexIndex& idx_vertex) const
-        {
-          assert (this->isValid (idx_vertex));
-          return (!this->getOutgoingHalfEdgeIndex (idx_vertex).isValid ());
-        }
+  /** \brief Get the stored edge data. */
+  inline EdgeDataCloud
+  getEdgeDataCloud() const
+  {
+    return (edge_data_cloud_);
+  }
+
+  /** \brief Change the stored edge data.
+   * \param[in] edge_data_cloud The new edge data. Must be the same as the current data.
+   * \return true if the cloud could be set.
+   */
+  inline bool
+  setEdgeDataCloud(const EdgeDataCloud& edge_data_cloud)
+  {
+    if (edge_data_cloud.size() == edge_data_cloud_.size()) {
+      edge_data_cloud_ = edge_data_cloud;
+      return (true);
+    }
+    return (false);
+  }
+
+  ////////////////////////////////////////////////////////////////////////
+  // get / set the face data cloud
+  ////////////////////////////////////////////////////////////////////////
+
+  /** \brief Get access to the stored face data.
+   * \warning Please make sure to NOT add or remove elements from the cloud.
+   */
+  inline FaceDataCloud&
+  getFaceDataCloud()
+  {
+    return (face_data_cloud_);
+  }
 
-        ////////////////////////////////////////////////////////////////////////
-        // isBoundary
-        ////////////////////////////////////////////////////////////////////////
+  /** \brief Get the stored face data. */
+  inline FaceDataCloud
+  getFaceDataCloud() const
+  {
+    return (face_data_cloud_);
+  }
+
+  /** \brief Change the stored face data.
+   * \param[in] face_data_cloud The new face data. Must be the same as the current data.
+   * \return true if the cloud could be set.
+   */
+  inline bool
+  setFaceDataCloud(const FaceDataCloud& face_data_cloud)
+  {
+    if (face_data_cloud.size() == face_data_cloud_.size()) {
+      face_data_cloud_ = face_data_cloud;
+      return (true);
+    }
+    return (false);
+  }
+
+  ////////////////////////////////////////////////////////////////////////
+  // getVertexIndex / getHalfEdgeIndex / getEdgeIndex / getFaceIndex
+  ////////////////////////////////////////////////////////////////////////
+
+  /** \brief Get the index associated to the given vertex data.
+   * \return Invalid index if the mesh does not have associated vertex data.
+   */
+  inline VertexIndex
+  getVertexIndex(const VertexData& vertex_data) const
+  {
+    if (HasVertexData::value) {
+      assert(&vertex_data >= &vertex_data_cloud_.front() &&
+             &vertex_data <= &vertex_data_cloud_.back());
+      return (VertexIndex(std::distance(&vertex_data_cloud_.front(), &vertex_data)));
+    }
+    return (VertexIndex());
+  }
+
+  /** \brief Get the index associated to the given half-edge data. */
+  inline HalfEdgeIndex
+  getHalfEdgeIndex(const HalfEdgeData& half_edge_data) const
+  {
+    if (HasHalfEdgeData::value) {
+      assert(&half_edge_data >= &half_edge_data_cloud_.front() &&
+             &half_edge_data <= &half_edge_data_cloud_.back());
+      return (HalfEdgeIndex(
+          std::distance(&half_edge_data_cloud_.front(), &half_edge_data)));
+    }
+    return (HalfEdgeIndex());
+  }
+
+  /** \brief Get the index associated to the given edge data. */
+  inline EdgeIndex
+  getEdgeIndex(const EdgeData& edge_data) const
+  {
+    if (HasEdgeData::value) {
+      assert(&edge_data >= &edge_data_cloud_.front() &&
+             &edge_data <= &edge_data_cloud_.back());
+      return (EdgeIndex(std::distance(&edge_data_cloud_.front(), &edge_data)));
+    }
+    return (EdgeIndex());
+  }
+
+  /** \brief Get the index associated to the given face data. */
+  inline FaceIndex
+  getFaceIndex(const FaceData& face_data) const
+  {
+    if (HasFaceData::value) {
+      assert(&face_data >= &face_data_cloud_.front() &&
+             &face_data <= &face_data_cloud_.back());
+      return (FaceIndex(std::distance(&face_data_cloud_.front(), &face_data)));
+    }
+    return (FaceIndex());
+  }
+
+protected:
+  ////////////////////////////////////////////////////////////////////////
+  // Types
+  ////////////////////////////////////////////////////////////////////////
+
+  // Elements
+  using Vertex = pcl::geometry::Vertex;
+  using HalfEdge = pcl::geometry::HalfEdge;
+  using Face = pcl::geometry::Face;
+
+  using Vertices = std::vector<Vertex>;
+  using HalfEdges = std::vector<HalfEdge>;
+  using Faces = std::vector<Face>;
+
+  using VertexIterator = typename Vertices::iterator;
+  using HalfEdgeIterator = typename HalfEdges::iterator;
+  using FaceIterator = typename Faces::iterator;
+
+  using VertexConstIterator = typename Vertices::const_iterator;
+  using HalfEdgeConstIterator = typename HalfEdges::const_iterator;
+  using FaceConstIterator = typename Faces::const_iterator;
+
+  /** \brief General implementation of addFace. */
+  FaceIndex
+  addFaceImplBase(const VertexIndices& vertices,
+                  const FaceData& face_data,
+                  const EdgeData& edge_data,
+                  const HalfEdgeData& half_edge_data)
+  {
+    const int n = static_cast<int>(vertices.size());
+    if (n < 3)
+      return (FaceIndex());
+
+    // Check for topological errors
+    inner_he_.resize(n);
+    free_he_.resize(n);
+    is_new_.resize(n);
+    make_adjacent_.resize(n);
+    for (int i = 0; i < n; ++i) {
+      if (!this->checkTopology1(vertices[i],
+                                vertices[(i + 1) % n],
+                                inner_he_[i],
+                                is_new_[i],
+                                IsManifold())) {
+        return (FaceIndex());
+      }
+    }
+    for (int i = 0; i < n; ++i) {
+      int j = (i + 1) % n;
+      if (!this->checkTopology2(inner_he_[i],
+                                inner_he_[j],
+                                is_new_[i],
+                                is_new_[j],
+                                this->isIsolated(vertices[j]),
+                                make_adjacent_[i],
+                                free_he_[i],
+                                IsManifold())) {
+        return (FaceIndex());
+      }
+    }
+
+    // Reconnect the existing half-edges if needed
+    if (!IsManifold::value) {
+      for (int i = 0; i < n; ++i) {
+        if (make_adjacent_[i]) {
+          this->makeAdjacent(inner_he_[i], inner_he_[(i + 1) % n], free_he_[i]);
+        }
+      }
+    }
+
+    // Add new half-edges if needed
+    for (int i = 0; i < n; ++i) {
+      if (is_new_[i]) {
+        inner_he_[i] = this->addEdge(
+            vertices[i], vertices[(i + 1) % n], half_edge_data, edge_data);
+      }
+    }
+
+    // Connect
+    for (int i = 0; i < n; ++i) {
+      int j = (i + 1) % n;
+      if (is_new_[i] && is_new_[j])
+        this->connectNewNew(inner_he_[i], inner_he_[j], vertices[j], IsManifold());
+      else if (is_new_[i] && !is_new_[j])
+        this->connectNewOld(inner_he_[i], inner_he_[j], vertices[j]);
+      else if (!is_new_[i] && is_new_[j])
+        this->connectOldNew(inner_he_[i], inner_he_[j], vertices[j]);
+      else
+        this->connectOldOld(inner_he_[i], inner_he_[j], vertices[j], IsManifold());
+    }
+    return (this->connectFace(inner_he_, face_data));
+  }
+
+  ////////////////////////////////////////////////////////////////////////
+  // addEdge
+  ////////////////////////////////////////////////////////////////////////
+
+  /** \brief Add an edge between the two given vertices and connect them with the
+   * vertices.
+   * \param[in]  idx_v_a   The first vertex index
+   * \param[in]  idx_v_b   The second vertex index
+   * \param[in]  he_data   Data associated with the half-edges. This is only added if
+   * the mesh has data associated with the half-edges.
+   * \param[in] edge_data Data associated with the edge. This is only added if the mesh
+   * has data associated with the edges.
+   * \return Index to the half-edge from vertex a to vertex b.
+   */
+  HalfEdgeIndex
+  addEdge(const VertexIndex& idx_v_a,
+          const VertexIndex& idx_v_b,
+          const HalfEdgeData& he_data,
+          const EdgeData& edge_data)
+  {
+    half_edges_.push_back(HalfEdge(idx_v_b));
+    half_edges_.push_back(HalfEdge(idx_v_a));
+
+    this->addData(half_edge_data_cloud_, he_data, HasHalfEdgeData());
+    this->addData(half_edge_data_cloud_, he_data, HasHalfEdgeData());
+    this->addData(edge_data_cloud_, edge_data, HasEdgeData());
+
+    return (HalfEdgeIndex(static_cast<int>(half_edges_.size() - 2)));
+  }
+
+  ////////////////////////////////////////////////////////////////////////
+  // topology checks
+  ////////////////////////////////////////////////////////////////////////
+
+  /** \brief Check if the edge between the two vertices can be added.
+   * \param[in]  idx_v_a   Index to the first vertex.
+   * \param[in]  idx_v_b   Index to the second vertex.
+   * \param[out] idx_he_ab Index to the half-edge ab if is_new_ab=false.
+   * \param[out] is_new_ab true if the edge between the vertices exists already. Must be
+   * initialized with true!
+   * \return true if the half-edge may be added.
+   */
+  bool
+  checkTopology1(const VertexIndex& idx_v_a,
+                 const VertexIndex& idx_v_b,
+                 HalfEdgeIndex& idx_he_ab,
+                 std::vector<bool>::reference is_new_ab,
+                 std::true_type /*is_manifold*/) const
+  {
+    is_new_ab = true;
+    if (this->isIsolated(idx_v_a))
+      return (true);
+
+    idx_he_ab = this->getOutgoingHalfEdgeIndex(idx_v_a);
+
+    if (!this->isBoundary(idx_he_ab))
+      return (false);
+    if (this->getTerminatingVertexIndex(idx_he_ab) == idx_v_b)
+      is_new_ab = false;
+    return (true);
+  }
+
+  /** \brief Non manifold version of checkTopology1 */
+  bool
+  checkTopology1(const VertexIndex& idx_v_a,
+                 const VertexIndex& idx_v_b,
+                 HalfEdgeIndex& idx_he_ab,
+                 std::vector<bool>::reference is_new_ab,
+                 std::false_type /*is_manifold*/) const
+  {
+    is_new_ab = true;
+    if (this->isIsolated(idx_v_a))
+      return (true);
+    if (!this->isBoundary(this->getOutgoingHalfEdgeIndex(idx_v_a)))
+      return (false);
+
+    VertexAroundVertexCirculator circ =
+        this->getVertexAroundVertexCirculator(this->getOutgoingHalfEdgeIndex(idx_v_a));
+    const VertexAroundVertexCirculator circ_end = circ;
+
+    do {
+      if (circ.getTargetIndex() == idx_v_b) {
+        idx_he_ab = circ.getCurrentHalfEdgeIndex();
+        if (!this->isBoundary(idx_he_ab))
+          return (false);
 
-        /** \brief Check if the given vertex lies on the boundary. Isolated vertices are considered to be on the boundary. */
-        inline bool
-        isBoundary (const VertexIndex& idx_vertex) const
-        {
-          assert (this->isValid (idx_vertex));
-          if (this->isIsolated (idx_vertex)) return (true);
-          return (this->isBoundary (this->getOutgoingHalfEdgeIndex (idx_vertex)));
-        }
+        is_new_ab = false;
+        return (true);
+      }
+    } while (++circ != circ_end);
+
+    return (true);
+  }
+
+  /** \brief Check if the face may be added (mesh does not become non-manifold). */
+  inline bool
+  checkTopology2(const HalfEdgeIndex& /*idx_he_ab*/,
+                 const HalfEdgeIndex& /*idx_he_bc*/,
+                 const bool is_new_ab,
+                 const bool is_new_bc,
+                 const bool is_isolated_b,
+                 std::vector<bool>::reference /*make_adjacent_ab_bc*/,
+                 HalfEdgeIndex& /*idx_free_half_edge*/,
+                 std::true_type /*is_manifold*/) const
+  {
+    return !(is_new_ab && is_new_bc && !is_isolated_b);
+  }
+
+  /** \brief Check if the half-edge bc is the next half-edge of ab.
+   * \param[in]  idx_he_ab Index to the half-edge between the vertices a and b.
+   * \param[in]  idx_he_bc Index to the half-edge between the vertices b and c.
+   * \param[in]  is_new_ab Half-edge ab is new.
+   * \param[in]  is_new_bc Half-edge bc is new.
+   * \param[out] make_adjacent_ab_bc Half-edges ab and bc need to be made adjacent.
+   * \param[out] idx_free_half_edge  Free half-edge (needed for makeAdjacent)
+   * \return true if addFace may be continued.
+   */
+  inline bool
+  checkTopology2(const HalfEdgeIndex& idx_he_ab,
+                 const HalfEdgeIndex& idx_he_bc,
+                 const bool is_new_ab,
+                 const bool is_new_bc,
+                 const bool /*is_isolated_b*/,
+                 std::vector<bool>::reference make_adjacent_ab_bc,
+                 HalfEdgeIndex& idx_free_half_edge,
+                 std::false_type /*is_manifold*/) const
+  {
+    if (is_new_ab || is_new_bc) {
+      make_adjacent_ab_bc = false;
+      return (true); // Make adjacent is only needed for two old half-edges
+    }
+
+    if (this->getNextHalfEdgeIndex(idx_he_ab) == idx_he_bc) {
+      make_adjacent_ab_bc = false;
+      return (true); // already adjacent
+    }
+
+    make_adjacent_ab_bc = true;
+
+    // Find the next boundary half edge
+    IncomingHalfEdgeAroundVertexCirculator circ =
+        this->getIncomingHalfEdgeAroundVertexCirculator(
+            this->getOppositeHalfEdgeIndex(idx_he_bc));
+
+    do
+      ++circ;
+    while (!this->isBoundary(circ.getTargetIndex()));
+    idx_free_half_edge = circ.getTargetIndex();
+
+    // This would detach the faces around the vertex from each other.
+    return (circ.getTargetIndex() != idx_he_ab);
+  }
+
+  /** \brief Make the half-edges bc the next half-edge of ab.
+   * \param[in]      idx_he_ab          Index to the half-edge between the vertices a
+   * and b.
+   * \param[in]      idx_he_bc          Index to the half-edge between the
+   * vertices b and c.
+   * \param[in, out] idx_free_half_edge Free half-edge needed to re-connect the
+   * half-edges around vertex b.
+   */
+  void
+  makeAdjacent(const HalfEdgeIndex& idx_he_ab,
+               const HalfEdgeIndex& idx_he_bc,
+               HalfEdgeIndex& idx_free_half_edge)
+  {
+    // Re-link. No references!
+    const HalfEdgeIndex idx_he_ab_next = this->getNextHalfEdgeIndex(idx_he_ab);
+    const HalfEdgeIndex idx_he_bc_prev = this->getPrevHalfEdgeIndex(idx_he_bc);
+    const HalfEdgeIndex idx_he_free_next =
+        this->getNextHalfEdgeIndex(idx_free_half_edge);
+
+    this->connectPrevNext(idx_he_ab, idx_he_bc);
+    this->connectPrevNext(idx_free_half_edge, idx_he_ab_next);
+    this->connectPrevNext(idx_he_bc_prev, idx_he_free_next);
+  }
+
+  ////////////////////////////////////////////////////////////////////////
+  // connect
+  ////////////////////////////////////////////////////////////////////////
+
+  /** \brief Add a face to the mesh and connect it to the half-edges.
+   * \param[in] inner_he  Inner half-edges of the face.
+   * \param[in] face_data Data that is stored in the face. This is only added if the
+   * mesh has data associated with the faces.
+   * \return Index to the new face.
+   */
+  FaceIndex
+  connectFace(const HalfEdgeIndices& inner_he, const FaceData& face_data)
+  {
+    faces_.push_back(Face(inner_he.back()));
+    this->addData(face_data_cloud_, face_data, HasFaceData());
 
-        /** \brief Check if the given half-edge lies on the bounddary. */
-        inline bool
-        isBoundary (const HalfEdgeIndex& idx_he) const
-        {
-          assert (this->isValid (idx_he));
-          return (!this->getFaceIndex (idx_he).isValid ());
-        }
+    const FaceIndex idx_face(static_cast<int>(this->sizeFaces() - 1));
 
-        /** \brief Check if the given edge lies on the boundary (any of the two half-edges lies on the boundary. */
-        inline bool
-        isBoundary (const EdgeIndex& idx_edge) const
-        {
-          assert (this->isValid (idx_edge));
-          const HalfEdgeIndex& idx = pcl::geometry::toHalfEdgeIndex (idx_edge);
-          return (this->isBoundary (idx) || this->isBoundary (this->getOppositeHalfEdgeIndex (idx)));
-        }
+    for (const auto& idx_half_edge : inner_he) {
+      this->setFaceIndex(idx_half_edge, idx_face);
+    }
 
-        /** \brief Check if the given face lies on the boundary. There are two versions of this method, selected by the template parameter.
-          * \tparam CheckVerticesT Check if any vertex lies on the boundary (true) or check if any edge lies on the boundary (false).
-          */
-        template <bool CheckVerticesT> inline bool
-        isBoundary (const FaceIndex& idx_face) const
-        {
-          assert (this->isValid (idx_face));
-          return (this->isBoundary (idx_face, std::integral_constant <bool, CheckVerticesT> ()));
-        }
+    return (idx_face);
+  }
 
-        /** \brief Check if the given face lies on the boundary. This method uses isBoundary \c true which checks if any vertex lies on the boundary. */
-        inline bool
-        isBoundary (const FaceIndex& idx_face) const
-        {
-          assert (this->isValid (idx_face));
-          return (this->isBoundary (idx_face, std::true_type ()));
-        }
+  /** \brief Connect the next and prev indices of the two half-edges with each other. */
+  inline void
+  connectPrevNext(const HalfEdgeIndex& idx_he_ab, const HalfEdgeIndex& idx_he_bc)
+  {
+    this->setNextHalfEdgeIndex(idx_he_ab, idx_he_bc);
+    this->setPrevHalfEdgeIndex(idx_he_bc, idx_he_ab);
+  }
+
+  /** \brief Both half-edges are new (manifold version). */
+  void
+  connectNewNew(const HalfEdgeIndex& idx_he_ab,
+                const HalfEdgeIndex& idx_he_bc,
+                const VertexIndex& idx_v_b,
+                std::true_type /*is_manifold*/)
+  {
+    const HalfEdgeIndex idx_he_ba = this->getOppositeHalfEdgeIndex(idx_he_ab);
+    const HalfEdgeIndex idx_he_cb = this->getOppositeHalfEdgeIndex(idx_he_bc);
 
-        ////////////////////////////////////////////////////////////////////////
-        // isManifold
-        ////////////////////////////////////////////////////////////////////////
+    this->connectPrevNext(idx_he_ab, idx_he_bc);
+    this->connectPrevNext(idx_he_cb, idx_he_ba);
 
-        /** \brief Check if the given vertex is manifold. Isolated vertices are manifold. */
-        inline bool
-        isManifold (const VertexIndex& idx_vertex) const
-        {
-          assert (this->isValid (idx_vertex));
-          if (this->isIsolated (idx_vertex)) return (true);
-          return (this->isManifold (idx_vertex, IsManifold ()));
-        }
+    this->setOutgoingHalfEdgeIndex(idx_v_b, idx_he_ba);
+  }
 
-        /** \brief Check if the mesh is manifold. */
-        inline bool
-        isManifold () const
-        {
-          return (this->isManifold (IsManifold ()));
-        }
+  /** \brief Both half-edges are new (non-manifold version). */
+  void
+  connectNewNew(const HalfEdgeIndex& idx_he_ab,
+                const HalfEdgeIndex& idx_he_bc,
+                const VertexIndex& idx_v_b,
+                std::false_type /*is_manifold*/)
+  {
+    if (this->isIsolated(idx_v_b)) {
+      this->connectNewNew(idx_he_ab, idx_he_bc, idx_v_b, std::true_type());
+    }
+    else {
+      const HalfEdgeIndex idx_he_ba = this->getOppositeHalfEdgeIndex(idx_he_ab);
+      const HalfEdgeIndex idx_he_cb = this->getOppositeHalfEdgeIndex(idx_he_bc);
+
+      // No references!
+      const HalfEdgeIndex idx_he_b_out = this->getOutgoingHalfEdgeIndex(idx_v_b);
+      const HalfEdgeIndex idx_he_b_out_prev = this->getPrevHalfEdgeIndex(idx_he_b_out);
+
+      this->connectPrevNext(idx_he_ab, idx_he_bc);
+      this->connectPrevNext(idx_he_cb, idx_he_b_out);
+      this->connectPrevNext(idx_he_b_out_prev, idx_he_ba);
+    }
+  }
+
+  /** \brief The first half-edge is new. */
+  void
+  connectNewOld(const HalfEdgeIndex& idx_he_ab,
+                const HalfEdgeIndex& idx_he_bc,
+                const VertexIndex& idx_v_b)
+  {
+    const HalfEdgeIndex idx_he_ba = this->getOppositeHalfEdgeIndex(idx_he_ab);
+    const HalfEdgeIndex idx_he_bc_prev =
+        this->getPrevHalfEdgeIndex(idx_he_bc); // No reference!
 
-        ////////////////////////////////////////////////////////////////////////
-        // size
-        ////////////////////////////////////////////////////////////////////////
+    this->connectPrevNext(idx_he_ab, idx_he_bc);
+    this->connectPrevNext(idx_he_bc_prev, idx_he_ba);
 
-        /** \brief Get the number of the vertices. */
-        inline std::size_t
-        sizeVertices () const
-        {
-          return (vertices_.size ());
-        }
+    this->setOutgoingHalfEdgeIndex(idx_v_b, idx_he_ba);
+  }
 
-        /** \brief Get the number of the half-edges. */
-        inline std::size_t
-        sizeHalfEdges () const
-        {
-          assert (half_edges_.size () % 2 == 0); // This would be a bug in the mesh.
-          return (half_edges_.size ());
-        }
+  /** \brief The second half-edge is new. */
+  void
+  connectOldNew(const HalfEdgeIndex& idx_he_ab,
+                const HalfEdgeIndex& idx_he_bc,
+                const VertexIndex& idx_v_b)
+  {
+    const HalfEdgeIndex idx_he_cb = this->getOppositeHalfEdgeIndex(idx_he_bc);
+    const HalfEdgeIndex idx_he_ab_next =
+        this->getNextHalfEdgeIndex(idx_he_ab); // No reference!
+
+    this->connectPrevNext(idx_he_ab, idx_he_bc);
+    this->connectPrevNext(idx_he_cb, idx_he_ab_next);
+
+    this->setOutgoingHalfEdgeIndex(idx_v_b, idx_he_ab_next);
+  }
+
+  /** \brief Both half-edges are old (manifold version). */
+  void
+  connectOldOld(const HalfEdgeIndex& /*idx_he_ab*/,
+                const HalfEdgeIndex& /*idx_he_bc*/,
+                const VertexIndex& /*idx_v_b*/,
+                std::true_type /*is_manifold*/)
+  {}
+
+  /** \brief Both half-edges are old (non-manifold version). */
+  void
+  connectOldOld(const HalfEdgeIndex& /*idx_he_ab*/,
+                const HalfEdgeIndex& idx_he_bc,
+                const VertexIndex& idx_v_b,
+                std::false_type /*is_manifold*/)
+  {
+    const HalfEdgeIndex& idx_he_b_out = this->getOutgoingHalfEdgeIndex(idx_v_b);
 
-        /** \brief Get the number of the edges. */
-        inline std::size_t
-        sizeEdges () const
-        {
-          assert (half_edges_.size () % 2 == 0); // This would be a bug in the mesh.
-          return (half_edges_.size () / 2);
-        }
+    // The outgoing half edge MUST be a boundary half-edge (if there is one)
+    if (idx_he_b_out == idx_he_bc) // he_bc is no longer on the boundary
+    {
+      OutgoingHalfEdgeAroundVertexCirculator circ =
+          this->getOutgoingHalfEdgeAroundVertexCirculator(idx_he_b_out);
+      const OutgoingHalfEdgeAroundVertexCirculator circ_end = circ;
+
+      while (++circ != circ_end) {
+        if (this->isBoundary(circ.getTargetIndex())) {
+          this->setOutgoingHalfEdgeIndex(idx_v_b, circ.getTargetIndex());
+          return;
+        }
+      }
+    }
+  }
+
+  ////////////////////////////////////////////////////////////////////////
+  // addData
+  ////////////////////////////////////////////////////////////////////////
+
+  /** \brief Add mesh data. */
+  template <class DataT>
+  inline void
+  addData(pcl::PointCloud<DataT>& cloud, const DataT& data, std::true_type /*has_data*/)
+  {
+    cloud.push_back(data);
+  }
+
+  /** \brief Does nothing. */
+  template <class DataT>
+  inline void
+  addData(pcl::PointCloud<DataT>& /*cloud*/,
+          const DataT& /*data*/,
+          std::false_type /*has_data*/)
+  {}
+
+  ////////////////////////////////////////////////////////////////////////
+  // deleteFace
+  ////////////////////////////////////////////////////////////////////////
+
+  /** \brief Manifold version of deleteFace. If the mesh becomes non-manifold due to the
+   * delete operation the faces around the non-manifold vertex are deleted until the
+   * mesh becomes manifold again. */
+  void
+  deleteFace(const FaceIndex& idx_face, std::true_type /*is_manifold*/)
+  {
+    assert(this->isValid(idx_face));
+    delete_faces_face_.clear();
+    delete_faces_face_.push_back(idx_face);
+
+    while (!delete_faces_face_.empty()) {
+      const FaceIndex idx_face_cur = delete_faces_face_.back();
+      delete_faces_face_.pop_back();
+
+      // This calls the non-manifold version of deleteFace, which will call the manifold
+      // version of reconnect.
+      this->deleteFace(idx_face_cur, std::false_type());
+    }
+  }
+
+  /** \brief Non-manifold version of deleteFace. */
+  void
+  deleteFace(const FaceIndex& idx_face, std::false_type /*is_manifold*/)
+  {
+    assert(this->isValid(idx_face));
+    if (this->isDeleted(idx_face))
+      return;
+
+    // Store all half-edges in the face
+    inner_he_.clear();
+    is_boundary_.clear();
+    InnerHalfEdgeAroundFaceCirculator circ =
+        this->getInnerHalfEdgeAroundFaceCirculator(idx_face);
+    const InnerHalfEdgeAroundFaceCirculator circ_end = circ;
+    do {
+      inner_he_.push_back(circ.getTargetIndex());
+      is_boundary_.push_back(
+          this->isBoundary(this->getOppositeHalfEdgeIndex(circ.getTargetIndex())));
+    } while (++circ != circ_end);
+    assert(inner_he_.size() >= 3); // Minimum should be a triangle.
+
+    const int n = static_cast<int>(inner_he_.size());
+    int j;
+
+    if (IsManifold::value) {
+      for (int i = 0; i < n; ++i) {
+        j = (i + 1) % n;
+        this->reconnect(inner_he_[i], inner_he_[j], is_boundary_[i], is_boundary_[j]);
+      }
+      for (int i = 0; i < n; ++i) {
+        this->getHalfEdge(inner_he_[i]).idx_face_.invalidate();
+      }
+    }
+    else {
+      for (int i = 0; i < n; ++i) {
+        j = (i + 1) % n;
+        this->reconnect(inner_he_[i], inner_he_[j], is_boundary_[i], is_boundary_[j]);
+        this->getHalfEdge(inner_he_[i]).idx_face_.invalidate();
+      }
+    }
+
+    this->markDeleted(idx_face);
+  }
+
+  ////////////////////////////////////////////////////////////////////////
+  // reconnect
+  ////////////////////////////////////////////////////////////////////////
+
+  /** \brief Deconnect the input half-edges from the mesh and adjust the indices of the
+   * connected half-edges. */
+  void
+  reconnect(const HalfEdgeIndex& idx_he_ab,
+            const HalfEdgeIndex& idx_he_bc,
+            const bool is_boundary_ba,
+            const bool is_boundary_cb)
+  {
+    const HalfEdgeIndex idx_he_ba = this->getOppositeHalfEdgeIndex(idx_he_ab);
+    const HalfEdgeIndex idx_he_cb = this->getOppositeHalfEdgeIndex(idx_he_bc);
+    const VertexIndex idx_v_b = this->getTerminatingVertexIndex(idx_he_ab);
 
-        /** \brief Get the number of the faces. */
-        inline std::size_t
-        sizeFaces () const
-        {
-          return (faces_.size ());
-        }
+    if (is_boundary_ba && is_boundary_cb) // boundary - boundary
+    {
+      const HalfEdgeIndex& idx_he_cb_next = this->getNextHalfEdgeIndex(idx_he_cb);
+
+      if (idx_he_cb_next == idx_he_ba) // Vertex b is isolated
+      {
+        this->markDeleted(idx_v_b);
+      }
+      else {
+        this->connectPrevNext(this->getPrevHalfEdgeIndex(idx_he_ba), idx_he_cb_next);
+        this->setOutgoingHalfEdgeIndex(idx_v_b, idx_he_cb_next);
+      }
+
+      this->markDeleted(idx_he_ab);
+      this->markDeleted(idx_he_ba);
+    }
+    else if (is_boundary_ba && !is_boundary_cb) // boundary - no boundary
+    {
+      this->connectPrevNext(this->getPrevHalfEdgeIndex(idx_he_ba), idx_he_bc);
+      this->setOutgoingHalfEdgeIndex(idx_v_b, idx_he_bc);
 
-        ////////////////////////////////////////////////////////////////////////
-        // empty
-        ////////////////////////////////////////////////////////////////////////
+      this->markDeleted(idx_he_ab);
+      this->markDeleted(idx_he_ba);
+    }
+    else if (!is_boundary_ba && is_boundary_cb) // no boundary - boundary
+    {
+      const HalfEdgeIndex& idx_he_cb_next = this->getNextHalfEdgeIndex(idx_he_cb);
+      this->connectPrevNext(idx_he_ab, idx_he_cb_next);
+      this->setOutgoingHalfEdgeIndex(idx_v_b, idx_he_cb_next);
+    }
+    else // no boundary - no boundary
+    {
+      this->reconnectNBNB(idx_he_bc, idx_he_cb, idx_v_b, IsManifold());
+    }
+  }
+
+  /** \brief Both edges are not on the boundary. Manifold version. */
+  void
+  reconnectNBNB(const HalfEdgeIndex& idx_he_bc,
+                const HalfEdgeIndex& idx_he_cb,
+                const VertexIndex& idx_v_b,
+                std::true_type /*is_manifold*/)
+  {
+    if (this->isBoundary(idx_v_b)) {
+      // Deletion of this face makes the mesh non-manifold
+      // -> delete the neighboring faces until it is manifold again
+      IncomingHalfEdgeAroundVertexCirculator circ =
+          this->getIncomingHalfEdgeAroundVertexCirculator(idx_he_cb);
 
-        /** \brief Check if the mesh is empty. */
-        inline bool
-        empty () const
-        {
-          return (this->emptyVertices () && this->emptyEdges () && this->emptyFaces ());
-        }
+      while (!this->isBoundary(circ.getTargetIndex())) {
+        delete_faces_face_.push_back(this->getFaceIndex((circ++).getTargetIndex()));
 
-        /** \brief Check if the vertices are empty. */
-        inline bool
-        emptyVertices () const
+#ifdef PCL_GEOMETRY_MESH_BASE_TEST_DELETE_FACE_MANIFOLD_2
+        if (circ == this->getIncomingHalfEdgeAroundVertexCirculator(
+                        idx_he_cb)) // Abort infinity loop
         {
-          return (vertices_.empty ());
+          // In a manifold mesh we can't invalidate the face while reconnecting!
+          // See the implementation of
+          // deleteFace (const FaceIndex&  idx_face,
+          //             std::false_type /*is_manifold*/)
+          pcl::geometry::g_pcl_geometry_mesh_base_test_delete_face_manifold_2_success =
+              false;
+          return;
         }
+#endif
+      }
+    }
+    else {
+      this->setOutgoingHalfEdgeIndex(idx_v_b, idx_he_bc);
+    }
+  }
+
+  /** \brief Both edges are not on the boundary. Non-manifold version. */
+  void
+  reconnectNBNB(const HalfEdgeIndex& idx_he_bc,
+                const HalfEdgeIndex& /*idx_he_cb*/,
+                const VertexIndex& idx_v_b,
+                std::false_type /*is_manifold*/)
+  {
+    if (!this->isBoundary(idx_v_b)) {
+      this->setOutgoingHalfEdgeIndex(idx_v_b, idx_he_bc);
+    }
+  }
+
+  ////////////////////////////////////////////////////////////////////////
+  // markDeleted
+  ////////////////////////////////////////////////////////////////////////
+
+  /** \brief Mark the given vertex as deleted. */
+  inline void
+  markDeleted(const VertexIndex& idx_vertex)
+  {
+    assert(this->isValid(idx_vertex));
+    this->getVertex(idx_vertex).idx_outgoing_half_edge_.invalidate();
+  }
 
-        /** \brief Check if the edges are empty. */
-        inline bool
-        emptyEdges () const
-        {
-          return (half_edges_.empty ());
-        }
+  /** \brief Mark the given half-edge as deleted. */
+  inline void
+  markDeleted(const HalfEdgeIndex& idx_he)
+  {
+    assert(this->isValid(idx_he));
+    this->getHalfEdge(idx_he).idx_terminating_vertex_.invalidate();
+  }
 
-        /** \brief Check if the faces are empty. */
-        inline bool
-        emptyFaces () const
-        {
-          return (faces_.empty ());
-        }
+  /** \brief Mark the given edge (both half-edges) as deleted. */
+  inline void
+  markDeleted(const EdgeIndex& idx_edge)
+  {
+    assert(this->isValid(idx_edge));
+    this->markDeleted(pcl::geometry::toHalfEdgeIndex(idx_edge, true));
+    this->markDeleted(pcl::geometry::toHalfEdgeIndex(idx_edge, false));
+  }
+
+  /** \brief Mark the given face as deleted. */
+  inline void
+  markDeleted(const FaceIndex& idx_face)
+  {
+    assert(this->isValid(idx_face));
+    this->getFace(idx_face).idx_inner_half_edge_.invalidate();
+  }
+
+  ////////////////////////////////////////////////////////////////////////
+  // For cleanUp
+  ////////////////////////////////////////////////////////////////////////
+
+  /** \brief Removes mesh elements and data that are marked as deleted from the
+   * container.
+   * \tparam ElementContainerT e.g. std::vector \<Vertex\>
+   * \tparam DataContainerT    e.g. std::vector \<VertexData\>
+   * \tparam IndexContainerT   e.g.  std::vector \<VertexIndex\>
+   * \tparam HasDataT          Integral constant specifying if the mesh has data
+   * associated with the elements.
+   * \param[in, out] elements Container for the mesh elements. Resized to the new size.
+   * \param[in, out] data_cloud Container for the mesh data. Resized to the new size.
+   * \return Container with the same size as the old input data. Holds the indices to
+   * the new elements for each non-deleted element and an invalid index if it is
+   * deleted.
+   */
+  template <class ElementContainerT,
+            class DataContainerT,
+            class IndexContainerT,
+            class HasDataT>
+  IndexContainerT
+  remove(ElementContainerT& elements, DataContainerT& data_cloud)
+  {
+    using Index = typename IndexContainerT::value_type;
+    using Element = typename ElementContainerT::value_type;
+
+    if (HasDataT::value)
+      assert(elements.size() == data_cloud.size());
+    else
+      assert(data_cloud.empty()); // Bug in this class!
+
+    IndexContainerT new_indices(elements.size(),
+                                typename IndexContainerT::value_type());
+    Index ind_old(0), ind_new(0);
+
+    typename ElementContainerT::const_iterator it_e_old = elements.begin();
+    typename ElementContainerT::iterator it_e_new = elements.begin();
+
+    typename DataContainerT::const_iterator it_d_old = data_cloud.begin();
+    typename DataContainerT::iterator it_d_new = data_cloud.begin();
+
+    typename IndexContainerT::iterator it_ind_new = new_indices.begin();
+    typename IndexContainerT::const_iterator it_ind_new_end = new_indices.end();
+
+    while (it_ind_new != it_ind_new_end) {
+      if (!this->isDeleted(ind_old)) {
+        *it_ind_new = ind_new++;
+
+        // TODO: Test for self assignment?
+        *it_e_new++ = *it_e_old;
+        this->assignIf(it_d_old, it_d_new, HasDataT());
+        this->incrementIf(it_d_new, HasDataT());
+      }
+      ++ind_old;
+      ++it_e_old;
+      this->incrementIf(it_d_old, HasDataT());
+      ++it_ind_new;
+    }
+
+    elements.resize(ind_new.get(), Element());
+    if (HasDataT::value) {
+      data_cloud.resize(ind_new.get());
+    }
+    else if (it_d_old != data_cloud.begin() || it_d_new != data_cloud.begin()) {
+      std::cerr << "TODO: Bug in MeshBase::remove!\n";
+      assert(false);
+      exit(EXIT_FAILURE);
+    }
+
+    return (new_indices);
+  }
+
+  /** \brief Increment the iterator. */
+  template <class IteratorT>
+  inline void
+  incrementIf(IteratorT& it, std::true_type /*has_data*/) const
+  {
+    ++it;
+  }
+
+  /** \brief Does nothing. */
+  template <class IteratorT>
+  inline void
+  incrementIf(IteratorT& /*it*/, std::false_type /*has_data*/) const
+  {}
+
+  /** \brief Assign the source iterator to the target iterator. */
+  template <class ConstIteratorT, class IteratorT>
+  inline void
+  assignIf(const ConstIteratorT source,
+           IteratorT target,
+           std::true_type /*has_data*/) const
+  {
+    *target = *source;
+  }
+
+  /** \brief Does nothing. */
+  template <class ConstIteratorT, class IteratorT>
+  inline void
+  assignIf(const ConstIteratorT /*source*/,
+           IteratorT /*target*/,
+           std::false_type /*has_data*/) const
+  {}
+
+  ////////////////////////////////////////////////////////////////////////
+  // Vertex / Half-edge / Face connectivity
+  ////////////////////////////////////////////////////////////////////////
+
+  /** \brief Set the outgoing half-edge index to a given vertex. */
+  inline void
+  setOutgoingHalfEdgeIndex(const VertexIndex& idx_vertex,
+                           const HalfEdgeIndex& idx_outgoing_half_edge)
+  {
+    assert(this->isValid(idx_vertex));
+    this->getVertex(idx_vertex).idx_outgoing_half_edge_ = idx_outgoing_half_edge;
+  }
+
+  /** \brief Set the terminating vertex index to a given half-edge. */
+  inline void
+  setTerminatingVertexIndex(const HalfEdgeIndex& idx_half_edge,
+                            const VertexIndex& idx_terminating_vertex)
+  {
+    assert(this->isValid(idx_half_edge));
+    this->getHalfEdge(idx_half_edge).idx_terminating_vertex_ = idx_terminating_vertex;
+  }
+
+  /** \brief Set the next half_edge index to a given half-edge. */
+  inline void
+  setNextHalfEdgeIndex(const HalfEdgeIndex& idx_half_edge,
+                       const HalfEdgeIndex& idx_next_half_edge)
+  {
+    assert(this->isValid(idx_half_edge));
+    this->getHalfEdge(idx_half_edge).idx_next_half_edge_ = idx_next_half_edge;
+  }
+
+  /** \brief Set the previous half-edge index to a given half-edge. */
+  inline void
+  setPrevHalfEdgeIndex(const HalfEdgeIndex& idx_half_edge,
+                       const HalfEdgeIndex& idx_prev_half_edge)
+  {
+    assert(this->isValid(idx_half_edge));
+    this->getHalfEdge(idx_half_edge).idx_prev_half_edge_ = idx_prev_half_edge;
+  }
 
-        ////////////////////////////////////////////////////////////////////////
-        // reserve
-        ////////////////////////////////////////////////////////////////////////
+  /** \brief Set the face index to a given half-edge. */
+  inline void
+  setFaceIndex(const HalfEdgeIndex& idx_half_edge, const FaceIndex& idx_face)
+  {
+    assert(this->isValid(idx_half_edge));
+    this->getHalfEdge(idx_half_edge).idx_face_ = idx_face;
+  }
+
+  /** \brief Set the inner half-edge index to a given face. */
+  inline void
+  setInnerHalfEdgeIndex(const FaceIndex& idx_face,
+                        const HalfEdgeIndex& idx_inner_half_edge)
+  {
+    assert(this->isValid(idx_face));
+    this->getFace(idx_face).idx_inner_half_edge_ = idx_inner_half_edge;
+  }
 
-        /** \brief Reserve storage space n vertices. */
-        inline void
-        reserveVertices (const std::size_t n)
-        {
-          vertices_.reserve (n);
-          this->reserveData (vertex_data_cloud_, n, HasVertexData ());
-        }
+  ////////////////////////////////////////////////////////////////////////
+  // isBoundary / isManifold
+  ////////////////////////////////////////////////////////////////////////
 
-        /** \brief Reserve storage space for n edges (2*n storage space is reserved for the half-edges). */
-        inline void
-        reserveEdges (const std::size_t n)
-        {
-          half_edges_.reserve (2*n);
-          this->reserveData (half_edge_data_cloud_, 2*n, HasHalfEdgeData ());
-          this->reserveData (edge_data_cloud_     ,   n, HasEdgeData     ());
-        }
+  /** \brief Check if any vertex of the face lies on the boundary. */
+  bool
+  isBoundary(const FaceIndex& idx_face, std::true_type /*check_vertices*/) const
+  {
+    VertexAroundFaceCirculator circ = this->getVertexAroundFaceCirculator(idx_face);
+    const VertexAroundFaceCirculator circ_end = circ;
 
-        /** \brief Reserve storage space for n faces. */
-        inline void
-        reserveFaces (const std::size_t n)
-        {
-          faces_.reserve (n);
-          this->reserveData (face_data_cloud_, n, HasFaceData ());
-        }
-
-        ////////////////////////////////////////////////////////////////////////
-        // resize
-        ////////////////////////////////////////////////////////////////////////
-
-        /** \brief Resize the the vertices to n elements. */
-        inline void
-        resizeVertices (const std::size_t n, const VertexData& data = VertexData ())
-        {
-          vertices_.resize (n);
-          this->resizeData (vertex_data_cloud_, n, data, HasVertexData ());
-        }
-
-        /** \brief Resize the edges to n elements (half-edges will hold 2*n elements). */
-        inline void
-        resizeEdges (const std::size_t       n,
-                     const EdgeData&    edge_data = EdgeData (),
-                     const HalfEdgeData he_data   = HalfEdgeData ())
-        {
-          half_edges_.resize (2*n);
-          this->resizeData (half_edge_data_cloud_, 2*n, he_data  , HasHalfEdgeData ());
-          this->resizeData (edge_data_cloud_     ,   n, edge_data, HasEdgeData     ());
-        }
-
-        /** \brief Resize the faces to n elements. */
-        inline void
-        resizeFaces (const std::size_t n, const FaceData& data = FaceData ())
-        {
-          faces_.resize (n);
-          this->resizeData (face_data_cloud_, n, data, HasFaceData ());
-        }
-
-        ////////////////////////////////////////////////////////////////////////
-        // clear
-        ////////////////////////////////////////////////////////////////////////
-
-        /** \brief Clear all mesh elements and data. */
-        void
-        clear ()
-        {
-          vertices_.clear ();
-          half_edges_.clear ();
-          faces_.clear ();
-
-          this->clearData (vertex_data_cloud_   , HasVertexData   ());
-          this->clearData (half_edge_data_cloud_, HasHalfEdgeData ());
-          this->clearData (edge_data_cloud_     , HasEdgeData     ());
-          this->clearData (face_data_cloud_     , HasFaceData     ());
-        }
-
-        ////////////////////////////////////////////////////////////////////////
-        // get / set the vertex data cloud
-        ////////////////////////////////////////////////////////////////////////
-
-        /** \brief Get access to the stored vertex data.
-          * \warning Please make sure to NOT add or remove elements from the cloud.
-          */
-        inline VertexDataCloud&
-        getVertexDataCloud ()
-        {
-          return (vertex_data_cloud_);
-        }
-
-        /** \brief Get the stored vertex data. */
-        inline VertexDataCloud
-        getVertexDataCloud () const
-        {
-          return (vertex_data_cloud_);
-        }
-
-        /** \brief Change the stored vertex data.
-          * \param[in] vertex_data_cloud The new vertex data. Must be the same as the current data.
-          * \return true if the cloud could be set.
-          */
-        inline bool
-        setVertexDataCloud (const VertexDataCloud& vertex_data_cloud)
-        {
-          if (vertex_data_cloud.size () == vertex_data_cloud_.size ())
-          {
-            vertex_data_cloud_ = vertex_data_cloud;
-            return (true);
-          }
-          return (false);
-        }
-
-        ////////////////////////////////////////////////////////////////////////
-        // get / set the half-edge data cloud
-        ////////////////////////////////////////////////////////////////////////
-
-        /** \brief Get access to the stored half-edge data.
-          * \warning Please make sure to NOT add or remove elements from the cloud.
-          */
-        inline HalfEdgeDataCloud&
-        getHalfEdgeDataCloud ()
-        {
-          return (half_edge_data_cloud_);
-        }
-
-        /** \brief Get the stored half-edge data. */
-        inline HalfEdgeDataCloud
-        getHalfEdgeDataCloud () const
-        {
-          return (half_edge_data_cloud_);
-        }
-
-        /** \brief Change the stored half-edge data.
-          * \param[in] half_edge_data_cloud The new half-edge data. Must be the same as the current data.
-          * \return true if the cloud could be set.
-          */
-        inline bool
-        setHalfEdgeDataCloud (const HalfEdgeDataCloud& half_edge_data_cloud)
-        {
-          if (half_edge_data_cloud.size () == half_edge_data_cloud_.size ())
-          {
-            half_edge_data_cloud_ = half_edge_data_cloud;
-            return (true);
-          }
-          return (false);
-        }
-
-        ////////////////////////////////////////////////////////////////////////
-        // get / set the edge data cloud
-        ////////////////////////////////////////////////////////////////////////
-
-        /** \brief Get access to the stored edge data.
-          * \warning Please make sure to NOT add or remove elements from the cloud.
-          */
-        inline EdgeDataCloud&
-        getEdgeDataCloud ()
-        {
-          return (edge_data_cloud_);
-        }
-
-        /** \brief Get the stored edge data. */
-        inline EdgeDataCloud
-        getEdgeDataCloud () const
-        {
-          return (edge_data_cloud_);
-        }
-
-        /** \brief Change the stored edge data.
-          * \param[in] edge_data_cloud The new edge data. Must be the same as the current data.
-          * \return true if the cloud could be set.
-          */
-        inline bool
-        setEdgeDataCloud (const EdgeDataCloud& edge_data_cloud)
-        {
-          if (edge_data_cloud.size () == edge_data_cloud_.size ())
-          {
-            edge_data_cloud_ = edge_data_cloud;
-            return (true);
-          }
-          return (false);
-        }
-
-        ////////////////////////////////////////////////////////////////////////
-        // get / set the face data cloud
-        ////////////////////////////////////////////////////////////////////////
-
-        /** \brief Get access to the stored face data.
-          * \warning Please make sure to NOT add or remove elements from the cloud.
-          */
-        inline FaceDataCloud&
-        getFaceDataCloud ()
-        {
-          return (face_data_cloud_);
-        }
-
-        /** \brief Get the stored face data. */
-        inline FaceDataCloud
-        getFaceDataCloud () const
-        {
-          return (face_data_cloud_);
-        }
-
-        /** \brief Change the stored face data.
-          * \param[in] face_data_cloud The new face data. Must be the same as the current data.
-          * \return true if the cloud could be set.
-          */
-        inline bool
-        setFaceDataCloud (const FaceDataCloud& face_data_cloud)
-        {
-          if (face_data_cloud.size () == face_data_cloud_.size ())
-          {
-            face_data_cloud_ = face_data_cloud;
-            return (true);
-          }
-          return (false);
-        }
-
-        ////////////////////////////////////////////////////////////////////////
-        // getVertexIndex / getHalfEdgeIndex / getEdgeIndex / getFaceIndex
-        ////////////////////////////////////////////////////////////////////////
-
-        /** \brief Get the index associated to the given vertex data.
-          * \return Invalid index if the mesh does not have associated vertex data.
-          */
-        inline VertexIndex
-        getVertexIndex (const VertexData& vertex_data) const
-        {
-          if (HasVertexData::value)
-          {
-            assert (&vertex_data >= &vertex_data_cloud_.front () && &vertex_data <= &vertex_data_cloud_.back ());
-            return (VertexIndex (std::distance (&vertex_data_cloud_.front (), &vertex_data)));
-          }
-          return (VertexIndex ());
-        }
-
-        /** \brief Get the index associated to the given half-edge data. */
-        inline HalfEdgeIndex
-        getHalfEdgeIndex (const HalfEdgeData& half_edge_data) const
-        {
-          if (HasHalfEdgeData::value)
-          {
-            assert (&half_edge_data >= &half_edge_data_cloud_.front () && &half_edge_data <= &half_edge_data_cloud_.back ());
-            return (HalfEdgeIndex (std::distance (&half_edge_data_cloud_.front (), &half_edge_data)));
-          }
-          return (HalfEdgeIndex ());
-        }
-
-        /** \brief Get the index associated to the given edge data. */
-        inline EdgeIndex
-        getEdgeIndex (const EdgeData& edge_data) const
-        {
-          if (HasEdgeData::value)
-          {
-            assert (&edge_data >= &edge_data_cloud_.front () && &edge_data <= &edge_data_cloud_.back ());
-            return (EdgeIndex (std::distance (&edge_data_cloud_.front (), &edge_data)));
-          }
-          return (EdgeIndex ());
-        }
-
-        /** \brief Get the index associated to the given face data. */
-        inline FaceIndex
-        getFaceIndex (const FaceData& face_data) const
-        {
-          if (HasFaceData::value)
-          {
-            assert (&face_data >= &face_data_cloud_.front () && &face_data <= &face_data_cloud_.back ());
-            return (FaceIndex (std::distance (&face_data_cloud_.front (), &face_data)));
-          }
-          return (FaceIndex ());
-        }
-
-      protected:
-
-        ////////////////////////////////////////////////////////////////////////
-        // Types
-        ////////////////////////////////////////////////////////////////////////
+    do {
+      if (this->isBoundary(circ.getTargetIndex())) {
+        return (true);
+      }
+    } while (++circ != circ_end);
 
-        // Elements
-        using Vertex = pcl::geometry::Vertex;
-        using HalfEdge = pcl::geometry::HalfEdge;
-        using Face = pcl::geometry::Face;
-
-        using Vertices = std::vector<Vertex>;
-        using HalfEdges = std::vector<HalfEdge>;
-        using Faces = std::vector<Face>;
-
-        using VertexIterator = typename Vertices::iterator;
-        using HalfEdgeIterator = typename HalfEdges::iterator;
-        using FaceIterator = typename Faces::iterator;
-
-        using VertexConstIterator = typename Vertices::const_iterator;
-        using HalfEdgeConstIterator = typename HalfEdges::const_iterator;
-        using FaceConstIterator = typename Faces::const_iterator;
-
-        /** \brief General implementation of addFace. */
-        FaceIndex
-        addFaceImplBase (const VertexIndices& vertices,
-                         const FaceData&      face_data,
-                         const EdgeData&      edge_data,
-                         const HalfEdgeData&  half_edge_data)
-        {
-          const int n = static_cast<int> (vertices.size ());
-          if (n < 3) return (FaceIndex ());
-
-          // Check for topological errors
-          inner_he_.resize      (n);
-          free_he_.resize       (n);
-          is_new_.resize        (n);
-          make_adjacent_.resize (n);
-          for (int i=0; i<n; ++i)
-          {
-            if (!this->checkTopology1 (vertices [i], vertices [(i+1)%n], inner_he_ [i], is_new_ [i], IsManifold ()))
-            {
-              return (FaceIndex ());
-            }
-          }
-          for (int i=0; i<n; ++i)
-          {
-            int j = (i+1)%n;
-            if (!this->checkTopology2 (inner_he_ [i], inner_he_ [j], is_new_ [i], is_new_ [j], this->isIsolated (vertices [j]), make_adjacent_ [i], free_he_ [i], IsManifold ()))
-            {
-              return (FaceIndex ());
-            }
-          }
-
-          // Reconnect the existing half-edges if needed
-          if (!IsManifold::value)
-          {
-            for (int i=0; i<n; ++i)
-            {
-              if (make_adjacent_ [i])
-              {
-                this->makeAdjacent (inner_he_ [i], inner_he_ [(i+1)%n], free_he_ [i]);
-              }
-            }
-          }
-
-          // Add new half-edges if needed
-          for (int i=0; i<n; ++i)
-          {
-            if (is_new_ [i])
-            {
-              inner_he_ [i] = this->addEdge (vertices [i], vertices [(i+1)%n], half_edge_data, edge_data);
-            }
-          }
-
-          // Connect
-          for (int i=0; i<n; ++i)
-          {
-            int j = (i+1)%n;
-            if      ( is_new_ [i] &&  is_new_ [j]) this->connectNewNew (inner_he_ [i], inner_he_ [j], vertices [j], IsManifold ());
-            else if ( is_new_ [i] && !is_new_ [j]) this->connectNewOld (inner_he_ [i], inner_he_ [j], vertices [j]);
-            else if (!is_new_ [i] &&  is_new_ [j]) this->connectOldNew (inner_he_ [i], inner_he_ [j], vertices [j]);
-            else                                   this->connectOldOld (inner_he_ [i], inner_he_ [j], vertices [j], IsManifold ());
-          }
-          return (this->connectFace (inner_he_, face_data));
-        }
-
-        ////////////////////////////////////////////////////////////////////////
-        // addEdge
-        ////////////////////////////////////////////////////////////////////////
-
-        /** \brief Add an edge between the two given vertices and connect them with the vertices.
-          * \param[in]  idx_v_a   The first vertex index
-          * \param[in]  idx_v_b   The second vertex index
-          * \param[in]  he_data   Data associated with the half-edges. This is only added if the mesh has data associated with the half-edges.
-          * \param[in]  edge_data Data associated with the edge. This is only added if the mesh has data associated with the edges.
-          * \return Index to the half-edge from vertex a to vertex b.
-          */
-        HalfEdgeIndex
-        addEdge (const VertexIndex&  idx_v_a,
-                 const VertexIndex&  idx_v_b,
-                 const HalfEdgeData& he_data,
-                 const EdgeData&     edge_data)
-        {
-          half_edges_.push_back (HalfEdge (idx_v_b));
-          half_edges_.push_back (HalfEdge (idx_v_a));
-
-          this->addData (half_edge_data_cloud_, he_data  , HasHalfEdgeData ());
-          this->addData (half_edge_data_cloud_, he_data  , HasHalfEdgeData ());
-          this->addData (edge_data_cloud_     , edge_data, HasEdgeData     ());
-
-          return (HalfEdgeIndex (static_cast <int> (half_edges_.size () - 2)));
-        }
-
-        ////////////////////////////////////////////////////////////////////////
-        // topology checks
-        ////////////////////////////////////////////////////////////////////////
-
-        /** \brief Check if the edge between the two vertices can be added.
-          * \param[in]  idx_v_a   Index to the first vertex.
-          * \param[in]  idx_v_b   Index to the second vertex.
-          * \param[out] idx_he_ab Index to the half-edge ab if is_new_ab=false.
-          * \param[out] is_new_ab true if the edge between the vertices exists already. Must be initialized with true!
-          * \return true if the half-edge may be added.
-          */
-        bool
-        checkTopology1 (const VertexIndex&            idx_v_a,
-                        const VertexIndex&            idx_v_b,
-                        HalfEdgeIndex&                idx_he_ab,
-                        std::vector <bool>::reference is_new_ab,
-                        std::true_type              /*is_manifold*/) const
-        {
-          is_new_ab = true;
-          if (this->isIsolated (idx_v_a)) return (true);
-
-          idx_he_ab = this->getOutgoingHalfEdgeIndex (idx_v_a);
-
-          if (!this->isBoundary (idx_he_ab))                          return (false);
-          if (this->getTerminatingVertexIndex (idx_he_ab) == idx_v_b) is_new_ab = false;
-          return (true);
-        }
-
-        /** \brief Non manifold version of checkTopology1 */
-        bool
-        checkTopology1 (const VertexIndex&            idx_v_a,
-                        const VertexIndex&            idx_v_b,
-                        HalfEdgeIndex&                idx_he_ab,
-                        std::vector <bool>::reference is_new_ab,
-                        std::false_type              /*is_manifold*/) const
-        {
-          is_new_ab = true;
-          if (this->isIsolated (idx_v_a))                                   return (true);
-          if (!this->isBoundary (this->getOutgoingHalfEdgeIndex (idx_v_a))) return (false);
-
-          VertexAroundVertexCirculator       circ     = this->getVertexAroundVertexCirculator (this->getOutgoingHalfEdgeIndex (idx_v_a));
-          const VertexAroundVertexCirculator circ_end = circ;
-
-          do
-          {
-            if (circ.getTargetIndex () == idx_v_b)
-            {
-              idx_he_ab = circ.getCurrentHalfEdgeIndex ();
-              if (!this->isBoundary (idx_he_ab)) return (false);
-
-              is_new_ab = false;
-              return (true);
-            }
-          } while (++circ!=circ_end);
-
-          return (true);
-        }
-
-        /** \brief Check if the face may be added (mesh does not become non-manifold). */
-        inline bool
-        checkTopology2 (const HalfEdgeIndex&          /*idx_he_ab*/,
-                        const HalfEdgeIndex&          /*idx_he_bc*/,
-                        const bool                    is_new_ab,
-                        const bool                    is_new_bc,
-                        const bool                    is_isolated_b,
-                        std::vector <bool>::reference /*make_adjacent_ab_bc*/,
-                        HalfEdgeIndex&                /*idx_free_half_edge*/,
-                        std::true_type              /*is_manifold*/) const
-        {
-          return !(is_new_ab && is_new_bc && !is_isolated_b);
-        }
-
-        /** \brief Check if the half-edge bc is the next half-edge of ab.
-          * \param[in]  idx_he_ab           Index to the half-edge between the vertices a and b.
-          * \param[in]  idx_he_bc           Index to the half-edge between the vertices b and c.
-          * \param[in]  is_new_ab           Half-edge ab is new.
-          * \param[in]  is_new_bc           Half-edge bc is new.
-          * \param[out] make_adjacent_ab_bc Half-edges ab and bc need to be made adjacent.
-          * \param[out] idx_free_half_edge  Free half-edge (needed for makeAdjacent)
-          * \return true if addFace may be continued.
-          */
-        inline bool
-        checkTopology2 (const HalfEdgeIndex&          idx_he_ab,
-                        const HalfEdgeIndex&          idx_he_bc,
-                        const bool                    is_new_ab,
-                        const bool                    is_new_bc,
-                        const bool                    /*is_isolated_b*/,
-                        std::vector <bool>::reference make_adjacent_ab_bc,
-                        HalfEdgeIndex&                idx_free_half_edge,
-                        std::false_type             /*is_manifold*/) const
-        {
-          if (is_new_ab || is_new_bc)
-          {
-            make_adjacent_ab_bc = false;
-            return (true); // Make adjacent is only needed for two old half-edges
-          }
-
-          if (this->getNextHalfEdgeIndex (idx_he_ab) == idx_he_bc)
-          {
-            make_adjacent_ab_bc = false;
-            return (true); // already adjacent
-          }
-
-          make_adjacent_ab_bc = true;
-
-          // Find the next boundary half edge
-          IncomingHalfEdgeAroundVertexCirculator circ = this->getIncomingHalfEdgeAroundVertexCirculator (this->getOppositeHalfEdgeIndex (idx_he_bc));
-
-          do ++circ; while (!this->isBoundary (circ.getTargetIndex ()));
-          idx_free_half_edge = circ.getTargetIndex ();
-
-          // This would detach the faces around the vertex from each other.
-          return (circ.getTargetIndex () != idx_he_ab);
-        }
-
-        /** \brief Make the half-edges bc the next half-edge of ab.
-          * \param[in]      idx_he_ab          Index to the half-edge between the vertices a and b.
-          * \param[in]      idx_he_bc          Index to the half-edge between the vertices b and c.
-          * \param[in, out] idx_free_half_edge Free half-edge needed to re-connect the half-edges around vertex b.
-          */
-        void
-        makeAdjacent (const HalfEdgeIndex& idx_he_ab,
-                      const HalfEdgeIndex& idx_he_bc,
-                      HalfEdgeIndex&       idx_free_half_edge)
-        {
-          // Re-link. No references!
-          const HalfEdgeIndex idx_he_ab_next   = this->getNextHalfEdgeIndex (idx_he_ab);
-          const HalfEdgeIndex idx_he_bc_prev   = this->getPrevHalfEdgeIndex (idx_he_bc);
-          const HalfEdgeIndex idx_he_free_next = this->getNextHalfEdgeIndex (idx_free_half_edge);
-
-          this->connectPrevNext (idx_he_ab,          idx_he_bc);
-          this->connectPrevNext (idx_free_half_edge, idx_he_ab_next);
-          this->connectPrevNext (idx_he_bc_prev,     idx_he_free_next);
-        }
-
-        ////////////////////////////////////////////////////////////////////////
-        // connect
-        ////////////////////////////////////////////////////////////////////////
-
-        /** \brief Add a face to the mesh and connect it to the half-edges.
-          * \param[in] inner_he  Inner half-edges of the face.
-          * \param[in] face_data Data that is stored in the face. This is only added if the mesh has data associated with the faces.
-          * \return Index to the new face.
-          */
-        FaceIndex
-        connectFace (const HalfEdgeIndices& inner_he,
-                     const FaceData&        face_data)
-        {
-          faces_.push_back (Face (inner_he.back ()));
-          this->addData (face_data_cloud_, face_data, HasFaceData ());
-
-          const FaceIndex idx_face (static_cast <int> (this->sizeFaces () - 1));
-
-          for (const auto &idx_half_edge : inner_he)
-          {
-            this->setFaceIndex (idx_half_edge, idx_face);
-          }
-
-          return (idx_face);
-        }
-
-        /** \brief Connect the next and prev indices of the two half-edges with each other. */
-        inline void
-        connectPrevNext (const HalfEdgeIndex& idx_he_ab,
-                         const HalfEdgeIndex& idx_he_bc)
-        {
-          this->setNextHalfEdgeIndex (idx_he_ab, idx_he_bc);
-          this->setPrevHalfEdgeIndex (idx_he_bc, idx_he_ab);
-        }
-
-        /** \brief Both half-edges are new (manifold version). */
-        void
-        connectNewNew (const HalfEdgeIndex& idx_he_ab,
-                       const HalfEdgeIndex& idx_he_bc,
-                       const VertexIndex&   idx_v_b,
-                       std::true_type     /*is_manifold*/)
-        {
-          const HalfEdgeIndex idx_he_ba = this->getOppositeHalfEdgeIndex (idx_he_ab);
-          const HalfEdgeIndex idx_he_cb = this->getOppositeHalfEdgeIndex (idx_he_bc);
-
-          this->connectPrevNext (idx_he_ab, idx_he_bc);
-          this->connectPrevNext (idx_he_cb, idx_he_ba);
-
-          this->setOutgoingHalfEdgeIndex (idx_v_b, idx_he_ba);
-        }
-
-        /** \brief Both half-edges are new (non-manifold version). */
-        void
-        connectNewNew (const HalfEdgeIndex& idx_he_ab,
-                       const HalfEdgeIndex& idx_he_bc,
-                       const VertexIndex&   idx_v_b,
-                       std::false_type    /*is_manifold*/)
-        {
-          if (this->isIsolated (idx_v_b))
-          {
-            this->connectNewNew (idx_he_ab, idx_he_bc, idx_v_b, std::true_type ());
-          }
-          else
-          {
-            const HalfEdgeIndex idx_he_ba = this->getOppositeHalfEdgeIndex (idx_he_ab);
-            const HalfEdgeIndex idx_he_cb = this->getOppositeHalfEdgeIndex (idx_he_bc);
-
-            // No references!
-            const HalfEdgeIndex idx_he_b_out      = this->getOutgoingHalfEdgeIndex (idx_v_b);
-            const HalfEdgeIndex idx_he_b_out_prev = this->getPrevHalfEdgeIndex (idx_he_b_out);
-
-            this->connectPrevNext (idx_he_ab,         idx_he_bc);
-            this->connectPrevNext (idx_he_cb,         idx_he_b_out);
-            this->connectPrevNext (idx_he_b_out_prev, idx_he_ba);
-          }
-        }
-
-        /** \brief The first half-edge is new. */
-        void
-        connectNewOld (const HalfEdgeIndex& idx_he_ab,
-                       const HalfEdgeIndex& idx_he_bc,
-                       const VertexIndex&   idx_v_b)
-        {
-          const HalfEdgeIndex idx_he_ba      = this->getOppositeHalfEdgeIndex (idx_he_ab);
-          const HalfEdgeIndex idx_he_bc_prev = this->getPrevHalfEdgeIndex (idx_he_bc); // No reference!
-
-          this->connectPrevNext (idx_he_ab,      idx_he_bc);
-          this->connectPrevNext (idx_he_bc_prev, idx_he_ba);
-
-          this->setOutgoingHalfEdgeIndex (idx_v_b, idx_he_ba);
-        }
-
-        /** \brief The second half-edge is new. */
-        void
-        connectOldNew (const HalfEdgeIndex& idx_he_ab,
-                       const HalfEdgeIndex& idx_he_bc,
-                       const VertexIndex&   idx_v_b)
-        {
-          const HalfEdgeIndex idx_he_cb      = this->getOppositeHalfEdgeIndex (idx_he_bc);
-          const HalfEdgeIndex idx_he_ab_next = this->getNextHalfEdgeIndex (idx_he_ab); // No reference!
-
-          this->connectPrevNext (idx_he_ab, idx_he_bc);
-          this->connectPrevNext (idx_he_cb, idx_he_ab_next);
-
-          this->setOutgoingHalfEdgeIndex (idx_v_b, idx_he_ab_next);
-        }
-
-        /** \brief Both half-edges are old (manifold version). */
-        void
-        connectOldOld (const HalfEdgeIndex& /*idx_he_ab*/,
-                       const HalfEdgeIndex& /*idx_he_bc*/,
-                       const VertexIndex&   /*idx_v_b*/,
-                       std::true_type     /*is_manifold*/)
-        {
-        }
-
-        /** \brief Both half-edges are old (non-manifold version). */
-        void
-        connectOldOld (const HalfEdgeIndex& /*idx_he_ab*/,
-                       const HalfEdgeIndex& idx_he_bc,
-                       const VertexIndex&   idx_v_b,
-                       std::false_type    /*is_manifold*/)
-        {
-          const HalfEdgeIndex& idx_he_b_out = this->getOutgoingHalfEdgeIndex (idx_v_b);
-
-          // The outgoing half edge MUST be a boundary half-edge (if there is one)
-          if (idx_he_b_out == idx_he_bc) // he_bc is no longer on the boundary
-          {
-            OutgoingHalfEdgeAroundVertexCirculator       circ     = this->getOutgoingHalfEdgeAroundVertexCirculator (idx_he_b_out);
-            const OutgoingHalfEdgeAroundVertexCirculator circ_end = circ;
-
-            while (++circ!=circ_end)
-            {
-              if (this->isBoundary (circ.getTargetIndex ()))
-              {
-                this->setOutgoingHalfEdgeIndex (idx_v_b, circ.getTargetIndex ());
-                return;
-              }
-            }
-          }
-        }
-
-        ////////////////////////////////////////////////////////////////////////
-        // addData
-        ////////////////////////////////////////////////////////////////////////
-
-        /** \brief Add mesh data. */
-        template <class DataT>
-        inline void
-        addData (pcl::PointCloud <DataT>& cloud, const DataT& data, std::true_type /*has_data*/)
-        {
-          cloud.push_back (data);
-        }
-
-        /** \brief Does nothing. */
-        template <class DataT>
-        inline void
-        addData (pcl::PointCloud <DataT>& /*cloud*/, const DataT& /*data*/, std::false_type /*has_data*/)
-        {
-        }
+    return (false);
+  }
 
-        ////////////////////////////////////////////////////////////////////////
-        // deleteFace
-        ////////////////////////////////////////////////////////////////////////
-
-        /** \brief Manifold version of deleteFace. If the mesh becomes non-manifold due to the delete operation the faces around the non-manifold vertex are deleted until the mesh becomes manifold again. */
-        void
-        deleteFace (const FaceIndex& idx_face,
-                    std::true_type /*is_manifold*/)
-        {
-          assert (this->isValid (idx_face));
-          delete_faces_face_.clear ();
-          delete_faces_face_.push_back (idx_face);
-
-          while (!delete_faces_face_.empty ())
-          {
-            const FaceIndex idx_face_cur = delete_faces_face_.back ();
-            delete_faces_face_.pop_back ();
-
-            // This calls the non-manifold version of deleteFace, which will call the manifold version of reconnect.
-            this->deleteFace (idx_face_cur, std::false_type ());
-          }
-        }
-
-        /** \brief Non-manifold version of deleteFace. */
-        void
-        deleteFace (const FaceIndex&  idx_face,
-                    std::false_type /*is_manifold*/)
-        {
-          assert (this->isValid (idx_face));
-          if (this->isDeleted (idx_face)) return;
-
-          // Store all half-edges in the face
-          inner_he_.clear ();
-          is_boundary_.clear ();
-          InnerHalfEdgeAroundFaceCirculator       circ     = this->getInnerHalfEdgeAroundFaceCirculator (idx_face);
-          const InnerHalfEdgeAroundFaceCirculator circ_end = circ;
-          do
-          {
-            inner_he_.push_back (circ.getTargetIndex ());
-            is_boundary_.push_back (this->isBoundary (this->getOppositeHalfEdgeIndex (circ.getTargetIndex ())));
-          } while (++circ != circ_end);
-          assert (inner_he_.size () >= 3); // Minimum should be a triangle.
-
-          const int n = static_cast <int> (inner_he_.size ());
-          int j;
-
-          if (IsManifold::value)
-          {
-            for (int i=0; i<n; ++i)
-            {
-              j = (i+1)%n;
-              this->reconnect (inner_he_ [i], inner_he_ [j], is_boundary_ [i], is_boundary_ [j]);
-            }
-            for (int i=0; i<n; ++i)
-            {
-              this->getHalfEdge (inner_he_ [i]).idx_face_.invalidate ();
-            }
-          }
-          else
-          {
-            for (int i=0; i<n; ++i)
-            {
-              j = (i+1)%n;
-              this->reconnect (inner_he_ [i], inner_he_ [j], is_boundary_ [i], is_boundary_ [j]);
-              this->getHalfEdge (inner_he_ [i]).idx_face_.invalidate ();
-            }
-          }
-
-          this->markDeleted (idx_face);
-        }
-
-        ////////////////////////////////////////////////////////////////////////
-        // reconnect
-        ////////////////////////////////////////////////////////////////////////
-
-        /** \brief Deconnect the input half-edges from the mesh and adjust the indices of the connected half-edges. */
-        void
-        reconnect (const HalfEdgeIndex& idx_he_ab,
-                   const HalfEdgeIndex& idx_he_bc,
-                   const bool           is_boundary_ba,
-                   const bool           is_boundary_cb)
-        {
-          const HalfEdgeIndex idx_he_ba = this->getOppositeHalfEdgeIndex  (idx_he_ab);
-          const HalfEdgeIndex idx_he_cb = this->getOppositeHalfEdgeIndex  (idx_he_bc);
-          const VertexIndex   idx_v_b   = this->getTerminatingVertexIndex (idx_he_ab);
-
-          if (is_boundary_ba && is_boundary_cb) // boundary - boundary
-          {
-            const HalfEdgeIndex& idx_he_cb_next = this->getNextHalfEdgeIndex (idx_he_cb);
-
-            if (idx_he_cb_next == idx_he_ba) // Vertex b is isolated
-            {
-              this->markDeleted (idx_v_b);
-            }
-            else
-            {
-              this->connectPrevNext (this->getPrevHalfEdgeIndex (idx_he_ba), idx_he_cb_next);
-              this->setOutgoingHalfEdgeIndex (idx_v_b, idx_he_cb_next);
-            }
-
-            this->markDeleted (idx_he_ab);
-            this->markDeleted (idx_he_ba);
-          }
-          else if (is_boundary_ba && !is_boundary_cb) // boundary - no boundary
-          {
-            this->connectPrevNext (this->getPrevHalfEdgeIndex (idx_he_ba), idx_he_bc);
-            this->setOutgoingHalfEdgeIndex (idx_v_b, idx_he_bc);
-
-            this->markDeleted (idx_he_ab);
-            this->markDeleted (idx_he_ba);
-          }
-          else if (!is_boundary_ba && is_boundary_cb) // no boundary - boundary
-          {
-            const HalfEdgeIndex& idx_he_cb_next = this->getNextHalfEdgeIndex (idx_he_cb);
-            this->connectPrevNext (idx_he_ab, idx_he_cb_next);
-            this->setOutgoingHalfEdgeIndex (idx_v_b, idx_he_cb_next);
-          }
-          else // no boundary - no boundary
-          {
-            this->reconnectNBNB (idx_he_bc, idx_he_cb, idx_v_b, IsManifold ());
-          }
-        }
-
-        /** \brief Both edges are not on the boundary. Manifold version. */
-        void
-        reconnectNBNB (const HalfEdgeIndex& idx_he_bc,
-                       const HalfEdgeIndex& idx_he_cb,
-                       const VertexIndex&   idx_v_b,
-                       std::true_type     /*is_manifold*/)
-        {
-          if (this->isBoundary (idx_v_b))
-          {
-            // Deletion of this face makes the mesh non-manifold
-            // -> delete the neighboring faces until it is manifold again
-            IncomingHalfEdgeAroundVertexCirculator circ = this->getIncomingHalfEdgeAroundVertexCirculator (idx_he_cb);
-
-            while (!this->isBoundary (circ.getTargetIndex ()))
-            {
-              delete_faces_face_.push_back (this->getFaceIndex ((circ++).getTargetIndex ()));
-
-#ifdef PCL_GEOMETRY_MESH_BASE_TEST_DELETE_FACE_MANIFOLD_2
-              if (circ == this->getIncomingHalfEdgeAroundVertexCirculator (idx_he_cb)) // Abort infinity loop
-              {
-                // In a manifold mesh we can't invalidate the face while reconnecting!
-                // See the implementation of
-                // deleteFace (const FaceIndex&  idx_face,
-                //             std::false_type /*is_manifold*/)
-                pcl::geometry::g_pcl_geometry_mesh_base_test_delete_face_manifold_2_success = false;
-                return;
-              }
-#endif
-            }
-          }
-          else
-          {
-            this->setOutgoingHalfEdgeIndex (idx_v_b, idx_he_bc);
-          }
-        }
-
-        /** \brief Both edges are not on the boundary. Non-manifold version. */
-        void
-        reconnectNBNB (const HalfEdgeIndex& idx_he_bc,
-                       const HalfEdgeIndex& /*idx_he_cb*/,
-                       const VertexIndex&   idx_v_b,
-                       std::false_type    /*is_manifold*/)
-        {
-          if (!this->isBoundary (idx_v_b))
-          {
-            this->setOutgoingHalfEdgeIndex (idx_v_b, idx_he_bc);
-          }
-        }
-
-        ////////////////////////////////////////////////////////////////////////
-        // markDeleted
-        ////////////////////////////////////////////////////////////////////////
-
-        /** \brief Mark the given vertex as deleted. */
-        inline void
-        markDeleted (const VertexIndex& idx_vertex)
-        {
-          assert (this->isValid (idx_vertex));
-          this->getVertex (idx_vertex).idx_outgoing_half_edge_.invalidate ();
-        }
-
-        /** \brief Mark the given half-edge as deleted. */
-        inline void
-        markDeleted (const HalfEdgeIndex& idx_he)
-        {
-          assert (this->isValid (idx_he));
-          this->getHalfEdge (idx_he).idx_terminating_vertex_.invalidate ();
-        }
-
-        /** \brief Mark the given edge (both half-edges) as deleted. */
-        inline void
-        markDeleted (const EdgeIndex& idx_edge)
-        {
-          assert (this->isValid (idx_edge));
-          this->markDeleted (pcl::geometry::toHalfEdgeIndex (idx_edge, true));
-          this->markDeleted (pcl::geometry::toHalfEdgeIndex (idx_edge, false));
-        }
-
-        /** \brief Mark the given face as deleted. */
-        inline void
-        markDeleted (const FaceIndex& idx_face)
-        {
-          assert (this->isValid (idx_face));
-          this->getFace (idx_face).idx_inner_half_edge_.invalidate ();
-        }
-
-        ////////////////////////////////////////////////////////////////////////
-        // For cleanUp
-        ////////////////////////////////////////////////////////////////////////
-
-        /** \brief Removes mesh elements and data that are marked as deleted from the container.
-          * \tparam ElementContainerT e.g. std::vector \<Vertex\>
-          * \tparam DataContainerT    e.g. std::vector \<VertexData\>
-          * \tparam IndexContainerT   e.g. std::vector \<VertexIndex\>
-          * \tparam HasDataT          Integral constant specifying if the mesh has data associated with the elements.
-          *
-          * \param[in, out] elements Container for the mesh elements. Resized to the new size.
-          * \param[in, out] data_cloud Container for the mesh data. Resized to the new size.
-          * \return Container with the same size as the old input data. Holds the indices to the new elements for each non-deleted element and an invalid index if it is deleted.
-          */
-        template <class ElementContainerT, class DataContainerT, class IndexContainerT, class HasDataT> IndexContainerT
-        remove (ElementContainerT& elements, DataContainerT& data_cloud)
-        {
-          using Index = typename IndexContainerT::value_type;
-          using Element = typename ElementContainerT::value_type;
-
-          if (HasDataT::value) assert (elements.size () == data_cloud.size ());
-          else                 assert (data_cloud.empty ()); // Bug in this class!
-
-          IndexContainerT new_indices (elements.size (), typename IndexContainerT::value_type ());
-          Index ind_old (0), ind_new (0);
-
-          typename ElementContainerT::const_iterator it_e_old = elements.begin ();
-          typename ElementContainerT::iterator       it_e_new = elements.begin ();
-
-          typename DataContainerT::const_iterator it_d_old = data_cloud.begin ();
-          typename DataContainerT::iterator       it_d_new = data_cloud.begin ();
-
-          typename IndexContainerT::iterator       it_ind_new     = new_indices.begin ();
-          typename IndexContainerT::const_iterator it_ind_new_end = new_indices.end ();
-
-          while (it_ind_new!=it_ind_new_end)
-          {
-            if (!this->isDeleted (ind_old))
-            {
-              *it_ind_new = ind_new++;
-
-              // TODO: Test for self assignment?
-              *it_e_new++ = *it_e_old;
-              this->assignIf    (it_d_old, it_d_new, HasDataT ());
-              this->incrementIf (          it_d_new, HasDataT ());
-            }
-            ++ind_old;
-            ++it_e_old;
-            this->incrementIf (it_d_old, HasDataT ());
-            ++it_ind_new;
-          }
-
-          elements.resize (ind_new.get (), Element ());
-          if (HasDataT::value)
-          {
-            data_cloud.resize (ind_new.get ());
-          }
-          else if (it_d_old != data_cloud.begin () || it_d_new != data_cloud.begin ())
-          {
-            std::cerr << "TODO: Bug in MeshBase::remove!\n";
-            assert (false);
-            exit (EXIT_FAILURE);
-          }
-
-          return (new_indices);
-        }
-
-        /** \brief Increment the iterator. */
-        template <class IteratorT> inline void
-        incrementIf (IteratorT& it, std::true_type /*has_data*/) const
-        {
-          ++it;
-        }
-
-        /** \brief Does nothing. */
-        template <class IteratorT> inline void
-        incrementIf (IteratorT& /*it*/, std::false_type /*has_data*/) const
-        {
-        }
-
-        /** \brief Assign the source iterator to the target iterator. */
-        template <class ConstIteratorT, class IteratorT> inline void
-        assignIf (const ConstIteratorT source, IteratorT target, std::true_type /*has_data*/) const
-        {
-          *target = *source;
-        }
-
-        /** \brief Does nothing. */
-        template <class ConstIteratorT, class IteratorT> inline void
-        assignIf (const ConstIteratorT /*source*/, IteratorT /*target*/, std::false_type /*has_data*/) const
-        {
-        }
-
-        ////////////////////////////////////////////////////////////////////////
-        // Vertex / Half-edge / Face connectivity
-        ////////////////////////////////////////////////////////////////////////
-
-        /** \brief Set the outgoing half-edge index to a given vertex. */
-        inline void
-        setOutgoingHalfEdgeIndex (const VertexIndex& idx_vertex, const HalfEdgeIndex& idx_outgoing_half_edge)
-        {
-          assert (this->isValid (idx_vertex));
-          this->getVertex (idx_vertex).idx_outgoing_half_edge_ = idx_outgoing_half_edge;
-        }
-
-        /** \brief Set the terminating vertex index to a given half-edge. */
-        inline void
-        setTerminatingVertexIndex (const HalfEdgeIndex& idx_half_edge, const VertexIndex& idx_terminating_vertex)
-        {
-          assert (this->isValid (idx_half_edge));
-          this->getHalfEdge (idx_half_edge).idx_terminating_vertex_ = idx_terminating_vertex;
-        }
-
-        /** \brief Set the next half_edge index to a given half-edge. */
-        inline void
-        setNextHalfEdgeIndex (const HalfEdgeIndex& idx_half_edge, const HalfEdgeIndex& idx_next_half_edge)
-        {
-          assert (this->isValid (idx_half_edge));
-          this->getHalfEdge (idx_half_edge).idx_next_half_edge_ = idx_next_half_edge;
-        }
-
-        /** \brief Set the previous half-edge index to a given half-edge. */
-        inline void
-        setPrevHalfEdgeIndex (const HalfEdgeIndex& idx_half_edge,
-                              const HalfEdgeIndex& idx_prev_half_edge)
-        {
-          assert (this->isValid (idx_half_edge));
-          this->getHalfEdge (idx_half_edge).idx_prev_half_edge_ = idx_prev_half_edge;
-        }
-
-        /** \brief Set the face index to a given half-edge. */
-        inline void
-        setFaceIndex (const HalfEdgeIndex& idx_half_edge, const FaceIndex& idx_face)
-        {
-          assert (this->isValid (idx_half_edge));
-          this->getHalfEdge (idx_half_edge).idx_face_ = idx_face;
-        }
-
-        /** \brief Set the inner half-edge index to a given face. */
-        inline void
-        setInnerHalfEdgeIndex (const FaceIndex& idx_face, const HalfEdgeIndex& idx_inner_half_edge)
-        {
-          assert (this->isValid (idx_face));
-          this->getFace (idx_face).idx_inner_half_edge_ = idx_inner_half_edge;
-        }
-
-        ////////////////////////////////////////////////////////////////////////
-        // isBoundary / isManifold
-        ////////////////////////////////////////////////////////////////////////
-
-        /** \brief Check if any vertex of the face lies on the boundary. */
-        bool
-        isBoundary (const FaceIndex& idx_face, std::true_type /*check_vertices*/) const
-        {
-          VertexAroundFaceCirculator       circ     = this->getVertexAroundFaceCirculator (idx_face);
-          const VertexAroundFaceCirculator circ_end = circ;
-
-          do
-          {
-            if (this->isBoundary (circ.getTargetIndex ()))
-            {
-              return (true);
-            }
-          } while (++circ!=circ_end);
-
-          return (false);
-        }
-
-        /** \brief Check if any edge of the face lies on the boundary. */
-        bool
-        isBoundary (const FaceIndex& idx_face, std::false_type /*check_vertices*/) const
-        {
-          OuterHalfEdgeAroundFaceCirculator       circ     = this->getOuterHalfEdgeAroundFaceCirculator (idx_face);
-          const OuterHalfEdgeAroundFaceCirculator circ_end = circ;
-
-          do
-          {
-            if (this->isBoundary (circ.getTargetIndex ()))
-            {
-              return (true);
-            }
-          } while (++circ!=circ_end);
-
-          return (false);
-        }
-
-        /** \brief Always manifold. */
-        inline bool
-        isManifold (const VertexIndex&, std::true_type /*is_manifold*/) const
-        {
-          return (true);
-        }
-
-        /** \brief Check if the given vertex is manifold. */
-        bool
-        isManifold (const VertexIndex& idx_vertex, std::false_type /*is_manifold*/) const
-        {
-          OutgoingHalfEdgeAroundVertexCirculator       circ     = this->getOutgoingHalfEdgeAroundVertexCirculator (idx_vertex);
-          const OutgoingHalfEdgeAroundVertexCirculator circ_end = circ;
-
-          if (!this->isBoundary ((circ++).getTargetIndex ())) return (true);
-          do
-          {
-            if (this->isBoundary (circ.getTargetIndex ())) return (false);
-          } while (++circ != circ_end);
-
-          return (true);
-        }
-
-        /** \brief Always manifold. */
-        inline bool
-        isManifold (std::true_type /*is_manifold*/) const
-        {
-          return (true);
-        }
-
-        /** \brief Check if all vertices in the mesh are manifold. */
-        bool
-        isManifold (std::false_type /*is_manifold*/) const
-        {
-          for (std::size_t i=0; i<this->sizeVertices (); ++i)
-          {
-            if (!this->isManifold (VertexIndex (i))) return (false);
-          }
-          return (true);
-        }
-
-        ////////////////////////////////////////////////////////////////////////
-        // reserveData / resizeData / clearData
-        ////////////////////////////////////////////////////////////////////////
-
-        /** \brief Reserve storage space for the mesh data. */
-        template <class DataCloudT> inline void
-        reserveData (DataCloudT& cloud, const std::size_t n, std::true_type /*has_data*/) const
-        {
-          cloud.reserve (n);
-        }
-
-        /** \brief Does nothing */
-        template <class DataCloudT> inline void
-        reserveData (DataCloudT& /*cloud*/, const std::size_t /*n*/, std::false_type /*has_data*/) const
-        {
-        }
-
-        /** \brief Resize the mesh data. */
-        template <class DataCloudT> inline void
-        resizeData (DataCloudT& /*data_cloud*/, const std::size_t n, const typename DataCloudT::value_type& data, std::true_type /*has_data*/) const
-        {
-          data.resize (n, data);
-        }
-
-        /** \brief Does nothing. */
-        template <class DataCloudT> inline void
-        resizeData (DataCloudT& /*data_cloud*/, const std::size_t /*n*/, const typename DataCloudT::value_type& /*data*/, std::false_type /*has_data*/) const
-        {
-        }
-
-        /** \brief Clear the mesh data. */
-        template <class DataCloudT> inline void
-        clearData (DataCloudT& cloud, std::true_type /*has_data*/) const
-        {
-          cloud.clear ();
-        }
+  /** \brief Check if any edge of the face lies on the boundary. */
+  bool
+  isBoundary(const FaceIndex& idx_face, std::false_type /*check_vertices*/) const
+  {
+    OuterHalfEdgeAroundFaceCirculator circ =
+        this->getOuterHalfEdgeAroundFaceCirculator(idx_face);
+    const OuterHalfEdgeAroundFaceCirculator circ_end = circ;
+
+    do {
+      if (this->isBoundary(circ.getTargetIndex())) {
+        return (true);
+      }
+    } while (++circ != circ_end);
+
+    return (false);
+  }
+
+  /** \brief Always manifold. */
+  inline bool
+  isManifold(const VertexIndex&, std::true_type /*is_manifold*/) const
+  {
+    return (true);
+  }
 
-        /** \brief Does nothing. */
-        template <class DataCloudT> inline void
-        clearData (DataCloudT& /*cloud*/, std::false_type /*has_data*/) const
-        {
-        }
+  /** \brief Check if the given vertex is manifold. */
+  bool
+  isManifold(const VertexIndex& idx_vertex, std::false_type /*is_manifold*/) const
+  {
+    OutgoingHalfEdgeAroundVertexCirculator circ =
+        this->getOutgoingHalfEdgeAroundVertexCirculator(idx_vertex);
+    const OutgoingHalfEdgeAroundVertexCirculator circ_end = circ;
 
-        ////////////////////////////////////////////////////////////////////////
-        // get / set Vertex
-        ////////////////////////////////////////////////////////////////////////
+    if (!this->isBoundary((circ++).getTargetIndex()))
+      return (true);
+    do {
+      if (this->isBoundary(circ.getTargetIndex()))
+        return (false);
+    } while (++circ != circ_end);
 
-        /** \brief Get the vertex for the given index. */
-        inline Vertex&
-        getVertex (const VertexIndex& idx_vertex)
-        {
-          assert (this->isValid (idx_vertex));
-          return (vertices_ [idx_vertex.get ()]);
-        }
+    return (true);
+  }
 
-        /** \brief Get the vertex for the given index. */
-        inline Vertex
-        getVertex (const VertexIndex& idx_vertex) const
-        {
-          assert (this->isValid (idx_vertex));
-          return (vertices_ [idx_vertex.get ()]);
-        }
+  /** \brief Always manifold. */
+  inline bool isManifold(std::true_type /*is_manifold*/) const { return (true); }
 
-        /** \brief Set the vertex at the given index. */
-        inline void
-        setVertex (const VertexIndex& idx_vertex, const Vertex& vertex)
-        {
-          assert (this->isValid (idx_vertex));
-          vertices_ [idx_vertex.get ()] = vertex;
-        }
+  /** \brief Check if all vertices in the mesh are manifold. */
+  bool isManifold(std::false_type /*is_manifold*/) const
+  {
+    for (std::size_t i = 0; i < this->sizeVertices(); ++i) {
+      if (!this->isManifold(VertexIndex(i)))
+        return (false);
+    }
+    return (true);
+  }
+
+  ////////////////////////////////////////////////////////////////////////
+  // reserveData / resizeData / clearData
+  ////////////////////////////////////////////////////////////////////////
+
+  /** \brief Reserve storage space for the mesh data. */
+  template <class DataCloudT>
+  inline void
+  reserveData(DataCloudT& cloud, const std::size_t n, std::true_type /*has_data*/) const
+  {
+    cloud.reserve(n);
+  }
+
+  /** \brief Does nothing */
+  template <class DataCloudT>
+  inline void
+  reserveData(DataCloudT& /*cloud*/,
+              const std::size_t /*n*/,
+              std::false_type /*has_data*/) const
+  {}
+
+  /** \brief Resize the mesh data. */
+  template <class DataCloudT>
+  inline void
+  resizeData(DataCloudT& /*data_cloud*/,
+             const std::size_t n,
+             const typename DataCloudT::value_type& data,
+             std::true_type /*has_data*/) const
+  {
+    data.resize(n, data);
+  }
+
+  /** \brief Does nothing. */
+  template <class DataCloudT>
+  inline void
+  resizeData(DataCloudT& /*data_cloud*/,
+             const std::size_t /*n*/,
+             const typename DataCloudT::value_type& /*data*/,
+             std::false_type /*has_data*/) const
+  {}
+
+  /** \brief Clear the mesh data. */
+  template <class DataCloudT>
+  inline void
+  clearData(DataCloudT& cloud, std::true_type /*has_data*/) const
+  {
+    cloud.clear();
+  }
+
+  /** \brief Does nothing. */
+  template <class DataCloudT>
+  inline void
+  clearData(DataCloudT& /*cloud*/, std::false_type /*has_data*/) const
+  {}
+
+  ////////////////////////////////////////////////////////////////////////
+  // get / set Vertex
+  ////////////////////////////////////////////////////////////////////////
+
+  /** \brief Get the vertex for the given index. */
+  inline Vertex&
+  getVertex(const VertexIndex& idx_vertex)
+  {
+    assert(this->isValid(idx_vertex));
+    return (vertices_[idx_vertex.get()]);
+  }
 
-        ////////////////////////////////////////////////////////////////////////
-        // get / set HalfEdge
-        ////////////////////////////////////////////////////////////////////////
+  /** \brief Get the vertex for the given index. */
+  inline Vertex
+  getVertex(const VertexIndex& idx_vertex) const
+  {
+    assert(this->isValid(idx_vertex));
+    return (vertices_[idx_vertex.get()]);
+  }
 
-        /** \brief Get the half-edge for the given index. */
-        inline HalfEdge&
-        getHalfEdge (const HalfEdgeIndex& idx_he)
-        {
-          assert (this->isValid (idx_he));
-          return (half_edges_ [idx_he.get ()]);
-        }
+  /** \brief Set the vertex at the given index. */
+  inline void
+  setVertex(const VertexIndex& idx_vertex, const Vertex& vertex)
+  {
+    assert(this->isValid(idx_vertex));
+    vertices_[idx_vertex.get()] = vertex;
+  }
 
-        /** \brief Get the half-edge for the given index. */
-        inline HalfEdge
-        getHalfEdge (const HalfEdgeIndex& idx_he) const
-        {
-          assert (this->isValid (idx_he));
-          return (half_edges_ [idx_he.get ()]);
-        }
+  ////////////////////////////////////////////////////////////////////////
+  // get / set HalfEdge
+  ////////////////////////////////////////////////////////////////////////
 
-        /** \brief Set the half-edge at the given index. */
-        inline void
-        setHalfEdge (const HalfEdgeIndex& idx_he, const HalfEdge& half_edge)
-        {
-          assert (this->isValid (idx_he));
-          half_edges_ [idx_he.get ()] = half_edge;
-        }
+  /** \brief Get the half-edge for the given index. */
+  inline HalfEdge&
+  getHalfEdge(const HalfEdgeIndex& idx_he)
+  {
+    assert(this->isValid(idx_he));
+    return (half_edges_[idx_he.get()]);
+  }
 
-        ////////////////////////////////////////////////////////////////////////
-        // get / set Face
-        ////////////////////////////////////////////////////////////////////////
+  /** \brief Get the half-edge for the given index. */
+  inline HalfEdge
+  getHalfEdge(const HalfEdgeIndex& idx_he) const
+  {
+    assert(this->isValid(idx_he));
+    return (half_edges_[idx_he.get()]);
+  }
 
-        /** \brief Get the face for the given index. */
-        inline Face&
-        getFace (const FaceIndex& idx_face)
-        {
-          assert (this->isValid (idx_face));
-          return (faces_ [idx_face.get ()]);
-        }
+  /** \brief Set the half-edge at the given index. */
+  inline void
+  setHalfEdge(const HalfEdgeIndex& idx_he, const HalfEdge& half_edge)
+  {
+    assert(this->isValid(idx_he));
+    half_edges_[idx_he.get()] = half_edge;
+  }
 
-        /** \brief Get the face for the given index. */
-        inline Face
-        getFace (const FaceIndex& idx_face) const
-        {
-          assert (this->isValid (idx_face));
-          return (faces_ [idx_face.get ()]);
-        }
+  ////////////////////////////////////////////////////////////////////////
+  // get / set Face
+  ////////////////////////////////////////////////////////////////////////
 
-        /** \brief Set the face at the given index. */
-        inline void
-        setFace (const FaceIndex& idx_face, const Face& face)
-        {
-          assert (this->isValid (idx_face));
-          faces_ [idx_face.get ()] = face;
-        }
+  /** \brief Get the face for the given index. */
+  inline Face&
+  getFace(const FaceIndex& idx_face)
+  {
+    assert(this->isValid(idx_face));
+    return (faces_[idx_face.get()]);
+  }
 
-      private:
+  /** \brief Get the face for the given index. */
+  inline Face
+  getFace(const FaceIndex& idx_face) const
+  {
+    assert(this->isValid(idx_face));
+    return (faces_[idx_face.get()]);
+  }
 
-        ////////////////////////////////////////////////////////////////////////
-        // Members
-        ////////////////////////////////////////////////////////////////////////
+  /** \brief Set the face at the given index. */
+  inline void
+  setFace(const FaceIndex& idx_face, const Face& face)
+  {
+    assert(this->isValid(idx_face));
+    faces_[idx_face.get()] = face;
+  }
 
-        /** \brief Data stored for the vertices. */
-        VertexDataCloud vertex_data_cloud_;
+private:
+  ////////////////////////////////////////////////////////////////////////
+  // Members
+  ////////////////////////////////////////////////////////////////////////
 
-        /** \brief Data stored for the half-edges. */
-        HalfEdgeDataCloud half_edge_data_cloud_;
+  /** \brief Data stored for the vertices. */
+  VertexDataCloud vertex_data_cloud_;
 
-        /** \brief Data stored for the edges. */
-        EdgeDataCloud edge_data_cloud_;
+  /** \brief Data stored for the half-edges. */
+  HalfEdgeDataCloud half_edge_data_cloud_;
 
-        /** \brief Data stored for the faces. */
-        FaceDataCloud face_data_cloud_;
+  /** \brief Data stored for the edges. */
+  EdgeDataCloud edge_data_cloud_;
 
-        /** \brief Connectivity information for the vertices. */
-        Vertices vertices_;
+  /** \brief Data stored for the faces. */
+  FaceDataCloud face_data_cloud_;
 
-        /** \brief Connectivity information for the half-edges. */
-        HalfEdges half_edges_;
+  /** \brief Connectivity information for the vertices. */
+  Vertices vertices_;
 
-        /** \brief Connectivity information for the faces. */
-        Faces faces_;
+  /** \brief Connectivity information for the half-edges. */
+  HalfEdges half_edges_;
 
-        // NOTE: It is MUCH faster to store these variables permamently.
+  /** \brief Connectivity information for the faces. */
+  Faces faces_;
 
-        /** \brief Storage for addFaceImplBase and deleteFace. */
-        HalfEdgeIndices inner_he_;
+  // NOTE: It is MUCH faster to store these variables permamently.
 
-        /** \brief Storage for addFaceImplBase. */
-        HalfEdgeIndices free_he_;
+  /** \brief Storage for addFaceImplBase and deleteFace. */
+  HalfEdgeIndices inner_he_;
 
-        /** \brief Storage for addFaceImplBase. */
-        std::vector <bool> is_new_;
+  /** \brief Storage for addFaceImplBase. */
+  HalfEdgeIndices free_he_;
 
-        /** \brief Storage for addFaceImplBase. */
-        std::vector <bool> make_adjacent_;
+  /** \brief Storage for addFaceImplBase. */
+  std::vector<bool> is_new_;
 
-        /** \brief Storage for deleteFace. */
-        std::vector <bool> is_boundary_;
+  /** \brief Storage for addFaceImplBase. */
+  std::vector<bool> make_adjacent_;
 
-        /** \brief Storage for deleteVertex. */
-        FaceIndices delete_faces_vertex_;
+  /** \brief Storage for deleteFace. */
+  std::vector<bool> is_boundary_;
 
-        /** \brief Storage for deleteFace. */
-        FaceIndices delete_faces_face_;
+  /** \brief Storage for deleteVertex. */
+  FaceIndices delete_faces_vertex_;
 
-      public:
+  /** \brief Storage for deleteFace. */
+  FaceIndices delete_faces_face_;
 
-        template <class MeshT>
-        friend class pcl::geometry::MeshIO;
+public:
+  template <class MeshT>
+  friend class pcl::geometry::MeshIO;
 
-        PCL_MAKE_ALIGNED_OPERATOR_NEW
-    };
-  } // End namespace geometry
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+};
+} // End namespace geometry
 } // End namespace pcl
diff --git a/geometry/include/pcl/geometry/mesh_circulators.h b/geometry/include/pcl/geometry/mesh_circulators.h
index 18dae530..198e238a 100644
--- a/geometry/include/pcl/geometry/mesh_circulators.h
+++ b/geometry/include/pcl/geometry/mesh_circulators.h
@@ -38,7 +38,8 @@
  *
  */
 
-// NOTE: This file has been created with 'pcl_src/geometry/include/pcl/geometry/mesh_circulators.py'
+// NOTE: This file has been created with
+// 'pcl_src/geometry/include/pcl/geometry/mesh_circulators.py'
 
 #pragma once
 
@@ -49,864 +50,823 @@
 // VertexAroundVertexCirculator
 ////////////////////////////////////////////////////////////////////////////////
 
-namespace pcl
-{
-  namespace geometry
-  {
-    /** \brief Circulates counter-clockwise around a vertex and returns an index to the terminating vertex of the outgoing half-edge (the target). The best way to declare the circulator is to use the method pcl::geometry::MeshBase::getVertexAroundVertexCirculator ().
-      * \tparam MeshT Mesh to which this circulator belongs to.
-      * \note The circulator can't be used to change the connectivity in the mesh (only const circulators are valid).
-      * \author Martin Saelzle
-      * \ingroup geometry
-      */
-    template <class MeshT>
-    class VertexAroundVertexCirculator
-        : boost::equality_comparable <pcl::geometry::VertexAroundVertexCirculator <MeshT>
-        , boost::unit_steppable      <pcl::geometry::VertexAroundVertexCirculator <MeshT>
-        > >
-    {
-      public:
-
-        using Base = boost::equality_comparable <pcl::geometry::VertexAroundVertexCirculator <MeshT>,
-                     boost::unit_steppable      <pcl::geometry::VertexAroundVertexCirculator <MeshT> > >;
-        using Self = pcl::geometry::VertexAroundVertexCirculator<MeshT>;
-
-        using Mesh = MeshT;
-        using VertexIndex = typename Mesh::VertexIndex;
-        using HalfEdgeIndex = typename Mesh::HalfEdgeIndex;
-
-        /** \brief Constructor resulting in an invalid circulator. */
-        VertexAroundVertexCirculator ()
-          : mesh_                   (nullptr),
-            idx_outgoing_half_edge_ ()
-        {
-        }
-
-        /** \brief Construct from the vertex around which we want to circulate. */
-        VertexAroundVertexCirculator (const VertexIndex& idx_vertex,
-                                      Mesh*const         mesh)
-          : mesh_                   (mesh),
-            idx_outgoing_half_edge_ (mesh->getOutgoingHalfEdgeIndex (idx_vertex))
-        {
-        }
-
-        /** \brief Construct directly from the outgoing half-edge. */
-        VertexAroundVertexCirculator (const HalfEdgeIndex& idx_outgoing_half_edge,
-                                      Mesh*const           mesh)
-          : mesh_                   (mesh),
-            idx_outgoing_half_edge_ (idx_outgoing_half_edge)
-        {
-        }
-
-        /** \brief Check if the circulator is valid.
-          * \warning Does NOT check if the stored mesh pointer is valid. You have to ensure this yourself when constructing the circulator. */
-        inline bool
-        isValid () const
-        {
-          return (idx_outgoing_half_edge_.isValid ());
-        }
-
-        /** \brief Comparison operators (with boost::operators): == !=
-          * \warning Does NOT check if the circulators belong to the same mesh. Please check this yourself. */
-        inline bool
-        operator == (const Self& other) const
-        {
-          return (idx_outgoing_half_edge_ == other.idx_outgoing_half_edge_);
-        }
-
-        /** \brief Increment operators (with boost::operators): ++ (pre and post) */
-        inline Self&
-        operator ++ ()
-        {
-          idx_outgoing_half_edge_ = mesh_->getNextHalfEdgeIndex (mesh_->getOppositeHalfEdgeIndex (idx_outgoing_half_edge_));
-          return (*this);
-        }
-
-        /** \brief Decrement operators (with boost::operators): -- (pre and post) */
-        inline Self&
-        operator -- ()
-        {
-          idx_outgoing_half_edge_ = mesh_->getOppositeHalfEdgeIndex (mesh_->getPrevHalfEdgeIndex (idx_outgoing_half_edge_));
-          return (*this);
-        }
-
-        /** \brief Get the index to the target vertex. */
-        inline VertexIndex
-        getTargetIndex () const
-        {
-          return (mesh_->getTerminatingVertexIndex (idx_outgoing_half_edge_));
-        }
-
-        /** \brief Get the half-edge that is currently stored in the circulator. */
-        inline HalfEdgeIndex
-        getCurrentHalfEdgeIndex () const
-        {
-          return (idx_outgoing_half_edge_);
-        }
-
-        /** \brief The mesh to which this circulator belongs to. */
-        Mesh* mesh_;
-
-        /** \brief The outgoing half-edge of the vertex around which we want to circulate. */
-        HalfEdgeIndex idx_outgoing_half_edge_;
-    };
-  } // End namespace geometry
+namespace pcl {
+namespace geometry {
+/** \brief Circulates counter-clockwise around a vertex and returns an index to the
+ * terminating vertex of the outgoing half-edge (the target). The best way to declare
+ * the circulator is to use the method
+ * pcl::geometry::MeshBase::getVertexAroundVertexCirculator ().
+ * \tparam MeshT Mesh to which this circulator belongs to.
+ * \note The circulator can't be used to change the connectivity in the mesh (only
+ * const circulators are valid).
+ * \author Martin Saelzle
+ * \ingroup geometry
+ */
+template <class MeshT>
+class VertexAroundVertexCirculator
+: boost::equality_comparable<
+      pcl::geometry::VertexAroundVertexCirculator<MeshT>,
+      boost::unit_steppable<pcl::geometry::VertexAroundVertexCirculator<MeshT>>> {
+public:
+  using Base = boost::equality_comparable<
+      pcl::geometry::VertexAroundVertexCirculator<MeshT>,
+      boost::unit_steppable<pcl::geometry::VertexAroundVertexCirculator<MeshT>>>;
+  using Self = pcl::geometry::VertexAroundVertexCirculator<MeshT>;
+
+  using Mesh = MeshT;
+  using VertexIndex = typename Mesh::VertexIndex;
+  using HalfEdgeIndex = typename Mesh::HalfEdgeIndex;
+
+  /** \brief Constructor resulting in an invalid circulator. */
+  VertexAroundVertexCirculator() : mesh_(nullptr), idx_outgoing_half_edge_() {}
+
+  /** \brief Construct from the vertex around which we want to circulate. */
+  VertexAroundVertexCirculator(const VertexIndex& idx_vertex, Mesh* const mesh)
+  : mesh_(mesh), idx_outgoing_half_edge_(mesh->getOutgoingHalfEdgeIndex(idx_vertex))
+  {}
+
+  /** \brief Construct directly from the outgoing half-edge. */
+  VertexAroundVertexCirculator(const HalfEdgeIndex& idx_outgoing_half_edge,
+                               Mesh* const mesh)
+  : mesh_(mesh), idx_outgoing_half_edge_(idx_outgoing_half_edge)
+  {}
+
+  /** \brief Check if the circulator is valid.
+   * \warning Does NOT check if the stored mesh pointer is valid. You have to ensure
+   * this yourself when constructing the circulator. */
+  inline bool
+  isValid() const
+  {
+    return (idx_outgoing_half_edge_.isValid());
+  }
+
+  /** \brief Comparison operators (with boost::operators): == !=
+   * \warning Does NOT check if the circulators belong to the same mesh. Please check
+   * this yourself. */
+  inline bool
+  operator==(const Self& other) const
+  {
+    return (idx_outgoing_half_edge_ == other.idx_outgoing_half_edge_);
+  }
+
+  /** \brief Increment operators (with boost::operators): ++ (pre and post) */
+  inline Self&
+  operator++()
+  {
+    idx_outgoing_half_edge_ = mesh_->getNextHalfEdgeIndex(
+        mesh_->getOppositeHalfEdgeIndex(idx_outgoing_half_edge_));
+    return (*this);
+  }
+
+  /** \brief Decrement operators (with boost::operators): -- (pre and post) */
+  inline Self&
+  operator--()
+  {
+    idx_outgoing_half_edge_ = mesh_->getOppositeHalfEdgeIndex(
+        mesh_->getPrevHalfEdgeIndex(idx_outgoing_half_edge_));
+    return (*this);
+  }
+
+  /** \brief Get the index to the target vertex. */
+  inline VertexIndex
+  getTargetIndex() const
+  {
+    return (mesh_->getTerminatingVertexIndex(idx_outgoing_half_edge_));
+  }
+
+  /** \brief Get the half-edge that is currently stored in the circulator. */
+  inline HalfEdgeIndex
+  getCurrentHalfEdgeIndex() const
+  {
+    return (idx_outgoing_half_edge_);
+  }
+
+  /** \brief The mesh to which this circulator belongs to. */
+  Mesh* mesh_;
+
+  /** \brief The outgoing half-edge of the vertex around which we want to circulate. */
+  HalfEdgeIndex idx_outgoing_half_edge_;
+};
+} // End namespace geometry
 } // End namespace pcl
 
 ////////////////////////////////////////////////////////////////////////////////
 // OutgoingHalfEdgeAroundVertexCirculator
 ////////////////////////////////////////////////////////////////////////////////
 
-namespace pcl
-{
-  namespace geometry
-  {
-    /** \brief Circulates counter-clockwise around a vertex and returns an index to the outgoing half-edge (the target). The best way to declare the circulator is to use the method pcl::geometry::MeshBase::getOutgoingHalfEdgeAroundVertexCirculator ().
-      * \tparam MeshT Mesh to which this circulator belongs to.
-      * \note The circulator can't be used to change the connectivity in the mesh (only const circulators are valid).
-      * \author Martin Saelzle
-      * \ingroup geometry
-      */
-    template <class MeshT>
-    class OutgoingHalfEdgeAroundVertexCirculator
-        : boost::equality_comparable <pcl::geometry::OutgoingHalfEdgeAroundVertexCirculator <MeshT>
-        , boost::unit_steppable      <pcl::geometry::OutgoingHalfEdgeAroundVertexCirculator <MeshT>
-        > >
-    {
-      public:
-
-        using Base = boost::equality_comparable <pcl::geometry::OutgoingHalfEdgeAroundVertexCirculator <MeshT>,
-                     boost::unit_steppable      <pcl::geometry::OutgoingHalfEdgeAroundVertexCirculator <MeshT> > >;
-        using Self = pcl::geometry::OutgoingHalfEdgeAroundVertexCirculator<MeshT>;
-
-        using Mesh = MeshT;
-        using VertexIndex = typename Mesh::VertexIndex;
-        using HalfEdgeIndex = typename Mesh::HalfEdgeIndex;
-
-        /** \brief Constructor resulting in an invalid circulator. */
-        OutgoingHalfEdgeAroundVertexCirculator ()
-          : mesh_                   (nullptr),
-            idx_outgoing_half_edge_ ()
-        {
-        }
-
-        /** \brief Construct from the vertex around which we want to circulate. */
-        OutgoingHalfEdgeAroundVertexCirculator (const VertexIndex& idx_vertex,
-                                                Mesh*const         mesh)
-          : mesh_                   (mesh),
-            idx_outgoing_half_edge_ (mesh->getOutgoingHalfEdgeIndex (idx_vertex))
-        {
-        }
-
-        /** \brief Construct directly from the outgoing half-edge. */
-        OutgoingHalfEdgeAroundVertexCirculator (const HalfEdgeIndex& idx_outgoing_half_edge,
-                                                Mesh*const           mesh)
-          : mesh_                   (mesh),
-            idx_outgoing_half_edge_ (idx_outgoing_half_edge)
-        {
-        }
-
-        /** \brief Check if the circulator is valid.
-          * \warning Does NOT check if the stored mesh pointer is valid. You have to ensure this yourself when constructing the circulator. */
-        inline bool
-        isValid () const
-        {
-          return (idx_outgoing_half_edge_.isValid ());
-        }
-
-        /** \brief Comparison operators (with boost::operators): == !=
-          * \warning Does NOT check if the circulators belong to the same mesh. Please check this yourself. */
-        inline bool
-        operator == (const Self& other) const
-        {
-          return (idx_outgoing_half_edge_ == other.idx_outgoing_half_edge_);
-        }
-
-        /** \brief Increment operators (with boost::operators): ++ (pre and post) */
-        inline Self&
-        operator ++ ()
-        {
-          idx_outgoing_half_edge_ = mesh_->getNextHalfEdgeIndex (mesh_->getOppositeHalfEdgeIndex (idx_outgoing_half_edge_));
-          return (*this);
-        }
-
-        /** \brief Decrement operators (with boost::operators): -- (pre and post) */
-        inline Self&
-        operator -- ()
-        {
-          idx_outgoing_half_edge_ = mesh_->getOppositeHalfEdgeIndex (mesh_->getPrevHalfEdgeIndex (idx_outgoing_half_edge_));
-          return (*this);
-        }
-
-        /** \brief Get the index to the outgoing half-edge. */
-        inline HalfEdgeIndex
-        getTargetIndex () const
-        {
-          return (idx_outgoing_half_edge_);
-        }
-
-        /** \brief Get the half-edge that is currently stored in the circulator. */
-        inline HalfEdgeIndex
-        getCurrentHalfEdgeIndex () const
-        {
-          return (idx_outgoing_half_edge_);
-        }
-
-        /** \brief The mesh to which this circulator belongs to. */
-        Mesh* mesh_;
-
-        /** \brief The outgoing half-edge of the vertex around which we want to circulate. */
-        HalfEdgeIndex idx_outgoing_half_edge_;
-    };
-  } // End namespace geometry
+namespace pcl {
+namespace geometry {
+/** \brief Circulates counter-clockwise around a vertex and returns an index to the
+ * outgoing half-edge (the target). The best way to declare the circulator is to use the
+ * method pcl::geometry::MeshBase::getOutgoingHalfEdgeAroundVertexCirculator ().
+ * \tparam MeshT Mesh to which this circulator belongs to.
+ * \note The circulator can't be used to
+ * change the connectivity in the mesh (only const circulators are valid).
+ * \author Martin Saelzle
+ * \ingroup geometry
+ */
+template <class MeshT>
+class OutgoingHalfEdgeAroundVertexCirculator
+: boost::equality_comparable<
+      pcl::geometry::OutgoingHalfEdgeAroundVertexCirculator<MeshT>,
+      boost::unit_steppable<
+          pcl::geometry::OutgoingHalfEdgeAroundVertexCirculator<MeshT>>> {
+public:
+  using Base = boost::equality_comparable<
+      pcl::geometry::OutgoingHalfEdgeAroundVertexCirculator<MeshT>,
+      boost::unit_steppable<
+          pcl::geometry::OutgoingHalfEdgeAroundVertexCirculator<MeshT>>>;
+  using Self = pcl::geometry::OutgoingHalfEdgeAroundVertexCirculator<MeshT>;
+
+  using Mesh = MeshT;
+  using VertexIndex = typename Mesh::VertexIndex;
+  using HalfEdgeIndex = typename Mesh::HalfEdgeIndex;
+
+  /** \brief Constructor resulting in an invalid circulator. */
+  OutgoingHalfEdgeAroundVertexCirculator() : mesh_(nullptr), idx_outgoing_half_edge_()
+  {}
+
+  /** \brief Construct from the vertex around which we want to circulate. */
+  OutgoingHalfEdgeAroundVertexCirculator(const VertexIndex& idx_vertex,
+                                         Mesh* const mesh)
+  : mesh_(mesh), idx_outgoing_half_edge_(mesh->getOutgoingHalfEdgeIndex(idx_vertex))
+  {}
+
+  /** \brief Construct directly from the outgoing half-edge. */
+  OutgoingHalfEdgeAroundVertexCirculator(const HalfEdgeIndex& idx_outgoing_half_edge,
+                                         Mesh* const mesh)
+  : mesh_(mesh), idx_outgoing_half_edge_(idx_outgoing_half_edge)
+  {}
+
+  /** \brief Check if the circulator is valid.
+   * \warning Does NOT check if the stored mesh pointer is valid. You have to ensure
+   * this yourself when constructing the circulator. */
+  inline bool
+  isValid() const
+  {
+    return (idx_outgoing_half_edge_.isValid());
+  }
+
+  /** \brief Comparison operators (with boost::operators): == !=
+   * \warning Does NOT check if the circulators belong to the same mesh. Please check
+   * this yourself. */
+  inline bool
+  operator==(const Self& other) const
+  {
+    return (idx_outgoing_half_edge_ == other.idx_outgoing_half_edge_);
+  }
+
+  /** \brief Increment operators (with boost::operators): ++ (pre and post) */
+  inline Self&
+  operator++()
+  {
+    idx_outgoing_half_edge_ = mesh_->getNextHalfEdgeIndex(
+        mesh_->getOppositeHalfEdgeIndex(idx_outgoing_half_edge_));
+    return (*this);
+  }
+
+  /** \brief Decrement operators (with boost::operators): -- (pre and post) */
+  inline Self&
+  operator--()
+  {
+    idx_outgoing_half_edge_ = mesh_->getOppositeHalfEdgeIndex(
+        mesh_->getPrevHalfEdgeIndex(idx_outgoing_half_edge_));
+    return (*this);
+  }
+
+  /** \brief Get the index to the outgoing half-edge. */
+  inline HalfEdgeIndex
+  getTargetIndex() const
+  {
+    return (idx_outgoing_half_edge_);
+  }
+
+  /** \brief Get the half-edge that is currently stored in the circulator. */
+  inline HalfEdgeIndex
+  getCurrentHalfEdgeIndex() const
+  {
+    return (idx_outgoing_half_edge_);
+  }
+
+  /** \brief The mesh to which this circulator belongs to. */
+  Mesh* mesh_;
+
+  /** \brief The outgoing half-edge of the vertex around which we want to circulate. */
+  HalfEdgeIndex idx_outgoing_half_edge_;
+};
+} // End namespace geometry
 } // End namespace pcl
 
 ////////////////////////////////////////////////////////////////////////////////
 // IncomingHalfEdgeAroundVertexCirculator
 ////////////////////////////////////////////////////////////////////////////////
 
-namespace pcl
-{
-  namespace geometry
-  {
-    /** \brief Circulates counter-clockwise around a vertex and returns an index to the incoming half-edge (the target). The best way to declare the circulator is to use the method pcl::geometry::MeshBase::getIncomingHalfEdgeAroundVertexCirculator ().
-      * \tparam MeshT Mesh to which this circulator belongs to.
-      * \note The circulator can't be used to change the connectivity in the mesh (only const circulators are valid).
-      * \author Martin Saelzle
-      * \ingroup geometry
-      */
-    template <class MeshT>
-    class IncomingHalfEdgeAroundVertexCirculator
-        : boost::equality_comparable <pcl::geometry::IncomingHalfEdgeAroundVertexCirculator <MeshT>
-        , boost::unit_steppable      <pcl::geometry::IncomingHalfEdgeAroundVertexCirculator <MeshT>
-        > >
-    {
-      public:
-
-        using Base = boost::equality_comparable <pcl::geometry::IncomingHalfEdgeAroundVertexCirculator <MeshT>,
-                     boost::unit_steppable      <pcl::geometry::IncomingHalfEdgeAroundVertexCirculator <MeshT> > >;
-        using Self = pcl::geometry::IncomingHalfEdgeAroundVertexCirculator<MeshT>;
-
-        using Mesh = MeshT;
-        using VertexIndex = typename Mesh::VertexIndex;
-        using HalfEdgeIndex = typename Mesh::HalfEdgeIndex;
-
-        /** \brief Constructor resulting in an invalid circulator. */
-        IncomingHalfEdgeAroundVertexCirculator ()
-          : mesh_                   (nullptr),
-            idx_incoming_half_edge_ ()
-        {
-        }
-
-        /** \brief Construct from the vertex around which we want to circulate. */
-        IncomingHalfEdgeAroundVertexCirculator (const VertexIndex& idx_vertex,
-                                                Mesh*const         mesh)
-          : mesh_                   (mesh),
-            idx_incoming_half_edge_ (mesh->getIncomingHalfEdgeIndex (idx_vertex))
-        {
-        }
-
-        /** \brief Construct directly from the incoming half-edge. */
-        IncomingHalfEdgeAroundVertexCirculator (const HalfEdgeIndex& idx_incoming_half_edge,
-                                                Mesh*const           mesh)
-          : mesh_                   (mesh),
-            idx_incoming_half_edge_ (idx_incoming_half_edge)
-        {
-        }
-
-        /** \brief Check if the circulator is valid.
-          * \warning Does NOT check if the stored mesh pointer is valid. You have to ensure this yourself when constructing the circulator. */
-        inline bool
-        isValid () const
-        {
-          return (idx_incoming_half_edge_.isValid ());
-        }
-
-        /** \brief Comparison operators (with boost::operators): == !=
-          * \warning Does NOT check if the circulators belong to the same mesh. Please check this yourself. */
-        inline bool
-        operator == (const Self& other) const
-        {
-          return (idx_incoming_half_edge_ == other.idx_incoming_half_edge_);
-        }
-
-        /** \brief Increment operators (with boost::operators): ++ (pre and post) */
-        inline Self&
-        operator ++ ()
-        {
-          idx_incoming_half_edge_ = mesh_->getOppositeHalfEdgeIndex (mesh_->getNextHalfEdgeIndex (idx_incoming_half_edge_));
-          return (*this);
-        }
-
-        /** \brief Decrement operators (with boost::operators): -- (pre and post) */
-        inline Self&
-        operator -- ()
-        {
-          idx_incoming_half_edge_ = mesh_->getPrevHalfEdgeIndex (mesh_->getOppositeHalfEdgeIndex (idx_incoming_half_edge_));
-          return (*this);
-        }
-
-        /** \brief Get the index to the incoming half-edge. */
-        inline HalfEdgeIndex
-        getTargetIndex () const
-        {
-          return (idx_incoming_half_edge_);
-        }
-
-        /** \brief Get the half-edge that is currently stored in the circulator. */
-        inline HalfEdgeIndex
-        getCurrentHalfEdgeIndex () const
-        {
-          return (idx_incoming_half_edge_);
-        }
-
-        /** \brief The mesh to which this circulator belongs to. */
-        Mesh* mesh_;
-
-        /** \brief The incoming half-edge of the vertex around which we want to circulate. */
-        HalfEdgeIndex idx_incoming_half_edge_;
-    };
-  } // End namespace geometry
+namespace pcl {
+namespace geometry {
+/** \brief Circulates counter-clockwise around a vertex and returns an index to the
+ * incoming half-edge (the target). The best way to declare the circulator is to use the
+ * method pcl::geometry::MeshBase::getIncomingHalfEdgeAroundVertexCirculator ().
+ * \tparam MeshT Mesh to which this circulator belongs to.
+ * \note The circulator can't be used to change the connectivity in the mesh (only
+ * const circulators are valid).
+ * \author Martin Saelzle
+ * \ingroup geometry
+ */
+template <class MeshT>
+class IncomingHalfEdgeAroundVertexCirculator
+: boost::equality_comparable<
+      pcl::geometry::IncomingHalfEdgeAroundVertexCirculator<MeshT>,
+      boost::unit_steppable<
+          pcl::geometry::IncomingHalfEdgeAroundVertexCirculator<MeshT>>> {
+public:
+  using Base = boost::equality_comparable<
+      pcl::geometry::IncomingHalfEdgeAroundVertexCirculator<MeshT>,
+      boost::unit_steppable<
+          pcl::geometry::IncomingHalfEdgeAroundVertexCirculator<MeshT>>>;
+  using Self = pcl::geometry::IncomingHalfEdgeAroundVertexCirculator<MeshT>;
+
+  using Mesh = MeshT;
+  using VertexIndex = typename Mesh::VertexIndex;
+  using HalfEdgeIndex = typename Mesh::HalfEdgeIndex;
+
+  /** \brief Constructor resulting in an invalid circulator. */
+  IncomingHalfEdgeAroundVertexCirculator() : mesh_(nullptr), idx_incoming_half_edge_()
+  {}
+
+  /** \brief Construct from the vertex around which we want to circulate. */
+  IncomingHalfEdgeAroundVertexCirculator(const VertexIndex& idx_vertex,
+                                         Mesh* const mesh)
+  : mesh_(mesh), idx_incoming_half_edge_(mesh->getIncomingHalfEdgeIndex(idx_vertex))
+  {}
+
+  /** \brief Construct directly from the incoming half-edge. */
+  IncomingHalfEdgeAroundVertexCirculator(const HalfEdgeIndex& idx_incoming_half_edge,
+                                         Mesh* const mesh)
+  : mesh_(mesh), idx_incoming_half_edge_(idx_incoming_half_edge)
+  {}
+
+  /** \brief Check if the circulator is valid.
+   * \warning Does NOT check if the stored mesh pointer is valid. You have to ensure
+   * this yourself when constructing the circulator. */
+  inline bool
+  isValid() const
+  {
+    return (idx_incoming_half_edge_.isValid());
+  }
+
+  /** \brief Comparison operators (with boost::operators): == !=
+   * \warning Does NOT check if the circulators belong to the same mesh. Please check
+   * this yourself. */
+  inline bool
+  operator==(const Self& other) const
+  {
+    return (idx_incoming_half_edge_ == other.idx_incoming_half_edge_);
+  }
+
+  /** \brief Increment operators (with boost::operators): ++ (pre and post) */
+  inline Self&
+  operator++()
+  {
+    idx_incoming_half_edge_ = mesh_->getOppositeHalfEdgeIndex(
+        mesh_->getNextHalfEdgeIndex(idx_incoming_half_edge_));
+    return (*this);
+  }
+
+  /** \brief Decrement operators (with boost::operators): -- (pre and post) */
+  inline Self&
+  operator--()
+  {
+    idx_incoming_half_edge_ = mesh_->getPrevHalfEdgeIndex(
+        mesh_->getOppositeHalfEdgeIndex(idx_incoming_half_edge_));
+    return (*this);
+  }
+
+  /** \brief Get the index to the incoming half-edge. */
+  inline HalfEdgeIndex
+  getTargetIndex() const
+  {
+    return (idx_incoming_half_edge_);
+  }
+
+  /** \brief Get the half-edge that is currently stored in the circulator. */
+  inline HalfEdgeIndex
+  getCurrentHalfEdgeIndex() const
+  {
+    return (idx_incoming_half_edge_);
+  }
+
+  /** \brief The mesh to which this circulator belongs to. */
+  Mesh* mesh_;
+
+  /** \brief The incoming half-edge of the vertex around which we want to circulate. */
+  HalfEdgeIndex idx_incoming_half_edge_;
+};
+} // End namespace geometry
 } // End namespace pcl
 
 ////////////////////////////////////////////////////////////////////////////////
 // FaceAroundVertexCirculator
 ////////////////////////////////////////////////////////////////////////////////
 
-namespace pcl
-{
-  namespace geometry
-  {
-    /** \brief Circulates counter-clockwise around a vertex and returns an index to the face of the outgoing half-edge (the target). The best way to declare the circulator is to use the method pcl::geometry::MeshBase::getFaceAroundVertexCirculator ().
-      * \tparam MeshT Mesh to which this circulator belongs to.
-      * \note The circulator can't be used to change the connectivity in the mesh (only const circulators are valid).
-      * \author Martin Saelzle
-      * \ingroup geometry
-      */
-    template <class MeshT>
-    class FaceAroundVertexCirculator
-        : boost::equality_comparable <pcl::geometry::FaceAroundVertexCirculator <MeshT>
-        , boost::unit_steppable      <pcl::geometry::FaceAroundVertexCirculator <MeshT>
-        > >
-    {
-      public:
-
-        using Base = boost::equality_comparable <pcl::geometry::FaceAroundVertexCirculator <MeshT>,
-                     boost::unit_steppable      <pcl::geometry::FaceAroundVertexCirculator <MeshT> > >;
-        using Self = pcl::geometry::FaceAroundVertexCirculator<MeshT>;
-
-        using Mesh = MeshT;
-        using FaceIndex = typename Mesh::FaceIndex;
-        using VertexIndex = typename Mesh::VertexIndex;
-        using HalfEdgeIndex = typename Mesh::HalfEdgeIndex;
-
-        /** \brief Constructor resulting in an invalid circulator. */
-        FaceAroundVertexCirculator ()
-          : mesh_                   (nullptr),
-            idx_outgoing_half_edge_ ()
-        {
-        }
-
-        /** \brief Construct from the vertex around which we want to circulate. */
-        FaceAroundVertexCirculator (const VertexIndex& idx_vertex,
-                                    Mesh*const         mesh)
-          : mesh_                   (mesh),
-            idx_outgoing_half_edge_ (mesh->getOutgoingHalfEdgeIndex (idx_vertex))
-        {
-        }
-
-        /** \brief Construct directly from the outgoing half-edge. */
-        FaceAroundVertexCirculator (const HalfEdgeIndex& idx_outgoing_half_edge,
-                                    Mesh*const           mesh)
-          : mesh_                   (mesh),
-            idx_outgoing_half_edge_ (idx_outgoing_half_edge)
-        {
-        }
-
-        /** \brief Check if the circulator is valid.
-          * \warning Does NOT check if the stored mesh pointer is valid. You have to ensure this yourself when constructing the circulator. */
-        inline bool
-        isValid () const
-        {
-          return (idx_outgoing_half_edge_.isValid ());
-        }
-
-        /** \brief Comparison operators (with boost::operators): == !=
-          * \warning Does NOT check if the circulators belong to the same mesh. Please check this yourself. */
-        inline bool
-        operator == (const Self& other) const
-        {
-          return (idx_outgoing_half_edge_ == other.idx_outgoing_half_edge_);
-        }
-
-        /** \brief Increment operators (with boost::operators): ++ (pre and post) */
-        inline Self&
-        operator ++ ()
-        {
-          idx_outgoing_half_edge_ = mesh_->getNextHalfEdgeIndex (mesh_->getOppositeHalfEdgeIndex (idx_outgoing_half_edge_));
-          return (*this);
-        }
-
-        /** \brief Decrement operators (with boost::operators): -- (pre and post) */
-        inline Self&
-        operator -- ()
-        {
-          idx_outgoing_half_edge_ = mesh_->getOppositeHalfEdgeIndex (mesh_->getPrevHalfEdgeIndex (idx_outgoing_half_edge_));
-          return (*this);
-        }
-
-        /** \brief Get the index to the target face. */
-        inline FaceIndex
-        getTargetIndex () const
-        {
-          return (mesh_->getFaceIndex (idx_outgoing_half_edge_));
-        }
-
-        /** \brief Get the half-edge that is currently stored in the circulator. */
-        inline HalfEdgeIndex
-        getCurrentHalfEdgeIndex () const
-        {
-          return (idx_outgoing_half_edge_);
-        }
-
-        /** \brief The mesh to which this circulator belongs to. */
-        Mesh* mesh_;
-
-        /** \brief The outgoing half-edge of the vertex around which we want to circulate. */
-        HalfEdgeIndex idx_outgoing_half_edge_;
-    };
-  } // End namespace geometry
+namespace pcl {
+namespace geometry {
+/** \brief Circulates counter-clockwise around a vertex and returns an index to the face
+ * of the outgoing half-edge (the target). The best way to declare the circulator is to
+ * use the method pcl::geometry::MeshBase::getFaceAroundVertexCirculator ().
+ * \tparam MeshT Mesh to which this circulator belongs to.
+ * \note The circulator can't be used to change the connectivity in the mesh (only
+ * const circulators are valid).
+ * \author Martin Saelzle
+ * \ingroup geometry
+ */
+template <class MeshT>
+class FaceAroundVertexCirculator
+: boost::equality_comparable<
+      pcl::geometry::FaceAroundVertexCirculator<MeshT>,
+      boost::unit_steppable<pcl::geometry::FaceAroundVertexCirculator<MeshT>>> {
+public:
+  using Base = boost::equality_comparable<
+      pcl::geometry::FaceAroundVertexCirculator<MeshT>,
+      boost::unit_steppable<pcl::geometry::FaceAroundVertexCirculator<MeshT>>>;
+  using Self = pcl::geometry::FaceAroundVertexCirculator<MeshT>;
+
+  using Mesh = MeshT;
+  using FaceIndex = typename Mesh::FaceIndex;
+  using VertexIndex = typename Mesh::VertexIndex;
+  using HalfEdgeIndex = typename Mesh::HalfEdgeIndex;
+
+  /** \brief Constructor resulting in an invalid circulator. */
+  FaceAroundVertexCirculator() : mesh_(nullptr), idx_outgoing_half_edge_() {}
+
+  /** \brief Construct from the vertex around which we want to circulate. */
+  FaceAroundVertexCirculator(const VertexIndex& idx_vertex, Mesh* const mesh)
+  : mesh_(mesh), idx_outgoing_half_edge_(mesh->getOutgoingHalfEdgeIndex(idx_vertex))
+  {}
+
+  /** \brief Construct directly from the outgoing half-edge. */
+  FaceAroundVertexCirculator(const HalfEdgeIndex& idx_outgoing_half_edge,
+                             Mesh* const mesh)
+  : mesh_(mesh), idx_outgoing_half_edge_(idx_outgoing_half_edge)
+  {}
+
+  /** \brief Check if the circulator is valid.
+   * \warning Does NOT check if the stored mesh pointer is valid. You have to ensure
+   * this yourself when constructing the circulator. */
+  inline bool
+  isValid() const
+  {
+    return (idx_outgoing_half_edge_.isValid());
+  }
+
+  /** \brief Comparison operators (with boost::operators): == !=
+   * \warning Does NOT check if the circulators belong to the same mesh. Please check
+   * this yourself. */
+  inline bool
+  operator==(const Self& other) const
+  {
+    return (idx_outgoing_half_edge_ == other.idx_outgoing_half_edge_);
+  }
+
+  /** \brief Increment operators (with boost::operators): ++ (pre and post) */
+  inline Self&
+  operator++()
+  {
+    idx_outgoing_half_edge_ = mesh_->getNextHalfEdgeIndex(
+        mesh_->getOppositeHalfEdgeIndex(idx_outgoing_half_edge_));
+    return (*this);
+  }
+
+  /** \brief Decrement operators (with boost::operators): -- (pre and post) */
+  inline Self&
+  operator--()
+  {
+    idx_outgoing_half_edge_ = mesh_->getOppositeHalfEdgeIndex(
+        mesh_->getPrevHalfEdgeIndex(idx_outgoing_half_edge_));
+    return (*this);
+  }
+
+  /** \brief Get the index to the target face. */
+  inline FaceIndex
+  getTargetIndex() const
+  {
+    return (mesh_->getFaceIndex(idx_outgoing_half_edge_));
+  }
+
+  /** \brief Get the half-edge that is currently stored in the circulator. */
+  inline HalfEdgeIndex
+  getCurrentHalfEdgeIndex() const
+  {
+    return (idx_outgoing_half_edge_);
+  }
+
+  /** \brief The mesh to which this circulator belongs to. */
+  Mesh* mesh_;
+
+  /** \brief The outgoing half-edge of the vertex around which we want to circulate. */
+  HalfEdgeIndex idx_outgoing_half_edge_;
+};
+} // End namespace geometry
 } // End namespace pcl
 
 ////////////////////////////////////////////////////////////////////////////////
 // VertexAroundFaceCirculator
 ////////////////////////////////////////////////////////////////////////////////
 
-namespace pcl
-{
-  namespace geometry
-  {
-    /** \brief Circulates clockwise around a face and returns an index to the terminating vertex of the inner half-edge (the target). The best way to declare the circulator is to use the method pcl::geometry::MeshBase::getVertexAroundFaceCirculator ().
-      * \tparam MeshT Mesh to which this circulator belongs to.
-      * \note The circulator can't be used to change the connectivity in the mesh (only const circulators are valid).
-      * \author Martin Saelzle
-      * \ingroup geometry
-      */
-    template <class MeshT>
-    class VertexAroundFaceCirculator
-        : boost::equality_comparable <pcl::geometry::VertexAroundFaceCirculator <MeshT>
-        , boost::unit_steppable      <pcl::geometry::VertexAroundFaceCirculator <MeshT>
-        > >
-    {
-      public:
-
-        using Base = boost::equality_comparable <pcl::geometry::VertexAroundFaceCirculator <MeshT>,
-                     boost::unit_steppable      <pcl::geometry::VertexAroundFaceCirculator <MeshT> > >;
-        using Self = pcl::geometry::VertexAroundFaceCirculator<MeshT>;
-
-        using Mesh = MeshT;
-        using VertexIndex = typename Mesh::VertexIndex;
-        using FaceIndex = typename Mesh::FaceIndex;
-        using HalfEdgeIndex = typename Mesh::HalfEdgeIndex;
-
-        /** \brief Constructor resulting in an invalid circulator. */
-        VertexAroundFaceCirculator ()
-          : mesh_                (nullptr),
-            idx_inner_half_edge_ ()
-        {
-        }
-
-        /** \brief Construct from the face around which we want to circulate. */
-        VertexAroundFaceCirculator (const FaceIndex& idx_face,
-                                    Mesh*const       mesh)
-          : mesh_                (mesh),
-            idx_inner_half_edge_ (mesh->getInnerHalfEdgeIndex (idx_face))
-        {
-        }
-
-        /** \brief Construct directly from the inner half-edge. */
-        VertexAroundFaceCirculator (const HalfEdgeIndex& idx_inner_half_edge,
-                                    Mesh*const           mesh)
-          : mesh_                (mesh),
-            idx_inner_half_edge_ (idx_inner_half_edge)
-        {
-        }
-
-        /** \brief Check if the circulator is valid.
-          * \warning Does NOT check if the stored mesh pointer is valid. You have to ensure this yourself when constructing the circulator. */
-        inline bool
-        isValid () const
-        {
-          return (idx_inner_half_edge_.isValid ());
-        }
-
-        /** \brief Comparison operators (with boost::operators): == !=
-          * \warning Does NOT check if the circulators belong to the same mesh. Please check this yourself. */
-        inline bool
-        operator == (const Self& other) const
-        {
-          return (idx_inner_half_edge_ == other.idx_inner_half_edge_);
-        }
-
-        /** \brief Increment operators (with boost::operators): ++ (pre and post) */
-        inline Self&
-        operator ++ ()
-        {
-          idx_inner_half_edge_ = mesh_->getNextHalfEdgeIndex (idx_inner_half_edge_);
-          return (*this);
-        }
-
-        /** \brief Decrement operators (with boost::operators): -- (pre and post) */
-        inline Self&
-        operator -- ()
-        {
-          idx_inner_half_edge_ = mesh_->getPrevHalfEdgeIndex (idx_inner_half_edge_);
-          return (*this);
-        }
-
-        /** \brief Get the index to the target vertex. */
-        inline VertexIndex
-        getTargetIndex () const
-        {
-          return (mesh_->getTerminatingVertexIndex (idx_inner_half_edge_));
-        }
-
-        /** \brief Get the half-edge that is currently stored in the circulator. */
-        inline HalfEdgeIndex
-        getCurrentHalfEdgeIndex () const
-        {
-          return (idx_inner_half_edge_);
-        }
-
-        /** \brief The mesh to which this circulator belongs to. */
-        Mesh* mesh_;
-
-        /** \brief The inner half-edge of the face around which we want to circulate. */
-        HalfEdgeIndex idx_inner_half_edge_;
-    };
-  } // End namespace geometry
+namespace pcl {
+namespace geometry {
+/** \brief Circulates clockwise around a face and returns an index to the terminating
+ * vertex of the inner half-edge (the target). The best way to declare the circulator is
+ * to use the method pcl::geometry::MeshBase::getVertexAroundFaceCirculator ().
+ * \tparam MeshT Mesh to which this circulator belongs to.
+ * \note The circulator can't be used to change the connectivity in the mesh (only
+ * const circulators are valid).
+ * \author Martin Saelzle
+ * \ingroup geometry
+ */
+template <class MeshT>
+class VertexAroundFaceCirculator
+: boost::equality_comparable<
+      pcl::geometry::VertexAroundFaceCirculator<MeshT>,
+      boost::unit_steppable<pcl::geometry::VertexAroundFaceCirculator<MeshT>>> {
+public:
+  using Base = boost::equality_comparable<
+      pcl::geometry::VertexAroundFaceCirculator<MeshT>,
+      boost::unit_steppable<pcl::geometry::VertexAroundFaceCirculator<MeshT>>>;
+  using Self = pcl::geometry::VertexAroundFaceCirculator<MeshT>;
+
+  using Mesh = MeshT;
+  using VertexIndex = typename Mesh::VertexIndex;
+  using FaceIndex = typename Mesh::FaceIndex;
+  using HalfEdgeIndex = typename Mesh::HalfEdgeIndex;
+
+  /** \brief Constructor resulting in an invalid circulator. */
+  VertexAroundFaceCirculator() : mesh_(nullptr), idx_inner_half_edge_() {}
+
+  /** \brief Construct from the face around which we want to circulate. */
+  VertexAroundFaceCirculator(const FaceIndex& idx_face, Mesh* const mesh)
+  : mesh_(mesh), idx_inner_half_edge_(mesh->getInnerHalfEdgeIndex(idx_face))
+  {}
+
+  /** \brief Construct directly from the inner half-edge. */
+  VertexAroundFaceCirculator(const HalfEdgeIndex& idx_inner_half_edge, Mesh* const mesh)
+  : mesh_(mesh), idx_inner_half_edge_(idx_inner_half_edge)
+  {}
+
+  /** \brief Check if the circulator is valid.
+   * \warning Does NOT check if the stored mesh pointer is valid. You have to ensure
+   * this yourself when constructing the circulator. */
+  inline bool
+  isValid() const
+  {
+    return (idx_inner_half_edge_.isValid());
+  }
+
+  /** \brief Comparison operators (with boost::operators): == !=
+   * \warning Does NOT check if the circulators belong to the same mesh. Please check
+   * this yourself. */
+  inline bool
+  operator==(const Self& other) const
+  {
+    return (idx_inner_half_edge_ == other.idx_inner_half_edge_);
+  }
+
+  /** \brief Increment operators (with boost::operators): ++ (pre and post) */
+  inline Self&
+  operator++()
+  {
+    idx_inner_half_edge_ = mesh_->getNextHalfEdgeIndex(idx_inner_half_edge_);
+    return (*this);
+  }
+
+  /** \brief Decrement operators (with boost::operators): -- (pre and post) */
+  inline Self&
+  operator--()
+  {
+    idx_inner_half_edge_ = mesh_->getPrevHalfEdgeIndex(idx_inner_half_edge_);
+    return (*this);
+  }
+
+  /** \brief Get the index to the target vertex. */
+  inline VertexIndex
+  getTargetIndex() const
+  {
+    return (mesh_->getTerminatingVertexIndex(idx_inner_half_edge_));
+  }
+
+  /** \brief Get the half-edge that is currently stored in the circulator. */
+  inline HalfEdgeIndex
+  getCurrentHalfEdgeIndex() const
+  {
+    return (idx_inner_half_edge_);
+  }
+
+  /** \brief The mesh to which this circulator belongs to. */
+  Mesh* mesh_;
+
+  /** \brief The inner half-edge of the face around which we want to circulate. */
+  HalfEdgeIndex idx_inner_half_edge_;
+};
+} // End namespace geometry
 } // End namespace pcl
 
 ////////////////////////////////////////////////////////////////////////////////
 // InnerHalfEdgeAroundFaceCirculator
 ////////////////////////////////////////////////////////////////////////////////
 
-namespace pcl
-{
-  namespace geometry
-  {
-    /** \brief Circulates clockwise around a face and returns an index to the inner half-edge (the target). The best way to declare the circulator is to use the method pcl::geometry::MeshBase::getInnerHalfEdgeAroundFaceCirculator ().
-      * \tparam MeshT Mesh to which this circulator belongs to.
-      * \note The circulator can't be used to change the connectivity in the mesh (only const circulators are valid).
-      * \author Martin Saelzle
-      * \ingroup geometry
-      */
-    template <class MeshT>
-    class InnerHalfEdgeAroundFaceCirculator
-        : boost::equality_comparable <pcl::geometry::InnerHalfEdgeAroundFaceCirculator <MeshT>
-        , boost::unit_steppable      <pcl::geometry::InnerHalfEdgeAroundFaceCirculator <MeshT>
-        > >
-    {
-      public:
-
-        using Base = boost::equality_comparable <pcl::geometry::InnerHalfEdgeAroundFaceCirculator <MeshT>,
-                     boost::unit_steppable      <pcl::geometry::InnerHalfEdgeAroundFaceCirculator <MeshT> > >;
-        using Self = pcl::geometry::InnerHalfEdgeAroundFaceCirculator<MeshT>;
-
-        using Mesh = MeshT;
-        using FaceIndex = typename Mesh::FaceIndex;
-        using HalfEdgeIndex = typename Mesh::HalfEdgeIndex;
-
-        /** \brief Constructor resulting in an invalid circulator. */
-        InnerHalfEdgeAroundFaceCirculator ()
-          : mesh_                (nullptr),
-            idx_inner_half_edge_ ()
-        {
-        }
-
-        /** \brief Construct from the face around which we want to circulate. */
-        InnerHalfEdgeAroundFaceCirculator (const FaceIndex& idx_face,
-                                           Mesh*const       mesh)
-          : mesh_                (mesh),
-            idx_inner_half_edge_ (mesh->getInnerHalfEdgeIndex (idx_face))
-        {
-        }
-
-        /** \brief Construct directly from the inner half-edge. */
-        InnerHalfEdgeAroundFaceCirculator (const HalfEdgeIndex& idx_inner_half_edge,
-                                           Mesh*const           mesh)
-          : mesh_                (mesh),
-            idx_inner_half_edge_ (idx_inner_half_edge)
-        {
-        }
-
-        /** \brief Check if the circulator is valid.
-          * \warning Does NOT check if the stored mesh pointer is valid. You have to ensure this yourself when constructing the circulator. */
-        inline bool
-        isValid () const
-        {
-          return (idx_inner_half_edge_.isValid ());
-        }
-
-        /** \brief Comparison operators (with boost::operators): == !=
-          * \warning Does NOT check if the circulators belong to the same mesh. Please check this yourself. */
-        inline bool
-        operator == (const Self& other) const
-        {
-          return (idx_inner_half_edge_ == other.idx_inner_half_edge_);
-        }
-
-        /** \brief Increment operators (with boost::operators): ++ (pre and post) */
-        inline Self&
-        operator ++ ()
-        {
-          idx_inner_half_edge_ = mesh_->getNextHalfEdgeIndex (idx_inner_half_edge_);
-          return (*this);
-        }
-
-        /** \brief Decrement operators (with boost::operators): -- (pre and post) */
-        inline Self&
-        operator -- ()
-        {
-          idx_inner_half_edge_ = mesh_->getPrevHalfEdgeIndex (idx_inner_half_edge_);
-          return (*this);
-        }
-
-        /** \brief Get the index to the inner half-edge. */
-        inline HalfEdgeIndex
-        getTargetIndex () const
-        {
-          return (idx_inner_half_edge_);
-        }
-
-        /** \brief Get the half-edge that is currently stored in the circulator. */
-        inline HalfEdgeIndex
-        getCurrentHalfEdgeIndex () const
-        {
-          return (idx_inner_half_edge_);
-        }
-
-        /** \brief The mesh to which this circulator belongs to. */
-        Mesh* mesh_;
-
-        /** \brief The inner half-edge of the face around which we want to circulate. */
-        HalfEdgeIndex idx_inner_half_edge_;
-    };
-  } // End namespace geometry
+namespace pcl {
+namespace geometry {
+/** \brief Circulates clockwise around a face and returns an index to the inner
+ * half-edge (the target). The best way to declare the circulator is to use the method
+ * pcl::geometry::MeshBase::getInnerHalfEdgeAroundFaceCirculator ().
+ * \tparam MeshT Mesh to which this circulator belongs to.
+ * \note The circulator can't be used to change the connectivity in the mesh (only
+ * const circulators are valid).
+ * \author Martin Saelzle
+ * \ingroup geometry
+ */
+template <class MeshT>
+class InnerHalfEdgeAroundFaceCirculator
+: boost::equality_comparable<
+      pcl::geometry::InnerHalfEdgeAroundFaceCirculator<MeshT>,
+      boost::unit_steppable<pcl::geometry::InnerHalfEdgeAroundFaceCirculator<MeshT>>> {
+public:
+  using Base = boost::equality_comparable<
+      pcl::geometry::InnerHalfEdgeAroundFaceCirculator<MeshT>,
+      boost::unit_steppable<pcl::geometry::InnerHalfEdgeAroundFaceCirculator<MeshT>>>;
+  using Self = pcl::geometry::InnerHalfEdgeAroundFaceCirculator<MeshT>;
+
+  using Mesh = MeshT;
+  using FaceIndex = typename Mesh::FaceIndex;
+  using HalfEdgeIndex = typename Mesh::HalfEdgeIndex;
+
+  /** \brief Constructor resulting in an invalid circulator. */
+  InnerHalfEdgeAroundFaceCirculator() : mesh_(nullptr), idx_inner_half_edge_() {}
+
+  /** \brief Construct from the face around which we want to circulate. */
+  InnerHalfEdgeAroundFaceCirculator(const FaceIndex& idx_face, Mesh* const mesh)
+  : mesh_(mesh), idx_inner_half_edge_(mesh->getInnerHalfEdgeIndex(idx_face))
+  {}
+
+  /** \brief Construct directly from the inner half-edge. */
+  InnerHalfEdgeAroundFaceCirculator(const HalfEdgeIndex& idx_inner_half_edge,
+                                    Mesh* const mesh)
+  : mesh_(mesh), idx_inner_half_edge_(idx_inner_half_edge)
+  {}
+
+  /** \brief Check if the circulator is valid.
+   * \warning Does NOT check if the stored mesh pointer is valid. You have to ensure
+   * this yourself when constructing the circulator. */
+  inline bool
+  isValid() const
+  {
+    return (idx_inner_half_edge_.isValid());
+  }
+
+  /** \brief Comparison operators (with boost::operators): == !=
+   * \warning Does NOT check if the circulators belong to the same mesh. Please check
+   * this yourself. */
+  inline bool
+  operator==(const Self& other) const
+  {
+    return (idx_inner_half_edge_ == other.idx_inner_half_edge_);
+  }
+
+  /** \brief Increment operators (with boost::operators): ++ (pre and post) */
+  inline Self&
+  operator++()
+  {
+    idx_inner_half_edge_ = mesh_->getNextHalfEdgeIndex(idx_inner_half_edge_);
+    return (*this);
+  }
+
+  /** \brief Decrement operators (with boost::operators): -- (pre and post) */
+  inline Self&
+  operator--()
+  {
+    idx_inner_half_edge_ = mesh_->getPrevHalfEdgeIndex(idx_inner_half_edge_);
+    return (*this);
+  }
+
+  /** \brief Get the index to the inner half-edge. */
+  inline HalfEdgeIndex
+  getTargetIndex() const
+  {
+    return (idx_inner_half_edge_);
+  }
+
+  /** \brief Get the half-edge that is currently stored in the circulator. */
+  inline HalfEdgeIndex
+  getCurrentHalfEdgeIndex() const
+  {
+    return (idx_inner_half_edge_);
+  }
+
+  /** \brief The mesh to which this circulator belongs to. */
+  Mesh* mesh_;
+
+  /** \brief The inner half-edge of the face around which we want to circulate. */
+  HalfEdgeIndex idx_inner_half_edge_;
+};
+} // End namespace geometry
 } // End namespace pcl
 
 ////////////////////////////////////////////////////////////////////////////////
 // OuterHalfEdgeAroundFaceCirculator
 ////////////////////////////////////////////////////////////////////////////////
 
-namespace pcl
-{
-  namespace geometry
-  {
-    /** \brief Circulates clockwise around a face and returns an index to the outer half-edge (the target). The best way to declare the circulator is to use the method pcl::geometry::MeshBase::getOuterHalfEdgeAroundFaceCirculator ().
-      * \tparam MeshT Mesh to which this circulator belongs to.
-      * \note The circulator can't be used to change the connectivity in the mesh (only const circulators are valid).
-      * \author Martin Saelzle
-      * \ingroup geometry
-      */
-    template <class MeshT>
-    class OuterHalfEdgeAroundFaceCirculator
-        : boost::equality_comparable <pcl::geometry::OuterHalfEdgeAroundFaceCirculator <MeshT>
-        , boost::unit_steppable      <pcl::geometry::OuterHalfEdgeAroundFaceCirculator <MeshT>
-        > >
-    {
-      public:
-
-        using Base = boost::equality_comparable <pcl::geometry::OuterHalfEdgeAroundFaceCirculator <MeshT>,
-                     boost::unit_steppable      <pcl::geometry::OuterHalfEdgeAroundFaceCirculator <MeshT> > >;
-        using Self = pcl::geometry::OuterHalfEdgeAroundFaceCirculator<MeshT>;
-
-        using Mesh = MeshT;
-        using FaceIndex = typename Mesh::FaceIndex;
-        using HalfEdgeIndex = typename Mesh::HalfEdgeIndex;
-
-        /** \brief Constructor resulting in an invalid circulator. */
-        OuterHalfEdgeAroundFaceCirculator ()
-          : mesh_                (nullptr),
-            idx_inner_half_edge_ ()
-        {
-        }
-
-        /** \brief Construct from the face around which we want to circulate. */
-        OuterHalfEdgeAroundFaceCirculator (const FaceIndex& idx_face,
-                                           Mesh*const       mesh)
-          : mesh_                (mesh),
-            idx_inner_half_edge_ (mesh->getInnerHalfEdgeIndex (idx_face))
-        {
-        }
-
-        /** \brief Construct directly from the inner half-edge. */
-        OuterHalfEdgeAroundFaceCirculator (const HalfEdgeIndex& idx_inner_half_edge,
-                                           Mesh*const           mesh)
-          : mesh_                (mesh),
-            idx_inner_half_edge_ (idx_inner_half_edge)
-        {
-        }
-
-        /** \brief Check if the circulator is valid.
-          * \warning Does NOT check if the stored mesh pointer is valid. You have to ensure this yourself when constructing the circulator. */
-        inline bool
-        isValid () const
-        {
-          return (idx_inner_half_edge_.isValid ());
-        }
-
-        /** \brief Comparison operators (with boost::operators): == !=
-          * \warning Does NOT check if the circulators belong to the same mesh. Please check this yourself. */
-        inline bool
-        operator == (const Self& other) const
-        {
-          return (idx_inner_half_edge_ == other.idx_inner_half_edge_);
-        }
-
-        /** \brief Increment operators (with boost::operators): ++ (pre and post) */
-        inline Self&
-        operator ++ ()
-        {
-          idx_inner_half_edge_ = mesh_->getNextHalfEdgeIndex (idx_inner_half_edge_);
-          return (*this);
-        }
-
-        /** \brief Decrement operators (with boost::operators): -- (pre and post) */
-        inline Self&
-        operator -- ()
-        {
-          idx_inner_half_edge_ = mesh_->getPrevHalfEdgeIndex (idx_inner_half_edge_);
-          return (*this);
-        }
-
-        /** \brief Get the index to the outer half-edge. */
-        inline HalfEdgeIndex
-        getTargetIndex () const
-        {
-          return (mesh_->getOppositeHalfEdgeIndex (idx_inner_half_edge_));
-        }
-
-        /** \brief Get the half-edge that is currently stored in the circulator. */
-        inline HalfEdgeIndex
-        getCurrentHalfEdgeIndex () const
-        {
-          return (idx_inner_half_edge_);
-        }
-
-        /** \brief The mesh to which this circulator belongs to. */
-        Mesh* mesh_;
-
-        /** \brief The inner half-edge of the face around which we want to circulate. */
-        HalfEdgeIndex idx_inner_half_edge_;
-    };
-  } // End namespace geometry
+namespace pcl {
+namespace geometry {
+/** \brief Circulates clockwise around a face and returns an index to the outer
+ * half-edge (the target). The best way to declare the circulator is to use the method
+ * pcl::geometry::MeshBase::getOuterHalfEdgeAroundFaceCirculator ().
+ * \tparam MeshT Mesh to which this circulator belongs to.
+ * \note The circulator can't be used to change the connectivity in the mesh (only
+ * const circulators are valid).
+ * \author Martin Saelzle
+ * \ingroup geometry
+ */
+template <class MeshT>
+class OuterHalfEdgeAroundFaceCirculator
+: boost::equality_comparable<
+      pcl::geometry::OuterHalfEdgeAroundFaceCirculator<MeshT>,
+      boost::unit_steppable<pcl::geometry::OuterHalfEdgeAroundFaceCirculator<MeshT>>> {
+public:
+  using Base = boost::equality_comparable<
+      pcl::geometry::OuterHalfEdgeAroundFaceCirculator<MeshT>,
+      boost::unit_steppable<pcl::geometry::OuterHalfEdgeAroundFaceCirculator<MeshT>>>;
+  using Self = pcl::geometry::OuterHalfEdgeAroundFaceCirculator<MeshT>;
+
+  using Mesh = MeshT;
+  using FaceIndex = typename Mesh::FaceIndex;
+  using HalfEdgeIndex = typename Mesh::HalfEdgeIndex;
+
+  /** \brief Constructor resulting in an invalid circulator. */
+  OuterHalfEdgeAroundFaceCirculator() : mesh_(nullptr), idx_inner_half_edge_() {}
+
+  /** \brief Construct from the face around which we want to circulate. */
+  OuterHalfEdgeAroundFaceCirculator(const FaceIndex& idx_face, Mesh* const mesh)
+  : mesh_(mesh), idx_inner_half_edge_(mesh->getInnerHalfEdgeIndex(idx_face))
+  {}
+
+  /** \brief Construct directly from the inner half-edge. */
+  OuterHalfEdgeAroundFaceCirculator(const HalfEdgeIndex& idx_inner_half_edge,
+                                    Mesh* const mesh)
+  : mesh_(mesh), idx_inner_half_edge_(idx_inner_half_edge)
+  {}
+
+  /** \brief Check if the circulator is valid.
+   * \warning Does NOT check if the stored mesh pointer is valid. You have to ensure
+   * this yourself when constructing the circulator. */
+  inline bool
+  isValid() const
+  {
+    return (idx_inner_half_edge_.isValid());
+  }
+
+  /** \brief Comparison operators (with boost::operators): == !=
+   * \warning Does NOT check if the circulators belong to the same mesh. Please check
+   * this yourself. */
+  inline bool
+  operator==(const Self& other) const
+  {
+    return (idx_inner_half_edge_ == other.idx_inner_half_edge_);
+  }
+
+  /** \brief Increment operators (with boost::operators): ++ (pre and post) */
+  inline Self&
+  operator++()
+  {
+    idx_inner_half_edge_ = mesh_->getNextHalfEdgeIndex(idx_inner_half_edge_);
+    return (*this);
+  }
+
+  /** \brief Decrement operators (with boost::operators): -- (pre and post) */
+  inline Self&
+  operator--()
+  {
+    idx_inner_half_edge_ = mesh_->getPrevHalfEdgeIndex(idx_inner_half_edge_);
+    return (*this);
+  }
+
+  /** \brief Get the index to the outer half-edge. */
+  inline HalfEdgeIndex
+  getTargetIndex() const
+  {
+    return (mesh_->getOppositeHalfEdgeIndex(idx_inner_half_edge_));
+  }
+
+  /** \brief Get the half-edge that is currently stored in the circulator. */
+  inline HalfEdgeIndex
+  getCurrentHalfEdgeIndex() const
+  {
+    return (idx_inner_half_edge_);
+  }
+
+  /** \brief The mesh to which this circulator belongs to. */
+  Mesh* mesh_;
+
+  /** \brief The inner half-edge of the face around which we want to circulate. */
+  HalfEdgeIndex idx_inner_half_edge_;
+};
+} // End namespace geometry
 } // End namespace pcl
 
 ////////////////////////////////////////////////////////////////////////////////
 // FaceAroundFaceCirculator
 ////////////////////////////////////////////////////////////////////////////////
 
-namespace pcl
-{
-  namespace geometry
-  {
-    /** \brief Circulates clockwise around a face and returns an index to the face of the outer half-edge (the target). The best way to declare the circulator is to use the method pcl::geometry::MeshBase::getFaceAroundFaceCirculator ().
-      * \tparam MeshT Mesh to which this circulator belongs to.
-      * \note The circulator can't be used to change the connectivity in the mesh (only const circulators are valid).
-      * \author Martin Saelzle
-      * \ingroup geometry
-      */
-    template <class MeshT>
-    class FaceAroundFaceCirculator
-        : boost::equality_comparable <pcl::geometry::FaceAroundFaceCirculator <MeshT>
-        , boost::unit_steppable      <pcl::geometry::FaceAroundFaceCirculator <MeshT>
-        > >
-    {
-      public:
-
-        using Base = boost::equality_comparable <pcl::geometry::FaceAroundFaceCirculator <MeshT>,
-                     boost::unit_steppable      <pcl::geometry::FaceAroundFaceCirculator <MeshT> > >;
-        using Self = pcl::geometry::FaceAroundFaceCirculator<MeshT>;
-
-        using Mesh = MeshT;
-        using FaceIndex = typename Mesh::FaceIndex;
-        using HalfEdgeIndex = typename Mesh::HalfEdgeIndex;
-
-        /** \brief Constructor resulting in an invalid circulator. */
-        FaceAroundFaceCirculator ()
-          : mesh_                (nullptr),
-            idx_inner_half_edge_ ()
-        {
-        }
-
-        /** \brief Construct from the face around which we want to circulate. */
-        FaceAroundFaceCirculator (const FaceIndex& idx_face,
-                                  Mesh*const       mesh)
-          : mesh_                (mesh),
-            idx_inner_half_edge_ (mesh->getInnerHalfEdgeIndex (idx_face))
-        {
-        }
-
-        /** \brief Construct directly from the inner half-edge. */
-        FaceAroundFaceCirculator (const HalfEdgeIndex& idx_inner_half_edge,
-                                  Mesh*const           mesh)
-          : mesh_                (mesh),
-            idx_inner_half_edge_ (idx_inner_half_edge)
-        {
-        }
-
-        /** \brief Check if the circulator is valid.
-          * \warning Does NOT check if the stored mesh pointer is valid. You have to ensure this yourself when constructing the circulator. */
-        inline bool
-        isValid () const
-        {
-          return (idx_inner_half_edge_.isValid ());
-        }
-
-        /** \brief Comparison operators (with boost::operators): == !=
-          * \warning Does NOT check if the circulators belong to the same mesh. Please check this yourself. */
-        inline bool
-        operator == (const Self& other) const
-        {
-          return (idx_inner_half_edge_ == other.idx_inner_half_edge_);
-        }
-
-        /** \brief Increment operators (with boost::operators): ++ (pre and post) */
-        inline Self&
-        operator ++ ()
-        {
-          idx_inner_half_edge_ = mesh_->getNextHalfEdgeIndex (idx_inner_half_edge_);
-          return (*this);
-        }
-
-        /** \brief Decrement operators (with boost::operators): -- (pre and post) */
-        inline Self&
-        operator -- ()
-        {
-          idx_inner_half_edge_ = mesh_->getPrevHalfEdgeIndex (idx_inner_half_edge_);
-          return (*this);
-        }
-
-        /** \brief Get the index to the target face. */
-        inline FaceIndex
-        getTargetIndex () const
-        {
-          return (mesh_->getOppositeFaceIndex (idx_inner_half_edge_));
-        }
-
-        /** \brief Get the half-edge that is currently stored in the circulator. */
-        inline HalfEdgeIndex
-        getCurrentHalfEdgeIndex () const
-        {
-          return (idx_inner_half_edge_);
-        }
-
-        /** \brief The mesh to which this circulator belongs to. */
-        Mesh* mesh_;
-
-        /** \brief The inner half-edge of the face around which we want to circulate. */
-        HalfEdgeIndex idx_inner_half_edge_;
-    };
-  } // End namespace geometry
+namespace pcl {
+namespace geometry {
+/** \brief Circulates clockwise around a face and returns an index to the face of the
+ * outer half-edge (the target). The best way to declare the circulator is to use the
+ * method pcl::geometry::MeshBase::getFaceAroundFaceCirculator ().
+ * \tparam MeshT Mesh to which this circulator belongs to.
+ * \note The circulator can't be used to change the connectivity in the mesh (only
+ * const circulators are valid).
+ * \author Martin Saelzle
+ * \ingroup geometry
+ */
+template <class MeshT>
+class FaceAroundFaceCirculator
+: boost::equality_comparable<
+      pcl::geometry::FaceAroundFaceCirculator<MeshT>,
+      boost::unit_steppable<pcl::geometry::FaceAroundFaceCirculator<MeshT>>> {
+public:
+  using Base = boost::equality_comparable<
+      pcl::geometry::FaceAroundFaceCirculator<MeshT>,
+      boost::unit_steppable<pcl::geometry::FaceAroundFaceCirculator<MeshT>>>;
+  using Self = pcl::geometry::FaceAroundFaceCirculator<MeshT>;
+
+  using Mesh = MeshT;
+  using FaceIndex = typename Mesh::FaceIndex;
+  using HalfEdgeIndex = typename Mesh::HalfEdgeIndex;
+
+  /** \brief Constructor resulting in an invalid circulator. */
+  FaceAroundFaceCirculator() : mesh_(nullptr), idx_inner_half_edge_() {}
+
+  /** \brief Construct from the face around which we want to circulate. */
+  FaceAroundFaceCirculator(const FaceIndex& idx_face, Mesh* const mesh)
+  : mesh_(mesh), idx_inner_half_edge_(mesh->getInnerHalfEdgeIndex(idx_face))
+  {}
+
+  /** \brief Construct directly from the inner half-edge. */
+  FaceAroundFaceCirculator(const HalfEdgeIndex& idx_inner_half_edge, Mesh* const mesh)
+  : mesh_(mesh), idx_inner_half_edge_(idx_inner_half_edge)
+  {}
+
+  /** \brief Check if the circulator is valid.
+   * \warning Does NOT check if the stored mesh pointer is valid. You have to ensure
+   * this yourself when constructing the circulator. */
+  inline bool
+  isValid() const
+  {
+    return (idx_inner_half_edge_.isValid());
+  }
+
+  /** \brief Comparison operators (with boost::operators): == !=
+   * \warning Does NOT check if the circulators belong to the same mesh. Please check
+   * this yourself. */
+  inline bool
+  operator==(const Self& other) const
+  {
+    return (idx_inner_half_edge_ == other.idx_inner_half_edge_);
+  }
+
+  /** \brief Increment operators (with boost::operators): ++ (pre and post) */
+  inline Self&
+  operator++()
+  {
+    idx_inner_half_edge_ = mesh_->getNextHalfEdgeIndex(idx_inner_half_edge_);
+    return (*this);
+  }
+
+  /** \brief Decrement operators (with boost::operators): -- (pre and post) */
+  inline Self&
+  operator--()
+  {
+    idx_inner_half_edge_ = mesh_->getPrevHalfEdgeIndex(idx_inner_half_edge_);
+    return (*this);
+  }
+
+  /** \brief Get the index to the target face. */
+  inline FaceIndex
+  getTargetIndex() const
+  {
+    return (mesh_->getOppositeFaceIndex(idx_inner_half_edge_));
+  }
+
+  /** \brief Get the half-edge that is currently stored in the circulator. */
+  inline HalfEdgeIndex
+  getCurrentHalfEdgeIndex() const
+  {
+    return (idx_inner_half_edge_);
+  }
+
+  /** \brief The mesh to which this circulator belongs to. */
+  Mesh* mesh_;
+
+  /** \brief The inner half-edge of the face around which we want to circulate. */
+  HalfEdgeIndex idx_inner_half_edge_;
+};
+} // End namespace geometry
 } // End namespace pcl
diff --git a/geometry/include/pcl/geometry/mesh_conversion.h b/geometry/include/pcl/geometry/mesh_conversion.h
index c900709d..68125dad 100644
--- a/geometry/include/pcl/geometry/mesh_conversion.h
+++ b/geometry/include/pcl/geometry/mesh_conversion.h
@@ -43,88 +43,85 @@
 #include <pcl/PolygonMesh.h>
 #include <pcl/conversions.h>
 
-namespace pcl
+namespace pcl {
+namespace geometry {
+/** \brief Convert a half-edge mesh to a face-vertex mesh.
+ * \param[in] half_edge_mesh The input mesh.
+ * \param[out] face_vertex_mesh The output mesh.
+ * \author Martin Saelzle
+ * \ingroup geometry
+ */
+template <class HalfEdgeMeshT>
+void
+toFaceVertexMesh(const HalfEdgeMeshT& half_edge_mesh,
+                 pcl::PolygonMesh& face_vertex_mesh)
 {
-  namespace geometry
-  {
-    /** \brief Convert a half-edge mesh to a face-vertex mesh.
-      * \param[in] half_edge_mesh The input mesh.
-      * \param[out] face_vertex_mesh The output mesh.
-      * \author Martin Saelzle
-      * \ingroup geometry
-      */
-    template <class HalfEdgeMeshT> void
-    toFaceVertexMesh (const HalfEdgeMeshT& half_edge_mesh, pcl::PolygonMesh& face_vertex_mesh)
-    {
-      using HalfEdgeMesh = HalfEdgeMeshT;
-      using VAFC = typename HalfEdgeMesh::VertexAroundFaceCirculator;
-      using FaceIndex = typename HalfEdgeMesh::FaceIndex;
-
-      pcl::Vertices polygon;
-      pcl::toPCLPointCloud2 (half_edge_mesh.getVertexDataCloud (), face_vertex_mesh.cloud);
+  using HalfEdgeMesh = HalfEdgeMeshT;
+  using VAFC = typename HalfEdgeMesh::VertexAroundFaceCirculator;
+  using FaceIndex = typename HalfEdgeMesh::FaceIndex;
 
-      face_vertex_mesh.polygons.reserve (half_edge_mesh.sizeFaces ());
-      for (std::size_t i=0; i<half_edge_mesh.sizeFaces (); ++i)
-      {
-        VAFC       circ     = half_edge_mesh.getVertexAroundFaceCirculator (FaceIndex (i));
-        const VAFC circ_end = circ;
-        polygon.vertices.clear ();
-        do
-        {
-          polygon.vertices.push_back (circ.getTargetIndex ().get ());
-        } while (++circ != circ_end);
-        face_vertex_mesh.polygons.push_back (polygon);
-      }
-    }
+  pcl::Vertices polygon;
+  pcl::toPCLPointCloud2(half_edge_mesh.getVertexDataCloud(), face_vertex_mesh.cloud);
 
-    /** \brief Convert a face-vertex mesh to a half-edge mesh.
-      * \param[in] face_vertex_mesh The input mesh.
-      * \param[out] half_edge_mesh The output mesh. It must have data associated with the vertices.
-      * \return The number of faces that could NOT be added to the half-edge mesh.
-      * \author Martin Saelzle
-      * \ingroup geometry
-      */
-    template <class HalfEdgeMeshT> int
-    toHalfEdgeMesh (const pcl::PolygonMesh& face_vertex_mesh, HalfEdgeMeshT& half_edge_mesh)
-    {
-      using HalfEdgeMesh = HalfEdgeMeshT;
-      using VertexDataCloud = typename HalfEdgeMesh::VertexDataCloud;
-      using VertexIndices = typename HalfEdgeMesh::VertexIndices;
+  face_vertex_mesh.polygons.reserve(half_edge_mesh.sizeFaces());
+  for (std::size_t i = 0; i < half_edge_mesh.sizeFaces(); ++i) {
+    VAFC circ = half_edge_mesh.getVertexAroundFaceCirculator(FaceIndex(i));
+    const VAFC circ_end = circ;
+    polygon.vertices.clear();
+    do {
+      polygon.vertices.push_back(circ.getTargetIndex().get());
+    } while (++circ != circ_end);
+    face_vertex_mesh.polygons.push_back(polygon);
+  }
+}
 
-      static_assert (HalfEdgeMesh::HasVertexData::value, "Output mesh must have data associated with the vertices!");
+/** \brief Convert a face-vertex mesh to a half-edge mesh.
+ * \param[in] face_vertex_mesh The input mesh.
+ * \param[out] half_edge_mesh The output mesh. It must have data associated with the
+ * vertices.
+ * \return The number of faces that could NOT be added to the half-edge mesh.
+ * \author Martin Saelzle
+ * \ingroup geometry
+ */
+template <class HalfEdgeMeshT>
+int
+toHalfEdgeMesh(const pcl::PolygonMesh& face_vertex_mesh, HalfEdgeMeshT& half_edge_mesh)
+{
+  using HalfEdgeMesh = HalfEdgeMeshT;
+  using VertexDataCloud = typename HalfEdgeMesh::VertexDataCloud;
+  using VertexIndices = typename HalfEdgeMesh::VertexIndices;
 
-      VertexDataCloud vertices;
-      pcl::fromPCLPointCloud2 (face_vertex_mesh.cloud, vertices);
+  static_assert(HalfEdgeMesh::HasVertexData::value,
+                "Output mesh must have data associated with the vertices!");
 
-      half_edge_mesh.reserveVertices (vertices.size ());
-      half_edge_mesh.reserveEdges (3 * face_vertex_mesh.polygons.size ());
-      half_edge_mesh.reserveFaces (    face_vertex_mesh.polygons.size ());
+  VertexDataCloud vertices;
+  pcl::fromPCLPointCloud2(face_vertex_mesh.cloud, vertices);
 
-      for (const auto &vertex : vertices)
-      {
-        half_edge_mesh.addVertex (vertex);
-      }
+  half_edge_mesh.reserveVertices(vertices.size());
+  half_edge_mesh.reserveEdges(3 * face_vertex_mesh.polygons.size());
+  half_edge_mesh.reserveFaces(face_vertex_mesh.polygons.size());
 
-      assert (half_edge_mesh.sizeVertices () == vertices.size ());
+  for (const auto& vertex : vertices) {
+    half_edge_mesh.addVertex(vertex);
+  }
 
-      int count_not_added = 0;
-      VertexIndices vi;
-      vi.reserve (3); // Minimum number (triangle)
-      for (const auto &polygon : face_vertex_mesh.polygons)
-      {
-        vi.clear ();
-        for (const unsigned int &vertex : polygon.vertices)
-        {
-          vi.emplace_back (vertex);
-        }
+  assert(half_edge_mesh.sizeVertices() == vertices.size());
 
-        if (!half_edge_mesh.addFace (vi).isValid ())
-        {
-          ++count_not_added;
-        }
-      }
+  int count_not_added = 0;
+  VertexIndices vi;
+  vi.reserve(3); // Minimum number (triangle)
+  for (const auto& polygon : face_vertex_mesh.polygons) {
+    vi.clear();
+    for (const auto& vertex : polygon.vertices) {
+      vi.emplace_back(vertex);
+    }
 
-      return (count_not_added);
+    if (!half_edge_mesh.addFace(vi).isValid()) {
+      ++count_not_added;
     }
-  } // End namespace geometry
+  }
+
+  return (count_not_added);
+}
+} // End namespace geometry
 } // End namespace pcl
diff --git a/geometry/include/pcl/geometry/mesh_elements.h b/geometry/include/pcl/geometry/mesh_elements.h
index 284462e1..3bd49770 100644
--- a/geometry/include/pcl/geometry/mesh_elements.h
+++ b/geometry/include/pcl/geometry/mesh_elements.h
@@ -42,146 +42,144 @@
 
 #include <pcl/geometry/mesh_indices.h>
 
-namespace pcl
-{
-  namespace geometry
-  {
-    template <class DerivedT, class MeshTraitsT, class MeshTagT>
-    class MeshBase;
-
-    template <class MeshT>
-    class MeshIO;
-  } // End namespace geometry
+namespace pcl {
+namespace geometry {
+template <class DerivedT, class MeshTraitsT, class MeshTagT>
+class MeshBase;
+
+template <class MeshT>
+class MeshIO;
+} // End namespace geometry
 } // End namespace pcl
 
 ////////////////////////////////////////////////////////////////////////////////
 // Vertex
 ////////////////////////////////////////////////////////////////////////////////
 
-namespace pcl
-{
-  namespace geometry
-  {
-    /** \brief A vertex is a node in the mesh.
-      * \author Martin Saelzle
-      * \ingroup geometry
-      */
-    class Vertex
-    {
-      private:
-
-        using HalfEdgeIndex = pcl::geometry::HalfEdgeIndex;
-
-        /** \brief Constructor.
-          * \param[in] idx_outgoing_half_edge Index to the outgoing half-edge. Defaults to an invalid index.
-          */
-        explicit Vertex (const HalfEdgeIndex& idx_outgoing_half_edge = HalfEdgeIndex ())
-          : idx_outgoing_half_edge_ (idx_outgoing_half_edge)
-        {}
-
-        /** \brief Index to the outgoing half-edge. The vertex is considered to be deleted if it stores an invalid outgoing half-edge index. */
-        HalfEdgeIndex idx_outgoing_half_edge_;
-
-        template <class DerivedT, class MeshTraitsT, class MeshTagT>
-        friend class pcl::geometry::MeshBase;
-
-        template <class MeshT>
-        friend class pcl::geometry::MeshIO;
-    };
-  } // End namespace geometry
+namespace pcl {
+namespace geometry {
+/** \brief A vertex is a node in the mesh.
+ * \author Martin Saelzle
+ * \ingroup geometry
+ */
+class Vertex {
+private:
+  using HalfEdgeIndex = pcl::geometry::HalfEdgeIndex;
+
+  /** \brief Constructor.
+   * \param[in] idx_outgoing_half_edge Index to the outgoing half-edge. Defaults to an
+   * invalid index.
+   */
+  explicit Vertex(const HalfEdgeIndex& idx_outgoing_half_edge = HalfEdgeIndex())
+  : idx_outgoing_half_edge_(idx_outgoing_half_edge)
+  {}
+
+  /** \brief Index to the outgoing half-edge. The vertex is considered to be deleted if
+   * it stores an invalid outgoing half-edge index. */
+  HalfEdgeIndex idx_outgoing_half_edge_;
+
+  template <class DerivedT, class MeshTraitsT, class MeshTagT>
+  friend class pcl::geometry::MeshBase;
+
+  template <class MeshT>
+  friend class pcl::geometry::MeshIO;
+};
+} // End namespace geometry
 } // End namespace pcl
 
 ////////////////////////////////////////////////////////////////////////////////
 // HalfEdge
 ////////////////////////////////////////////////////////////////////////////////
 
-namespace pcl
-{
-  namespace geometry
-  {
-    /** \brief An edge is a connection between two vertices. In a half-edge mesh the edge is split into two half-edges with opposite orientation. Each half-edge stores the index to the terminating vertex, the next half-edge, the previous half-edge and the face it belongs to. The opposite half-edge is accessed implicitly.
-      * \author Martin Saelzle
-      * \ingroup geometry
-      */
-    class HalfEdge
-    {
-      private:
-
-        using VertexIndex = pcl::geometry::VertexIndex;
-        using HalfEdgeIndex = pcl::geometry::HalfEdgeIndex;
-        using FaceIndex = pcl::geometry::FaceIndex;
-
-        /** \brief Constructor.
-          * \param[in] idx_terminating_vertex Index to the terminating vertex. Defaults to an invalid index.
-          * \param[in] idx_next_half_edge     Index to the next half-edge. Defaults to an invalid index.
-          * \param[in] idx_prev_half_edge     Index to the previous half-edge. Defaults to an invalid index.
-          * \param[in] idx_face               Index to the face. Defaults to an invalid index.
-          */
-        explicit HalfEdge (const VertexIndex&   idx_terminating_vertex = VertexIndex   (),
-                           const HalfEdgeIndex& idx_next_half_edge     = HalfEdgeIndex (),
-                           const HalfEdgeIndex& idx_prev_half_edge     = HalfEdgeIndex (),
-                           const FaceIndex&     idx_face               = FaceIndex     ())
-          : idx_terminating_vertex_ (idx_terminating_vertex),
-            idx_next_half_edge_     (idx_next_half_edge),
-            idx_prev_half_edge_     (idx_prev_half_edge),
-            idx_face_               (idx_face)
-        {
-        }
-
-        /** \brief Index to the terminating vertex. The half-edge is considered to be deleted if it stores an invalid terminating vertex index. */
-        VertexIndex idx_terminating_vertex_;
-
-        /** \brief Index to the next half-edge. */
-        HalfEdgeIndex idx_next_half_edge_;
-
-        /** \brief Index to the previous half-edge. */
-        HalfEdgeIndex idx_prev_half_edge_;
-
-        /** \brief Index to the face. The half-edge is considered to be on the boundary if it stores an invalid face index. */
-        FaceIndex idx_face_;
-
-        template <class DerivedT, class MeshTraitsT, class MeshTagT>
-        friend class pcl::geometry::MeshBase;
-
-        template <class MeshT>
-        friend class pcl::geometry::MeshIO;
-    };
-  } // End namespace geometry
+namespace pcl {
+namespace geometry {
+/** \brief An edge is a connection between two vertices. In a half-edge mesh the edge is
+ * split into two half-edges with opposite orientation. Each half-edge stores the index
+ * to the terminating vertex, the next half-edge, the previous half-edge and the face it
+ * belongs to. The opposite half-edge is accessed implicitly.
+ * \author Martin Saelzle
+ * \ingroup geometry
+ */
+class HalfEdge {
+private:
+  using VertexIndex = pcl::geometry::VertexIndex;
+  using HalfEdgeIndex = pcl::geometry::HalfEdgeIndex;
+  using FaceIndex = pcl::geometry::FaceIndex;
+
+  /** \brief Constructor.
+   * \param[in] idx_terminating_vertex Index to the terminating vertex. Defaults to an
+   * invalid index.
+   * \param[in] idx_next_half_edge     Index to the next half-edge.
+   * Defaults to an invalid index.
+   * \param[in] idx_prev_half_edge     Index to the
+   * previous half-edge. Defaults to an invalid index.
+   * \param[in] idx_face Index to the
+   * face. Defaults to an invalid index.
+   */
+  explicit HalfEdge(const VertexIndex& idx_terminating_vertex = VertexIndex(),
+                    const HalfEdgeIndex& idx_next_half_edge = HalfEdgeIndex(),
+                    const HalfEdgeIndex& idx_prev_half_edge = HalfEdgeIndex(),
+                    const FaceIndex& idx_face = FaceIndex())
+  : idx_terminating_vertex_(idx_terminating_vertex)
+  , idx_next_half_edge_(idx_next_half_edge)
+  , idx_prev_half_edge_(idx_prev_half_edge)
+  , idx_face_(idx_face)
+  {}
+
+  /** \brief Index to the terminating vertex. The half-edge is considered to be deleted
+   * if it stores an invalid terminating vertex index. */
+  VertexIndex idx_terminating_vertex_;
+
+  /** \brief Index to the next half-edge. */
+  HalfEdgeIndex idx_next_half_edge_;
+
+  /** \brief Index to the previous half-edge. */
+  HalfEdgeIndex idx_prev_half_edge_;
+
+  /** \brief Index to the face. The half-edge is considered to be on the boundary if it
+   * stores an invalid face index. */
+  FaceIndex idx_face_;
+
+  template <class DerivedT, class MeshTraitsT, class MeshTagT>
+  friend class pcl::geometry::MeshBase;
+
+  template <class MeshT>
+  friend class pcl::geometry::MeshIO;
+};
+} // End namespace geometry
 } // End namespace pcl
 
 ////////////////////////////////////////////////////////////////////////////////
 // Face
 ////////////////////////////////////////////////////////////////////////////////
 
-namespace pcl
-{
-  namespace geometry
-  {
-    /** \brief A face is a closed loop of edges.
-      * \author Martin Saelzle
-      * \ingroup geometry
-      */
-    class Face
-    {
-      private:
-
-        using HalfEdgeIndex = pcl::geometry::HalfEdgeIndex;
-
-        /** \brief Constructor.
-          * \param[in] inner_half_edge_idx Index to the outgoing half-edge. Defaults to an invalid index
-          */
-        explicit Face (const HalfEdgeIndex& idx_inner_half_edge = HalfEdgeIndex ())
-          : idx_inner_half_edge_ (idx_inner_half_edge)
-        {}
-
-        /** \brief Index to the inner half-edge. The face is considered to be deleted if it stores an invalid inner half-edge index. */
-        HalfEdgeIndex idx_inner_half_edge_;
-
-        template <class DerivedT, class MeshTraitsT, class MeshTagT>
-        friend class pcl::geometry::MeshBase;
-
-        template <class MeshT>
-        friend class pcl::geometry::MeshIO;
-    };
-  } // End namespace geometry
+namespace pcl {
+namespace geometry {
+/** \brief A face is a closed loop of edges.
+ * \author Martin Saelzle
+ * \ingroup geometry
+ */
+class Face {
+private:
+  using HalfEdgeIndex = pcl::geometry::HalfEdgeIndex;
+
+  /** \brief Constructor.
+   * \param[in] inner_half_edge_idx Index to the outgoing half-edge. Defaults to an
+   * invalid index
+   */
+  explicit Face(const HalfEdgeIndex& idx_inner_half_edge = HalfEdgeIndex())
+  : idx_inner_half_edge_(idx_inner_half_edge)
+  {}
+
+  /** \brief Index to the inner half-edge. The face is considered to be deleted if it
+   * stores an invalid inner half-edge index. */
+  HalfEdgeIndex idx_inner_half_edge_;
+
+  template <class DerivedT, class MeshTraitsT, class MeshTagT>
+  friend class pcl::geometry::MeshBase;
+
+  template <class MeshT>
+  friend class pcl::geometry::MeshIO;
+};
+} // End namespace geometry
 } // End namespace pcl
diff --git a/geometry/include/pcl/geometry/mesh_indices.h b/geometry/include/pcl/geometry/mesh_indices.h
index 8c5a10e0..f21e1b83 100644
--- a/geometry/include/pcl/geometry/mesh_indices.h
+++ b/geometry/include/pcl/geometry/mesh_indices.h
@@ -38,593 +38,204 @@
  *
  */
 
-// NOTE: This file has been created with 'pcl_src/geometry/include/pcl/geometry/mesh_indices.py'
-
 #pragma once
 
-#include <iostream>
+#include <boost/operators.hpp>
 
-#include <pcl/geometry/boost.h>
+#include <iostream>
 
 ////////////////////////////////////////////////////////////////////////////////
-// VertexIndex
+// MeshIndex
 ////////////////////////////////////////////////////////////////////////////////
 
-namespace pcl
-{
-  namespace geometry
+namespace pcl {
+namespace detail {
+
+template <class IndexTagT>
+class MeshIndex;
+
+template <class IndexTagT>
+std::istream&
+operator>>(std::istream& is, MeshIndex<IndexTagT>&);
+
+template <class IndexTagT>
+class MeshIndex
+: boost::totally_ordered<
+      MeshIndex<IndexTagT>,                                      // < > <= >= == !=
+      boost::unit_steppable<MeshIndex<IndexTagT>,                // ++ -- (pre and post)
+                            boost::additive<MeshIndex<IndexTagT> // += +
+                                                                 // -= -
+                                            >>> {
+
+public:
+  using Base = boost::totally_ordered<
+      MeshIndex<IndexTagT>,
+      boost::unit_steppable<MeshIndex<IndexTagT>,
+                            boost::additive<MeshIndex<IndexTagT>>>>;
+  using Self = MeshIndex<IndexTagT>;
+
+  /** \brief Constructor. Initializes with an invalid index. */
+  MeshIndex() : index_(-1) {}
+
+  /** \brief Constructor.
+   * \param[in] index The integer index.
+   */
+  explicit MeshIndex(const int index) : index_(index) {}
+
+  /** \brief Returns true if the index is valid. */
+  inline bool
+  isValid() const
   {
-    /** \brief Index used to access elements in the half-edge mesh. It is basically just a wrapper around an integer with a few added methods.
-      * \author Martin Saelzle
-      * \ingroup geometry
-      */
-    class VertexIndex
-        : boost::totally_ordered <pcl::geometry::VertexIndex // < > <= >= == !=
-        , boost::unit_steppable  <pcl::geometry::VertexIndex // ++ -- (pre and post)
-        , boost::additive        <pcl::geometry::VertexIndex // += + -= -
-        > > >
-    {
-      public:
-
-        using Base = boost::totally_ordered <pcl::geometry::VertexIndex,
-                     boost::unit_steppable  <pcl::geometry::VertexIndex,
-                     boost::additive        <pcl::geometry::VertexIndex> > >;
-        using Self = pcl::geometry::VertexIndex;
-
-        /** \brief Constructor. Initializes with an invalid index. */
-        VertexIndex ()
-          : index_ (-1)
-        {
-        }
-
-        /** \brief Constructor.
-          * \param[in] index The integer index.
-          */
-        explicit VertexIndex (const int index)
-          : index_ (index)
-        {
-        }
-
-        /** \brief Returns true if the index is valid. */
-        inline bool
-        isValid () const
-        {
-          return (index_ >= 0);
-        }
-
-        /** \brief Invalidate the index. */
-        inline void
-        invalidate ()
-        {
-          index_ = -1;
-        }
-
-        /** \brief Get the index. */
-        inline int
-        get () const
-        {
-          return (index_);
-        }
-
-        /** \brief Set the index. */
-        inline void
-        set (const int index)
-        {
-          index_ = index;
-        }
-
-        /** \brief Comparison operators (with boost::operators): < > <= >= */
-        inline bool
-        operator < (const Self& other) const
-        {
-          return (this->get () < other.get ());
-        }
-
-        /** \brief Comparison operators (with boost::operators): == != */
-        inline bool
-        operator == (const Self& other) const
-        {
-          return (this->get () == other.get ());
-        }
+    return (index_ >= 0);
+  }
 
-        /** \brief Increment operators (with boost::operators): ++ (pre and post) */
-        inline Self&
-        operator ++ ()
-        {
-          ++index_;
-          return (*this);
-        }
-
-        /** \brief Decrement operators (with boost::operators): \-\- (pre and post) */
-        inline Self&
-        operator -- ()
-        {
-          --index_;
-          return (*this);
-        }
-
-        /** \brief Addition operators (with boost::operators): + += */
-        inline Self&
-        operator += (const Self& other)
-        {
-          index_ += other.get ();
-          return (*this);
-        }
-
-        /** \brief Subtraction operators (with boost::operators): - -= */
-        inline Self&
-        operator -= (const Self& other)
-        {
-          index_ -= other.get ();
-          return (*this);
-        }
-
-      private:
-
-        /** \brief Stored index. */
-        int index_;
-
-        friend std::istream&
-        operator >> (std::istream& is, pcl::geometry::VertexIndex& index);
-    };
-
-    /** \brief ostream operator. */
-    inline std::ostream&
-    operator << (std::ostream& os, const pcl::geometry::VertexIndex& index)
-    {
-      return (os << index.get ());
-    }
-
-    /** \brief istream operator. */
-    inline std::istream&
-    operator >> (std::istream& is, pcl::geometry::VertexIndex& index)
-    {
-      return (is >> index.index_);
-    }
-
-  } // End namespace geometry
-} // End namespace pcl
-
-////////////////////////////////////////////////////////////////////////////////
-// HalfEdgeIndex
-////////////////////////////////////////////////////////////////////////////////
-
-namespace pcl
-{
-  namespace geometry
+  /** \brief Invalidate the index. */
+  inline void
+  invalidate()
   {
-    /** \brief Index used to access elements in the half-edge mesh. It is basically just a wrapper around an integer with a few added methods.
-      * \author Martin Saelzle
-      * \ingroup geometry
-      */
-    class HalfEdgeIndex
-        : boost::totally_ordered <pcl::geometry::HalfEdgeIndex // < > <= >= == !=
-        , boost::unit_steppable  <pcl::geometry::HalfEdgeIndex // ++ -- (pre and post)
-        , boost::additive        <pcl::geometry::HalfEdgeIndex // += + -= -
-        > > >
-    {
-      public:
-
-        using Base = boost::totally_ordered <pcl::geometry::HalfEdgeIndex,
-                     boost::unit_steppable  <pcl::geometry::HalfEdgeIndex,
-                     boost::additive        <pcl::geometry::HalfEdgeIndex> > >;
-        using Self = pcl::geometry::HalfEdgeIndex;
-
-        /** \brief Constructor. Initializes with an invalid index. */
-        HalfEdgeIndex ()
-          : index_ (-1)
-        {
-        }
-
-        /** \brief Constructor.
-          * \param[in] index The integer index.
-          */
-        explicit HalfEdgeIndex (const int index)
-          : index_ (index)
-        {
-        }
-
-        /** \brief Returns true if the index is valid. */
-        inline bool
-        isValid () const
-        {
-          return (index_ >= 0);
-        }
-
-        /** \brief Invalidate the index. */
-        inline void
-        invalidate ()
-        {
-          index_ = -1;
-        }
-
-        /** \brief Get the index. */
-        inline int
-        get () const
-        {
-          return (index_);
-        }
-
-        /** \brief Set the index. */
-        inline void
-        set (const int index)
-        {
-          index_ = index;
-        }
-
-        /** \brief Comparison operators (with boost::operators): < > <= >= */
-        inline bool
-        operator < (const Self& other) const
-        {
-          return (this->get () < other.get ());
-        }
-
-        /** \brief Comparison operators (with boost::operators): == != */
-        inline bool
-        operator == (const Self& other) const
-        {
-          return (this->get () == other.get ());
-        }
-
-        /** \brief Increment operators (with boost::operators): ++ (pre and post) */
-        inline Self&
-        operator ++ ()
-        {
-          ++index_;
-          return (*this);
-        }
-
-        /** \brief Decrement operators (with boost::operators): \-\- (pre and post) */
-        inline Self&
-        operator -- ()
-        {
-          --index_;
-          return (*this);
-        }
-
-        /** \brief Addition operators (with boost::operators): + += */
-        inline Self&
-        operator += (const Self& other)
-        {
-          index_ += other.get ();
-          return (*this);
-        }
-
-        /** \brief Subtraction operators (with boost::operators): - -= */
-        inline Self&
-        operator -= (const Self& other)
-        {
-          index_ -= other.get ();
-          return (*this);
-        }
-
-      private:
-
-        /** \brief Stored index. */
-        int index_;
-
-        friend std::istream&
-        operator >> (std::istream& is, pcl::geometry::HalfEdgeIndex& index);
-    };
-
-    /** \brief ostream operator. */
-    inline std::ostream&
-    operator << (std::ostream& os, const pcl::geometry::HalfEdgeIndex& index)
-    {
-      return (os << index.get ());
-    }
-
-    /** \brief istream operator. */
-    inline std::istream&
-    operator >> (std::istream& is, pcl::geometry::HalfEdgeIndex& index)
-    {
-      return (is >> index.index_);
-    }
-
-  } // End namespace geometry
-} // End namespace pcl
-
-////////////////////////////////////////////////////////////////////////////////
-// EdgeIndex
-////////////////////////////////////////////////////////////////////////////////
+    index_ = -1;
+  }
 
-namespace pcl
-{
-  namespace geometry
+  /** \brief Get the index. */
+  inline int
+  get() const
   {
-    /** \brief Index used to access elements in the half-edge mesh. It is basically just a wrapper around an integer with a few added methods.
-      * \author Martin Saelzle
-      * \ingroup geometry
-      */
-    class EdgeIndex
-        : boost::totally_ordered <pcl::geometry::EdgeIndex // < > <= >= == !=
-        , boost::unit_steppable  <pcl::geometry::EdgeIndex // ++ -- (pre and post)
-        , boost::additive        <pcl::geometry::EdgeIndex // += + -= -
-        > > >
-    {
-      public:
-
-        using Base = boost::totally_ordered <pcl::geometry::EdgeIndex,
-                     boost::unit_steppable  <pcl::geometry::EdgeIndex,
-                     boost::additive        <pcl::geometry::EdgeIndex> > >;
-        using Self = pcl::geometry::EdgeIndex;
-
-        /** \brief Constructor. Initializes with an invalid index. */
-        EdgeIndex ()
-          : index_ (-1)
-        {
-        }
-
-        /** \brief Constructor.
-          * \param[in] index The integer index.
-          */
-        explicit EdgeIndex (const int index)
-          : index_ (index)
-        {
-        }
-
-        /** \brief Returns true if the index is valid. */
-        inline bool
-        isValid () const
-        {
-          return (index_ >= 0);
-        }
-
-        /** \brief Invalidate the index. */
-        inline void
-        invalidate ()
-        {
-          index_ = -1;
-        }
-
-        /** \brief Get the index. */
-        inline int
-        get () const
-        {
-          return (index_);
-        }
-
-        /** \brief Set the index. */
-        inline void
-        set (const int index)
-        {
-          index_ = index;
-        }
-
-        /** \brief Comparison operators (with boost::operators): < > <= >= */
-        inline bool
-        operator < (const Self& other) const
-        {
-          return (this->get () < other.get ());
-        }
-
-        /** \brief Comparison operators (with boost::operators): == != */
-        inline bool
-        operator == (const Self& other) const
-        {
-          return (this->get () == other.get ());
-        }
-
-        /** \brief Increment operators (with boost::operators): ++ (pre and post) */
-        inline Self&
-        operator ++ ()
-        {
-          ++index_;
-          return (*this);
-        }
+    return (index_);
+  }
 
-        /** \brief Decrement operators (with boost::operators): \-\- (pre and post) */
-        inline Self&
-        operator -- ()
-        {
-          --index_;
-          return (*this);
-        }
-
-        /** \brief Addition operators (with boost::operators): + += */
-        inline Self&
-        operator += (const Self& other)
-        {
-          index_ += other.get ();
-          return (*this);
-        }
-
-        /** \brief Subtraction operators (with boost::operators): - -= */
-        inline Self&
-        operator -= (const Self& other)
-        {
-          index_ -= other.get ();
-          return (*this);
-        }
-
-      private:
-
-        /** \brief Stored index. */
-        int index_;
-
-        friend std::istream&
-        operator >> (std::istream& is, pcl::geometry::EdgeIndex& index);
-    };
-
-    /** \brief ostream operator. */
-    inline std::ostream&
-    operator << (std::ostream& os, const pcl::geometry::EdgeIndex& index)
-    {
-      return (os << index.get ());
-    }
-
-    /** \brief istream operator. */
-    inline std::istream&
-    operator >> (std::istream& is, pcl::geometry::EdgeIndex& index)
-    {
-      return (is >> index.index_);
-    }
-
-  } // End namespace geometry
-} // End namespace pcl
-
-////////////////////////////////////////////////////////////////////////////////
-// FaceIndex
-////////////////////////////////////////////////////////////////////////////////
-
-namespace pcl
-{
-  namespace geometry
+  /** \brief Set the index. */
+  inline void
+  set(const int index)
   {
-    /** \brief Index used to access elements in the half-edge mesh. It is basically just a wrapper around an integer with a few added methods.
-      * \author Martin Saelzle
-      * \ingroup geometry
-      */
-    class FaceIndex
-        : boost::totally_ordered <pcl::geometry::FaceIndex // < > <= >= == !=
-        , boost::unit_steppable  <pcl::geometry::FaceIndex // ++ -- (pre and post)
-        , boost::additive        <pcl::geometry::FaceIndex // += + -= -
-        > > >
-    {
-      public:
-
-        using Base = boost::totally_ordered <pcl::geometry::FaceIndex,
-                     boost::unit_steppable  <pcl::geometry::FaceIndex,
-                     boost::additive        <pcl::geometry::FaceIndex> > >;
-        using Self = pcl::geometry::FaceIndex;
-
-        /** \brief Constructor. Initializes with an invalid index. */
-        FaceIndex ()
-          : index_ (-1)
-        {
-        }
-
-        /** \brief Constructor.
-          * \param[in] index The integer index.
-          */
-        explicit FaceIndex (const int index)
-          : index_ (index)
-        {
-        }
-
-        /** \brief Returns true if the index is valid. */
-        inline bool
-        isValid () const
-        {
-          return (index_ >= 0);
-        }
+    index_ = index;
+  }
 
-        /** \brief Invalidate the index. */
-        inline void
-        invalidate ()
-        {
-          index_ = -1;
-        }
-
-        /** \brief Get the index. */
-        inline int
-        get () const
-        {
-          return (index_);
-        }
-
-        /** \brief Set the index. */
-        inline void
-        set (const int index)
-        {
-          index_ = index;
-        }
+  /** \brief Comparison operators (with boost::operators): < > <= >= */
+  inline bool
+  operator<(const Self& other) const
+  {
+    return (this->get() < other.get());
+  }
 
-        /** \brief Comparison operators (with boost::operators): < > <= >= */
-        inline bool
-        operator < (const Self& other) const
-        {
-          return (this->get () < other.get ());
-        }
+  /** \brief Comparison operators (with boost::operators): == != */
+  inline bool
+  operator==(const Self& other) const
+  {
+    return (this->get() == other.get());
+  }
 
-        /** \brief Comparison operators (with boost::operators): == != */
-        inline bool
-        operator == (const Self& other) const
-        {
-          return (this->get () == other.get ());
-        }
+  /** \brief Increment operators (with boost::operators): ++ (pre and post) */
+  inline Self&
+  operator++()
+  {
+    ++index_;
+    return (*this);
+  }
 
-        /** \brief Increment operators (with boost::operators): ++ (pre and post) */
-        inline Self&
-        operator ++ ()
-        {
-          ++index_;
-          return (*this);
-        }
+  /** \brief Decrement operators (with boost::operators): \-\- (pre and post) */
+  inline Self&
+  operator--()
+  {
+    --index_;
+    return (*this);
+  }
 
-        /** \brief Decrement operators (with boost::operators): \-\- (pre and post) */
-        inline Self&
-        operator -- ()
-        {
-          --index_;
-          return (*this);
-        }
+  /** \brief Addition operators (with boost::operators): + += */
+  inline Self&
+  operator+=(const Self& other)
+  {
+    index_ += other.get();
+    return (*this);
+  }
 
-        /** \brief Addition operators (with boost::operators): + += */
-        inline Self&
-        operator += (const Self& other)
-        {
-          index_ += other.get ();
-          return (*this);
-        }
+  /** \brief Subtraction operators (with boost::operators): - -= */
+  inline Self&
+  operator-=(const Self& other)
+  {
+    index_ -= other.get();
+    return (*this);
+  }
 
-        /** \brief Subtraction operators (with boost::operators): - -= */
-        inline Self&
-        operator -= (const Self& other)
-        {
-          index_ -= other.get ();
-          return (*this);
-        }
+private:
+  /** \brief Stored index. */
+  int index_;
 
-      private:
+  friend std::istream& operator>><>(std::istream& is, MeshIndex<IndexTagT>& index);
+};
 
-        /** \brief Stored index. */
-        int index_;
+/** \brief ostream operator. */
+template <class IndexTagT>
+inline std::ostream&
+operator<<(std::ostream& os, const MeshIndex<IndexTagT>& index)
+{
+  return (os << index.get());
+}
 
-        friend std::istream&
-        operator >> (std::istream& is, pcl::geometry::FaceIndex& index);
-    };
+/** \brief istream operator. */
+template <class IndexTagT>
+inline std::istream&
+operator>>(std::istream& is, MeshIndex<IndexTagT>& index)
+{
+  return (is >> index.index_);
+}
 
-    /** \brief ostream operator. */
-    inline std::ostream&
-    operator << (std::ostream& os, const pcl::geometry::FaceIndex& index)
-    {
-      return (os << index.get ());
-    }
+} // End namespace detail
+} // End namespace pcl
 
-    /** \brief istream operator. */
-    inline std::istream&
-    operator >> (std::istream& is, pcl::geometry::FaceIndex& index)
-    {
-      return (is >> index.index_);
-    }
+namespace pcl {
+namespace geometry {
+/** \brief Index used to access elements in the half-edge mesh. It is basically just a
+ * wrapper around an integer with a few added methods.
+ * \author Martin Saelzle
+ * \ingroup geometry
+ */
+using VertexIndex = pcl::detail::MeshIndex<struct VertexIndexTag>;
+/** \brief Index used to access elements in the half-edge mesh. It is basically just a
+ * wrapper around an integer with a few added methods.
+ * \author Martin Saelzle
+ * \ingroup geometry
+ */
+using HalfEdgeIndex = pcl::detail::MeshIndex<struct HalfEdgeIndexTag>;
+/** \brief Index used to access elements in the half-edge mesh. It is basically just a
+ * wrapper around an integer with a few added methods.
+ * \author Martin Saelzle
+ * \ingroup geometry
+ */
+using EdgeIndex = pcl::detail::MeshIndex<struct EdgeIndexTag>;
+/** \brief Index used to access elements in the half-edge mesh. It is basically just a
+ * wrapper around an integer with a few added methods.
+ * \author Martin Saelzle
+ * \ingroup geometry
+ */
+using FaceIndex = pcl::detail::MeshIndex<struct FaceIndexTag>;
 
-  } // End namespace geometry
+} // End namespace geometry
 } // End namespace pcl
 
 ////////////////////////////////////////////////////////////////////////////////
 // Conversions
 ////////////////////////////////////////////////////////////////////////////////
 
-namespace pcl
+namespace pcl {
+namespace geometry {
+/** \brief Convert the given half-edge index to an edge index. */
+inline EdgeIndex
+toEdgeIndex(const HalfEdgeIndex& index)
 {
-  namespace geometry
-  {
-    /** \brief Convert the given half-edge index to an edge index. */
-    inline pcl::geometry::EdgeIndex
-    toEdgeIndex (const HalfEdgeIndex& index)
-    {
-      return (index.isValid () ? EdgeIndex (index.get () / 2) : EdgeIndex ());
-    }
+  return (index.isValid() ? EdgeIndex(index.get() / 2) : EdgeIndex());
+}
 
-    /** \brief Convert the given edge index to a half-edge index.
-      * \param index
-      * \param[in] get_first The first half-edge of the edge is returned if this variable is true; elsewise the second.
-      */
-    inline pcl::geometry::HalfEdgeIndex
-    toHalfEdgeIndex (const EdgeIndex& index, const bool get_first=true)
-    {
-      return (index.isValid () ? HalfEdgeIndex (index.get () * 2 + static_cast <int> (!get_first)) : HalfEdgeIndex ());
-    }
-  } // End namespace geometry
+/** \brief Convert the given edge index to a half-edge index.
+ * \param index
+ * \param[in] get_first The first half-edge of the edge is returned if this
+ * variable is true; elsewise the second.
+ */
+inline HalfEdgeIndex
+toHalfEdgeIndex(const EdgeIndex& index, const bool get_first = true)
+{
+  return (index.isValid()
+              ? HalfEdgeIndex(index.get() * 2 + static_cast<int>(!get_first))
+              : HalfEdgeIndex());
+}
+} // End namespace geometry
 } // End namespace pcl
diff --git a/geometry/include/pcl/geometry/mesh_indices.py b/geometry/include/pcl/geometry/mesh_indices.py
deleted file mode 100644
index 70eb3952..00000000
--- a/geometry/include/pcl/geometry/mesh_indices.py
+++ /dev/null
@@ -1,265 +0,0 @@
-##
-#  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
-#     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 os
-
-filename    = os.path.join (os.path.dirname (__file__), 'mesh_indices.h')
-class_names = ['VertexIndex', 'HalfEdgeIndex', 'EdgeIndex', 'FaceIndex']
-
-################################################################################
-
-f = open (filename, 'w')
-
-f.write ('/*\n')
-f.write (' * Software License Agreement (BSD License)\n')
-f.write (' *\n')
-f.write (' * Point Cloud Library (PCL) - www.pointclouds.org\n')
-f.write (' * Copyright (c) 2009-2012, Willow Garage, Inc.\n')
-f.write (' * Copyright (c) 2012-, Open Perception, Inc.\n')
-f.write (' *\n')
-f.write (' * All rights reserved.\n')
-f.write (' *\n')
-f.write (' * Redistribution and use in source and binary forms, with or without\n')
-f.write (' * modification, are permitted provided that the following conditions\n')
-f.write (' * are met:\n')
-f.write (' *\n')
-f.write (' *  * Redistributions of source code must retain the above copyright\n')
-f.write (' *    notice, this list of conditions and the following disclaimer.\n')
-f.write (' *  * Redistributions in binary form must reproduce the above\n')
-f.write (' *    copyright notice, this list of conditions and the following\n')
-f.write (' *    disclaimer in the documentation and/or other materials provided\n')
-f.write (' *    with the distribution.\n')
-f.write (' *  * Neither the name of the copyright holder(s) nor the names of its\n')
-f.write (' *    contributors may be used to endorse or promote products derived\n')
-f.write (' *    from this software without specific prior written permission.\n')
-f.write (' *\n')
-f.write (' * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n')
-f.write (' * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n')
-f.write (' * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n')
-f.write (' * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n')
-f.write (' * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n')
-f.write (' * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n')
-f.write (' * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n')
-f.write (' * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n')
-f.write (' * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n')
-f.write (' * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n')
-f.write (' * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n')
-f.write (' * POSSIBILITY OF SUCH DAMAGE.\n')
-f.write (' *\n')
-f.write (' * $Id$\n')
-f.write (' *\n')
-f.write (' */\n\n')
-
-f.write ("// NOTE: This file has been created with 'pcl_src/geometry/include/pcl/geometry/mesh_indices.py'\n\n")
-
-f.write ('#ifndef PCL_GEOMETRY_MESH_INDICES_H\n')
-f.write ('#define PCL_GEOMETRY_MESH_INDICES_H\n\n')
-
-f.write ('#include <iostream>\n\n')
-
-f.write ('#include <pcl/geometry/boost.h>\n\n')
-
-for cn in class_names:
-
-    f.write ('////////////////////////////////////////////////////////////////////////////////\n')
-    f.write ('// ' + cn + '\n')
-    f.write ('////////////////////////////////////////////////////////////////////////////////\n\n')
-
-    f.write ('namespace pcl\n')
-    f.write ('{\n')
-    f.write ('  namespace geometry\n')
-    f.write ('  {\n')
-    f.write ('    /** \\brief Index used to access elements in the half-edge mesh. It is basically just a wrapper around an integer with a few added methods.\n')
-    f.write ('      * \\author Martin Saelzle\n')
-    f.write ('      * \ingroup geometry\n')
-    f.write ('      */\n')
-    f.write ('    class ' + cn + '\n')
-    f.write ('        : boost::totally_ordered <pcl::geometry::' + cn + ' // < > <= >= == !=\n')
-    f.write ('        , boost::unit_steppable  <pcl::geometry::' + cn + ' // ++ -- (pre and post)\n')
-    f.write ('        , boost::additive        <pcl::geometry::' + cn + ' // += + -= -\n')
-    f.write ('        > > >\n')
-    f.write ('    {\n')
-    f.write ('      public:\n\n')
-
-    f.write ('        typedef boost::totally_ordered <pcl::geometry::' + cn + ',\n')
-    f.write ('                boost::unit_steppable  <pcl::geometry::' + cn + ',\n')
-    f.write ('                boost::additive        <pcl::geometry::' + cn + '> > > Base;\n')
-    f.write ('        typedef pcl::geometry::' + cn + '                              Self;\n\n')
-
-    f.write ('        /** \\brief Constructor. Initializes with an invalid index. */\n')
-    f.write ('        ' + cn + ' ()\n')
-    f.write ('          : index_ (-1)\n')
-    f.write ('        {\n')
-    f.write ('        }\n\n')
-
-    f.write ('        /** \\brief Constructor.\n')
-    f.write ('          * \param[in] index The integer index.\n')
-    f.write ('          */\n')
-    f.write ('        explicit ' + cn + ' (const int index)\n')
-    f.write ('          : index_ (index)\n')
-    f.write ('        {\n')
-    f.write ('        }\n\n')
-
-    f.write ('        /** \\brief Returns true if the index is valid. */\n')
-    f.write ('        inline bool\n')
-    f.write ('        isValid () const\n')
-    f.write ('        {\n')
-    f.write ('          return (index_ >= 0);\n')
-    f.write ('        }\n\n')
-
-    f.write ('        /** \\brief Invalidate the index. */\n')
-    f.write ('        inline void\n')
-    f.write ('        invalidate ()\n')
-    f.write ('        {\n')
-    f.write ('          index_ = -1;\n')
-    f.write ('        }\n\n')
-
-    f.write ('        /** \\brief Get the index. */\n')
-    f.write ('        inline int\n')
-    f.write ('        get () const\n')
-    f.write ('        {\n')
-    f.write ('          return (index_);\n')
-    f.write ('        }\n\n')
-
-    f.write ('        /** \\brief Set the index. */\n')
-    f.write ('        inline void\n')
-    f.write ('        set (const int index)\n')
-    f.write ('        {\n')
-    f.write ('          index_ = index;\n')
-    f.write ('        }\n\n')
-
-    f.write ('        /** \\brief Comparison operators (with boost::operators): < > <= >= */\n')
-    f.write ('        inline bool\n')
-    f.write ('        operator < (const Self& other) const\n')
-    f.write ('        {\n')
-    f.write ('          return (this->get () < other.get ());\n')
-    f.write ('        }\n\n')
-
-    f.write ('        /** \\brief Comparison operators (with boost::operators): == != */\n')
-    f.write ('        inline bool\n')
-    f.write ('        operator == (const Self& other) const\n')
-    f.write ('        {\n')
-    f.write ('          return (this->get () == other.get ());\n')
-    f.write ('        }\n\n')
-
-    f.write ('        /** \\brief Increment operators (with boost::operators): ++ (pre and post) */\n')
-    f.write ('        inline Self&\n')
-    f.write ('        operator ++ ()\n')
-    f.write ('        {\n')
-    f.write ('          ++index_;\n')
-    f.write ('          return (*this);\n')
-    f.write ('        }\n\n')
-
-    f.write ('        /** \\brief Decrement operators (with boost::operators): \-\- (pre and post) */\n')
-    f.write ('        inline Self&\n')
-    f.write ('        operator -- ()\n')
-    f.write ('        {\n')
-    f.write ('          --index_;\n')
-    f.write ('          return (*this);\n')
-    f.write ('        }\n\n')
-
-    f.write ('        /** \\brief Addition operators (with boost::operators): + += */\n')
-    f.write ('        inline Self&\n')
-    f.write ('        operator += (const Self& other)\n')
-    f.write ('        {\n')
-    f.write ('          index_ += other.get ();\n')
-    f.write ('          return (*this);\n')
-    f.write ('        }\n\n')
-
-    f.write ('        /** \\brief Subtraction operators (with boost::operators): - -= */\n')
-    f.write ('        inline Self&\n')
-    f.write ('        operator -= (const Self& other)\n')
-    f.write ('        {\n')
-    f.write ('          index_ -= other.get ();\n')
-    f.write ('          return (*this);\n')
-    f.write ('        }\n\n')
-
-    f.write ('      private:\n\n')
-
-    f.write ('        /** \\brief Stored index. */\n')
-    f.write ('        int index_;\n\n')
-
-    f.write ('        friend std::istream&\n')
-    f.write ('        operator >> (std::istream& is, pcl::geometry::' + cn + '& index);\n')
-    f.write ('    };\n\n')
-
-    f.write ('    /** \\brief ostream operator. */\n')
-    f.write ('    inline std::ostream&\n')
-    f.write ('    operator << (std::ostream& os, const pcl::geometry::' + cn + '& index)\n')
-    f.write ('    {\n')
-    f.write ('      return (os << index.get ());\n')
-    f.write ('    }\n\n')
-
-    f.write ('    /** \\brief istream operator. */\n')
-    f.write ('    inline std::istream&\n')
-    f.write ('    operator >> (std::istream& is, pcl::geometry::' + cn + '& index)\n')
-    f.write ('    {\n')
-    f.write ('      return (is >> index.index_);\n')
-    f.write ('    }\n\n')
-
-    f.write ('  } // End namespace geometry\n')
-    f.write ('} // End namespace pcl\n\n')
-
-f.write ('////////////////////////////////////////////////////////////////////////////////\n')
-f.write ('// Conversions\n')
-f.write ('////////////////////////////////////////////////////////////////////////////////\n\n')
-
-f.write ('namespace pcl\n')
-f.write ('{\n')
-f.write ('  namespace geometry\n')
-f.write ('  {\n')
-f.write ('    /** \\brief Convert the given half-edge index to an edge index. */\n')
-f.write ('    inline pcl::geometry::EdgeIndex\n')
-f.write ('    toEdgeIndex (const HalfEdgeIndex& index)\n')
-f.write ('    {\n')
-f.write ('      return (index.isValid () ? EdgeIndex (index.get () / 2) : EdgeIndex ());\n')
-f.write ('    }\n\n')
-
-f.write ('    /** \\brief Convert the given edge index to a half-edge index.\n')
-f.write ('      * \\param[in] get_first The first half-edge of the edge is returned if this variable is true; elsewise the second.\n')
-f.write ('      */\n')
-f.write ('    inline pcl::geometry::HalfEdgeIndex\n')
-f.write ('    toHalfEdgeIndex (const EdgeIndex& index, const bool get_first=true)\n')
-f.write ('    {\n')
-f.write ('      return (index.isValid () ? HalfEdgeIndex (index.get () * 2 + static_cast <int> (!get_first)) : HalfEdgeIndex ());\n')
-f.write ('    }\n')
-f.write ('  } // End namespace geometry\n')
-f.write ('} // End namespace pcl\n\n')
-
-f.write ('#endif // PCL_GEOMETRY_MESH_INDICES_H\n')
-
-f.close()
diff --git a/geometry/include/pcl/geometry/mesh_io.h b/geometry/include/pcl/geometry/mesh_io.h
index 169b1fc3..838d7913 100644
--- a/geometry/include/pcl/geometry/mesh_io.h
+++ b/geometry/include/pcl/geometry/mesh_io.h
@@ -41,222 +41,218 @@
 #pragma once
 
 #include <fstream>
-#include <string>
-#include <sstream>
 #include <iostream>
+#include <sstream>
+#include <string>
 
-namespace pcl
-{
-  namespace geometry
+namespace pcl {
+namespace geometry {
+/** \brief Read / write the half-edge mesh from / to a file.
+ * \tparam MeshT e.g. pcl::geometry::TriangleMesh or pcl::geometry::PolygonMesh
+ * \author Martin Saelzle
+ * \ingroup geometry
+ * \todo
+ *  - Only writes the topology (not the mesh data).
+ *  - Supports only ascii.
+ *  - Does not consider the mesh traits (e.g. manifold or not)
+ */
+template <class MeshT>
+class MeshIO {
+public:
+  using Mesh = MeshT;
+
+  using Vertex = typename Mesh::Vertex;
+  using HalfEdge = typename Mesh::HalfEdge;
+  using Face = typename Mesh::Face;
+
+  using Vertices = typename Mesh::Vertices;
+  using HalfEdges = typename Mesh::HalfEdges;
+  using Faces = typename Mesh::Faces;
+
+  using VertexIndex = typename Mesh::VertexIndex;
+  using HalfEdgeIndex = typename Mesh::HalfEdgeIndex;
+  using FaceIndex = typename Mesh::FaceIndex;
+
+  /** \brief Constructor. */
+  MeshIO() {}
+
+  /** \brief Read the mesh from a file with the given filename.
+   * \param[in] filename Path to the file.
+   * \param[out] mesh The loaded mesh.
+   * \return true if success.
+   */
+  bool
+  read(const std::string& filename, Mesh& mesh) const
   {
-    /** \brief Read / write the half-edge mesh from / to a file.
-      * \tparam MeshT e.g. pcl::geometry::TriangleMesh or pcl::geometry::PolygonMesh
-      * \author Martin Saelzle
-      * \ingroup geometry
-      * \todo
-      *  - Only writes the topology (not the mesh data).
-      *  - Supports only ascii.
-      *  - Does not consider the mesh traits (e.g. manifold or not)
-      */
-    template <class MeshT>
-    class MeshIO
+    std::ifstream file(filename.c_str());
+
+    if (!file.is_open()) {
+      std::cerr << "Error in MeshIO::read: Could not open the file '" << filename
+                << "'\n";
+      return (false);
+    }
+
+    // Read the header
+    std::string line;
+    unsigned int line_number = 1;
+    int n_v = -1, n_he = -1, n_f = -1;
+
+    if (!std::getline(file, line) || line != "PCL half-edge mesh") {
+      std::cerr << "Error loading '" << filename << "' (line " << line_number
+                << "): Wrong file format.\n";
+      return (false);
+    }
+    ++line_number;
+
+    if (!std::getline(file, line)) {
+      std::cerr << "Error loading '" << filename << "'' (line " << line_number
+                << "): Number of vertices / half-edges / faces not found.\n";
+      return (false);
+    }
     {
-      public:
-
-        using Mesh = MeshT;
-
-        using Vertex = typename Mesh::Vertex;
-        using HalfEdge = typename Mesh::HalfEdge;
-        using Face = typename Mesh::Face;
-
-        using Vertices = typename Mesh::Vertices;
-        using HalfEdges = typename Mesh::HalfEdges;
-        using Faces = typename Mesh::Faces;
-
-        using VertexIndex = typename Mesh::VertexIndex;
-        using HalfEdgeIndex = typename Mesh::HalfEdgeIndex;
-        using FaceIndex = typename Mesh::FaceIndex;
-
-        /** \brief Constructor. */
-        MeshIO ()
-        {
+      std::istringstream iss(line);
+      if (!(iss >> n_v >> n_he >> n_f) || iss.good()) // Don't allow more than 3 en
+      {
+        std::cerr << "Error loading '" << filename << "'' (line " << line_number
+                  << "): Could not read the number of vertices / half-edges / faces.\n";
+        return (false);
+      }
+    }
+    if (n_v < 0 || n_he < 0 || n_f < 0) {
+      std::cerr << "Error loading '" << filename << "'' (line " << line_number
+                << "): Invalid number of vertices / half-edges / faces.\n";
+      return (false);
+    }
+    ++line_number;
+
+    // Read the vertices.
+    {
+      mesh.vertices_.reserve(n_v);
+      HalfEdgeIndex idx_ohe; // Outgoing half-edge;
+
+      for (int i = 0; i < n_v; ++i, ++line_number) {
+        if (!std::getline(file, line)) {
+          std::cerr << "Error loading '" << filename << "'' (line " << line_number
+                    << "): Could not read the line.\n";
+          return (false);
         }
 
-        /** \brief Read the mesh from a file with the given filename.
-          * \param[in] filename Path to the file.
-          * \param[out] mesh The loaded mesh.
-          * \return true if success.
-          */
-        bool
-        read (const std::string& filename, Mesh& mesh) const
-        {
-          std::ifstream file (filename.c_str ());
-
-          if (!file.is_open ())
-          {
-            std::cerr << "Error in MeshIO::read: Could not open the file '" << filename << "'\n";
-            return (false);
-          }
-
-          // Read the header
-          std::string line;
-          unsigned int line_number = 1;
-          int n_v = -1, n_he = -1, n_f = -1;
-
-          if (!std::getline (file, line) || line != "PCL half-edge mesh")
-          {
-            std::cerr << "Error loading '" << filename << "' (line " << line_number << "): Wrong file format.\n";
-            return (false);
-          }
-          ++line_number;
-
-          if (!std::getline (file, line))
-          {
-            std::cerr << "Error loading '" << filename << "'' (line " << line_number << "): Number of vertices / half-edges / faces not found.\n";
-            return (false);
-          }
-          {
-            std::istringstream iss (line);
-            if (!(iss >> n_v >> n_he >> n_f) || iss.good ()) // Don't allow more than 3 en
-            {
-              std::cerr << "Error loading '" << filename << "'' (line " << line_number << "): Could not read the number of vertices / half-edges / faces.\n";
-              return (false);
-            }
-          }
-          if (n_v < 0 || n_he < 0 || n_f < 0)
-          {
-            std::cerr << "Error loading '" << filename << "'' (line " << line_number << "): Invalid number of vertices / half-edges / faces.\n";
-            return (false);
-          }
-          ++line_number;
-
-          // Read the vertices.
-          {
-            mesh.vertices_.reserve (n_v);
-            HalfEdgeIndex idx_ohe; // Outgoing half-edge;
-
-            for (int i=0; i<n_v; ++i, ++line_number)
-            {
-              if (!std::getline (file, line))
-              {
-                std::cerr << "Error loading '" << filename << "'' (line " << line_number << "): Could not read the line.\n";
-                return (false);
-              }
-
-              std::istringstream iss (line);
-              if (!(iss >> idx_ohe) || iss.good ())
-              {
-                std::cerr << "Error loading '" << filename << "'' (line " << line_number << "): Could not read the vertex.\n";
-                return (false);
-              }
-              mesh.vertices_.push_back (Vertex (idx_ohe));
-            }
-          }
-
-          // Read the half-edges.
-          {
-            mesh.half_edges_.reserve (n_he);
-            VertexIndex   idx_tv;  // Terminating vertex.
-            HalfEdgeIndex idx_nhe; // Next half-edge;
-            HalfEdgeIndex idx_phe; // Previous half-edge.
-            FaceIndex     idx_f;   // Face.
-
-            for (int i=0; i<n_he; ++i, ++line_number)
-            {
-              if (!std::getline (file, line))
-              {
-                std::cerr << "Error loading '" << filename << "'' (line " << line_number << "): Could not read the line.\n";
-                return (false);
-              }
-
-              std::istringstream iss (line);
-              if (!(iss >> idx_tv >> idx_nhe >> idx_phe >> idx_f) || iss.good ())
-              {
-                std::cerr << "Error loading '" << filename << "'' (line " << line_number << "): Could not read the half-edge.\n";
-                return (false);
-              }
-              mesh.half_edges_.push_back (HalfEdge (idx_tv, idx_nhe, idx_phe, idx_f));
-            }
-          }
-
-          // Read the faces.
-          {
-            mesh.faces_.reserve (n_f);
-            HalfEdgeIndex idx_ihe; // Inner half-edge.
-
-            for (int i=0; i<n_f; ++i, ++line_number)
-            {
-              if (!std::getline (file, line))
-              {
-                std::cerr << "Error loading '" << filename << "'' (line " << line_number << "): Could not read the line.\n";
-                return (false);
-              }
-
-              std::istringstream iss (line);
-              if (!(iss >> idx_ihe) || iss.good ())
-              {
-                std::cerr << "Error loading '" << filename << "'' (line " << line_number << "): Could not read the face.\n";
-                return (false);
-              }
-              mesh.faces_.push_back (Face (idx_ihe));
-            }
-          }
-
-          // Set the data
-          if (Mesh::HasVertexData::value)   mesh.vertex_data_cloud_.   resize (n_v);
-          if (Mesh::HasHalfEdgeData::value) mesh.half_edge_data_cloud_.resize (n_he);
-          if (Mesh::HasEdgeData::value)     mesh.edge_data_cloud_.     resize (n_he / 2);
-          if (Mesh::HasFaceData::value)     mesh.face_data_cloud_.     resize (n_f);
-
-          return (true);
+        std::istringstream iss(line);
+        if (!(iss >> idx_ohe) || iss.good()) {
+          std::cerr << "Error loading '" << filename << "'' (line " << line_number
+                    << "): Could not read the vertex.\n";
+          return (false);
         }
+        mesh.vertices_.push_back(Vertex(idx_ohe));
+      }
+    }
 
-        /** \brief Write the mesh to a file with the given filename.
-          * \param[in] filename Path to the file.
-          * \param[in] mesh The saved mesh.
-          * \return true if success
-          */
-        bool
-        write (const std::string& filename, const Mesh& mesh) const
-        {
-          std::ofstream file (filename.c_str ());
-
-          // Write the header
-          if (!file.is_open ())
-          {
-            std::cerr << "Error in MeshIO::write: Could not open the file '" << filename << "'\n";
-            return (false);
-          }
-
-          file << "PCL half-edge mesh\n";
-          file << mesh.sizeVertices ()  << " "
-               << mesh.sizeHalfEdges () << " "
-               << mesh.sizeFaces ()     << "\n";
-
-          // Write the vertices
-          for (typename Vertices::const_iterator it=mesh.vertices_.begin (); it!=mesh.vertices_.end (); ++it)
-          {
-            file << it->idx_outgoing_half_edge_ << "\n";
-          }
-
-          // Write the half-edges
-          for (typename HalfEdges::const_iterator it=mesh.half_edges_.begin (); it!=mesh.half_edges_.end (); ++it)
-          {
-            file << it->idx_terminating_vertex_ << " "
-                 << it->idx_next_half_edge_     << " "
-                 << it->idx_prev_half_edge_     << " "
-                 << it->idx_face_               << "\n";
-          }
+    // Read the half-edges.
+    {
+      mesh.half_edges_.reserve(n_he);
+      VertexIndex idx_tv;    // Terminating vertex.
+      HalfEdgeIndex idx_nhe; // Next half-edge;
+      HalfEdgeIndex idx_phe; // Previous half-edge.
+      FaceIndex idx_f;       // Face.
+
+      for (int i = 0; i < n_he; ++i, ++line_number) {
+        if (!std::getline(file, line)) {
+          std::cerr << "Error loading '" << filename << "'' (line " << line_number
+                    << "): Could not read the line.\n";
+          return (false);
+        }
 
-          // Write the faces
-          for (typename Faces::const_iterator it=mesh.faces_.begin (); it!=mesh.faces_.end (); ++it)
-          {
-            file << it->idx_inner_half_edge_ << "\n";
-          }
+        std::istringstream iss(line);
+        if (!(iss >> idx_tv >> idx_nhe >> idx_phe >> idx_f) || iss.good()) {
+          std::cerr << "Error loading '" << filename << "'' (line " << line_number
+                    << "): Could not read the half-edge.\n";
+          return (false);
+        }
+        mesh.half_edges_.push_back(HalfEdge(idx_tv, idx_nhe, idx_phe, idx_f));
+      }
+    }
 
-          return (true);
+    // Read the faces.
+    {
+      mesh.faces_.reserve(n_f);
+      HalfEdgeIndex idx_ihe; // Inner half-edge.
+
+      for (int i = 0; i < n_f; ++i, ++line_number) {
+        if (!std::getline(file, line)) {
+          std::cerr << "Error loading '" << filename << "'' (line " << line_number
+                    << "): Could not read the line.\n";
+          return (false);
         }
-    };
 
-  } // End namespace geometry
+        std::istringstream iss(line);
+        if (!(iss >> idx_ihe) || iss.good()) {
+          std::cerr << "Error loading '" << filename << "'' (line " << line_number
+                    << "): Could not read the face.\n";
+          return (false);
+        }
+        mesh.faces_.push_back(Face(idx_ihe));
+      }
+    }
+
+    // Set the data
+    if (Mesh::HasVertexData::value)
+      mesh.vertex_data_cloud_.resize(n_v);
+    if (Mesh::HasHalfEdgeData::value)
+      mesh.half_edge_data_cloud_.resize(n_he);
+    if (Mesh::HasEdgeData::value)
+      mesh.edge_data_cloud_.resize(n_he / 2);
+    if (Mesh::HasFaceData::value)
+      mesh.face_data_cloud_.resize(n_f);
+
+    return (true);
+  }
+
+  /** \brief Write the mesh to a file with the given filename.
+   * \param[in] filename Path to the file.
+   * \param[in] mesh The saved mesh.
+   * \return true if success
+   */
+  bool
+  write(const std::string& filename, const Mesh& mesh) const
+  {
+    std::ofstream file(filename.c_str());
+
+    // Write the header
+    if (!file.is_open()) {
+      std::cerr << "Error in MeshIO::write: Could not open the file '" << filename
+                << "'\n";
+      return (false);
+    }
+
+    file << "PCL half-edge mesh\n";
+    file << mesh.sizeVertices() << " " << mesh.sizeHalfEdges() << " "
+         << mesh.sizeFaces() << "\n";
+
+    // Write the vertices
+    for (typename Vertices::const_iterator it = mesh.vertices_.begin();
+         it != mesh.vertices_.end();
+         ++it) {
+      file << it->idx_outgoing_half_edge_ << "\n";
+    }
+
+    // Write the half-edges
+    for (typename HalfEdges::const_iterator it = mesh.half_edges_.begin();
+         it != mesh.half_edges_.end();
+         ++it) {
+      file << it->idx_terminating_vertex_ << " " << it->idx_next_half_edge_ << " "
+           << it->idx_prev_half_edge_ << " " << it->idx_face_ << "\n";
+    }
+
+    // Write the faces
+    for (typename Faces::const_iterator it = mesh.faces_.begin();
+         it != mesh.faces_.end();
+         ++it) {
+      file << it->idx_inner_half_edge_ << "\n";
+    }
+
+    return (true);
+  }
+};
+
+} // End namespace geometry
 } // End namespace pcl
diff --git a/geometry/include/pcl/geometry/mesh_traits.h b/geometry/include/pcl/geometry/mesh_traits.h
index d088d90e..e779b8a3 100644
--- a/geometry/include/pcl/geometry/mesh_traits.h
+++ b/geometry/include/pcl/geometry/mesh_traits.h
@@ -42,34 +42,32 @@
 
 #include <type_traits>
 
-namespace pcl
-{
-  namespace geometry
-  {
-    /** \brief No data is associated with the vertices / half-edges / edges / faces. */
-    struct NoData {};
+namespace pcl {
+namespace geometry {
+/** \brief No data is associated with the vertices / half-edges / edges / faces. */
+struct NoData {};
 
-    /** \brief The mesh traits are used to set up compile time settings for the mesh.
-      * \tparam VertexDataT   Data stored for each vertex. Defaults to pcl::NoData.
-      * \tparam HalfEdgeDataT Data stored for each half-edge. Defaults to pcl::NoData.
-      * \tparam EdgeDataT     Data stored for each edge. Defaults to pcl::NoData.
-      * \tparam FaceDataT     Data stored for each face. Defaults to pcl::NoData.
-      * \author Martin Saelzle
-      * \ingroup geometry
-      */
-    template <class VertexDataT   = pcl::geometry::NoData,
-              class HalfEdgeDataT = pcl::geometry::NoData,
-              class EdgeDataT     = pcl::geometry::NoData,
-              class FaceDataT     = pcl::geometry::NoData>
-    struct DefaultMeshTraits
-    {
-      using VertexData = VertexDataT;
-      using HalfEdgeData = HalfEdgeDataT;
-      using EdgeData = EdgeDataT;
-      using FaceData = FaceDataT;
+/** \brief The mesh traits are used to set up compile time settings for the mesh.
+ * \tparam VertexDataT   Data stored for each vertex. Defaults to pcl::NoData.
+ * \tparam HalfEdgeDataT Data stored for each half-edge. Defaults to pcl::NoData.
+ * \tparam EdgeDataT     Data stored for each edge. Defaults to pcl::NoData.
+ * \tparam FaceDataT     Data stored for each face. Defaults to pcl::NoData.
+ * \author Martin Saelzle
+ * \ingroup geometry
+ */
+template <class VertexDataT = pcl::geometry::NoData,
+          class HalfEdgeDataT = pcl::geometry::NoData,
+          class EdgeDataT = pcl::geometry::NoData,
+          class FaceDataT = pcl::geometry::NoData>
+struct DefaultMeshTraits {
+  using VertexData = VertexDataT;
+  using HalfEdgeData = HalfEdgeDataT;
+  using EdgeData = EdgeDataT;
+  using FaceData = FaceDataT;
 
-      /** \brief Specifies whether the mesh is manifold or not (only non-manifold vertices can be represented). */
-      using IsManifold = std::false_type;
-    };
-  } // End namespace geometry
+  /** \brief Specifies whether the mesh is manifold or not (only non-manifold vertices
+   * can be represented). */
+  using IsManifold = std::false_type;
+};
+} // End namespace geometry
 } // End namespace pcl
diff --git a/geometry/include/pcl/geometry/organized_index_iterator.h b/geometry/include/pcl/geometry/organized_index_iterator.h
index 2047bb3b..fa3fab1c 100644
--- a/geometry/include/pcl/geometry/organized_index_iterator.h
+++ b/geometry/include/pcl/geometry/organized_index_iterator.h
@@ -35,64 +35,73 @@
 
 #pragma once
 
-namespace pcl
-{
-/** \brief base class for iterators on 2-dimensional maps like images/organized clouds etc.
-  * \author Suat Gedikli <gedikli@willowgarage.com>
-  * \ingroup  geometry
-  */
-class OrganizedIndexIterator
-{
-  public:
-    /** \brief constructor
-     *  \param[in] width the width of the image/organized cloud
-     */
-    OrganizedIndexIterator (unsigned width);
-    
-    /** \brief virtual destructor*/
-    virtual ~OrganizedIndexIterator ();
-    
-    /** \brief go to next pixel/point in image/cloud*/
-    virtual void operator ++ () = 0;
-    
-    /** \brief go to next pixel/point in image/cloud*/
-    virtual void operator ++ (int);
-    
-    /** \brief returns the pixel/point index in the linearized memory of the image/cloud
-      * \return the pixel/point index in the linearized memory of the image/cloud 
-      */
-    unsigned operator* () const;
-    
-    /** \brief returns the pixel/point index in the linearized memory of the image/cloud 
-      * \return the pixel/point index in the linearized memory of the image/cloud 
-      */
-    virtual unsigned getIndex () const;
-
-    /** \brief returns the row index (y-coordinate) of the current pixel/point
-      * \return  the row index (y-coordinate) of the current pixel/point
-      */
-    virtual unsigned getRowIndex () const;
-
-    /** \brief returns the col index (x-coordinate) of the current pixel/point
-      * \return  the col index (x-coordinate) of the current pixel/point
-      */
-    virtual unsigned getColumnIndex () const;
-
-    /** \brief return whether the current visited pixel/point is valid or not.
-      * \return true if the current pixel/point is within the points to be iterated over, false otherwise
-      */
-    virtual bool isValid () const = 0;
-
-    /** \brief resets the iterator to the beginning of the line
-      */
-    virtual void reset () = 0;
-    
-  protected:
-    /** \brief the width of the image/cloud*/
-    unsigned width_;
-    
-    /** \brief the index of the current pixel/point*/
-    unsigned index_;
+namespace pcl {
+/** \brief base class for iterators on 2-dimensional maps like images/organized clouds
+ * etc.
+ * \author Suat Gedikli <gedikli@willowgarage.com>
+ * \ingroup  geometry
+ */
+class OrganizedIndexIterator {
+public:
+  /**
+   * \brief constructor
+   * \param[in] width the width of the image/organized cloud
+   */
+  OrganizedIndexIterator(unsigned width);
+
+  /** \brief virtual destructor*/
+  virtual ~OrganizedIndexIterator();
+
+  /** \brief go to next pixel/point in image/cloud*/
+  virtual void
+  operator++() = 0;
+
+  /** \brief go to next pixel/point in image/cloud*/
+  virtual void
+  operator++(int);
+
+  /** \brief returns the pixel/point index in the linearized memory of the image/cloud
+   * \return the pixel/point index in the linearized memory of the image/cloud
+   */
+  unsigned
+  operator*() const;
+
+  /** \brief returns the pixel/point index in the linearized memory of the image/cloud
+   * \return the pixel/point index in the linearized memory of the image/cloud
+   */
+  virtual unsigned
+  getIndex() const;
+
+  /** \brief returns the row index (y-coordinate) of the current pixel/point
+   * \return  the row index (y-coordinate) of the current pixel/point
+   */
+  virtual unsigned
+  getRowIndex() const;
+
+  /** \brief returns the col index (x-coordinate) of the current pixel/point
+   * \return  the col index (x-coordinate) of the current pixel/point
+   */
+  virtual unsigned
+  getColumnIndex() const;
+
+  /** \brief return whether the current visited pixel/point is valid or not.
+   * \return true if the current pixel/point is within the points to be iterated over,
+   * false otherwise
+   */
+  virtual bool
+  isValid() const = 0;
+
+  /** \brief resets the iterator to the beginning of the line
+   */
+  virtual void
+  reset() = 0;
+
+protected:
+  /** \brief the width of the image/cloud*/
+  unsigned width_;
+
+  /** \brief the index of the current pixel/point*/
+  unsigned index_;
 };
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -100,50 +109,46 @@ class OrganizedIndexIterator
 ////////////////////////////////////////////////////////////////////////////////
 
 ////////////////////////////////////////////////////////////////////////////////
-inline OrganizedIndexIterator::OrganizedIndexIterator (unsigned width)
-: width_ (width)
-, index_ (0)
-{  
-}
+inline OrganizedIndexIterator::OrganizedIndexIterator(unsigned width)
+: width_(width), index_(0)
+{}
 
 ////////////////////////////////////////////////////////////////////////////////
-inline OrganizedIndexIterator::~OrganizedIndexIterator ()
-{  
-}
+inline OrganizedIndexIterator::~OrganizedIndexIterator() {}
 
 ////////////////////////////////////////////////////////////////////////////////
 inline void
-OrganizedIndexIterator::operator++ (int)
+OrganizedIndexIterator::operator++(int)
 {
-  return operator ++();
+  return operator++();
 }
 
 ////////////////////////////////////////////////////////////////////////////////
 inline unsigned
-pcl::OrganizedIndexIterator::operator * () const
+pcl::OrganizedIndexIterator::operator*() const
 {
   return index_;
 }
 
 ////////////////////////////////////////////////////////////////////////////////
 inline unsigned
-pcl::OrganizedIndexIterator::getIndex () const
+pcl::OrganizedIndexIterator::getIndex() const
 {
   return index_;
 }
 
 ////////////////////////////////////////////////////////////////////////////////
-/** \brief default implementation. Should be overloaded 
+/** \brief default implementation. Should be overloaded
  */
 inline unsigned
-pcl::OrganizedIndexIterator::getRowIndex () const
+pcl::OrganizedIndexIterator::getRowIndex() const
 {
   return index_ / width_;
 }
 
 ////////////////////////////////////////////////////////////////////////////////
 inline unsigned
-pcl::OrganizedIndexIterator::getColumnIndex () const
+pcl::OrganizedIndexIterator::getColumnIndex() const
 {
   return index_ % width_;
 }
diff --git a/geometry/include/pcl/geometry/planar_polygon.h b/geometry/include/pcl/geometry/planar_polygon.h
index 8a419c88..e544799b 100644
--- a/geometry/include/pcl/geometry/planar_polygon.h
+++ b/geometry/include/pcl/geometry/planar_polygon.h
@@ -39,104 +39,100 @@
 
 #pragma once
 
-#include <pcl/common/eigen.h>
+#include <pcl/ModelCoefficients.h>
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
 #include <pcl/point_cloud.h>
-#include <pcl/ModelCoefficients.h>
 
-namespace pcl
-{
-  /** \brief PlanarPolygon represents a planar (2D) polygon, potentially in a 3D space.
-    * \author Alex Trevor 
-    */
-  template <typename PointT>
-  class PlanarPolygon
+namespace pcl {
+/** \brief PlanarPolygon represents a planar (2D) polygon, potentially in a 3D space.
+ * \author Alex Trevor
+ */
+template <typename PointT>
+class PlanarPolygon {
+public:
+  using Ptr = shared_ptr<PlanarPolygon<PointT>>;
+  using ConstPtr = shared_ptr<const PlanarPolygon<PointT>>;
+
+  /** \brief Empty constructor for PlanarPolygon */
+  PlanarPolygon() : contour_() {}
+
+  /** \brief Constructor for PlanarPolygon
+   * \param[in] contour a vector of points bounding the polygon
+   * \param[in] coefficients a vector of the plane's coefficients (a,b,c,d)
+   */
+  PlanarPolygon(typename pcl::PointCloud<PointT>::VectorType& contour,
+                Eigen::Vector4f& coefficients)
+  : contour_(contour), coefficients_(coefficients)
+  {}
+
+  /** \brief Destructor. */
+  virtual ~PlanarPolygon() {}
+
+  /** \brief Set the internal contour
+   * \param[in] contour the new planar polygonal contour
+   */
+  void
+  setContour(const pcl::PointCloud<PointT>& contour)
+  {
+    contour_ = contour.points;
+  }
+
+  /** \brief Getter for the contour / boundary */
+  typename pcl::PointCloud<PointT>::VectorType&
+  getContour()
+  {
+    return (contour_);
+  }
+
+  /** \brief Getter for the contour / boundary */
+  const typename pcl::PointCloud<PointT>::VectorType&
+  getContour() const
   {
-    public:
-      using Ptr = shared_ptr<PlanarPolygon<PointT> >;
-      using ConstPtr = shared_ptr<const PlanarPolygon<PointT> >;
-
-       /** \brief Empty constructor for PlanarPolygon */
-      PlanarPolygon () : contour_ ()
-      {}
-      
-      /** \brief Constructor for PlanarPolygon
-        * \param[in] contour a vector of points bounding the polygon
-        * \param[in] coefficients a vector of the plane's coefficients (a,b,c,d)
-        */
-      PlanarPolygon (typename pcl::PointCloud<PointT>::VectorType &contour,
-                     Eigen::Vector4f& coefficients) 
-        : contour_ (contour), coefficients_ (coefficients)
-      {}
-      
-      /** \brief Destructor. */
-      virtual ~PlanarPolygon () {}
-
-      /** \brief Set the internal contour
-        * \param[in] contour the new planar polygonal contour
-        */
-      void
-      setContour (const pcl::PointCloud<PointT> &contour)
-      {
-        contour_ = contour.points;
-      }
-
-      /** \brief Getter for the contour / boundary */
-      typename pcl::PointCloud<PointT>::VectorType&
-      getContour ()
-      {
-        return (contour_);
-      }
-      
-      /** \brief Getter for the contour / boundary */
-      const typename pcl::PointCloud<PointT>::VectorType&
-      getContour () const
-      {
-        return (contour_);
-      }
-
-      /** \brief Setr the internal coefficients
-        * \param[in] coefficients the new coefficients to be set 
-        */
-      void
-      setCoefficients (const Eigen::Vector4f &coefficients)
-      {
-        coefficients_ = coefficients;
-      }
-
-      /** \brief Set the internal coefficients
-        * \param[in] coefficients the new coefficients to be set 
-        */
-      void
-      setCoefficients (const pcl::ModelCoefficients &coefficients)
-      {
-        for (int i = 0; i < 4; i++)
-          coefficients_[i] = coefficients.values.at (i);
-      }
-
-      /** \brief Getter for the coefficients */
-      Eigen::Vector4f&
-      getCoefficients ()
-      {
-        return (coefficients_);
-      }
-
-      /** \brief Getter for the coefficients */
-      const Eigen::Vector4f&
-      getCoefficients () const
-      {
-        return (coefficients_);
-      }
-      
-    protected:
-      /** \brief A list of points on the boundary/contour of the planar region. */
-      typename pcl::PointCloud<PointT>::VectorType contour_;
-      
-      /** \brief A list of model coefficients (a,b,c,d). */
-      Eigen::Vector4f coefficients_;
-    
-    public:
-      PCL_MAKE_ALIGNED_OPERATOR_NEW
-  };
-}
+    return (contour_);
+  }
+
+  /** \brief Setr the internal coefficients
+   * \param[in] coefficients the new coefficients to be set
+   */
+  void
+  setCoefficients(const Eigen::Vector4f& coefficients)
+  {
+    coefficients_ = coefficients;
+  }
+
+  /** \brief Set the internal coefficients
+   * \param[in] coefficients the new coefficients to be set
+   */
+  void
+  setCoefficients(const pcl::ModelCoefficients& coefficients)
+  {
+    for (int i = 0; i < 4; i++)
+      coefficients_[i] = coefficients.values.at(i);
+  }
+
+  /** \brief Getter for the coefficients */
+  Eigen::Vector4f&
+  getCoefficients()
+  {
+    return (coefficients_);
+  }
+
+  /** \brief Getter for the coefficients */
+  const Eigen::Vector4f&
+  getCoefficients() const
+  {
+    return (coefficients_);
+  }
+
+protected:
+  /** \brief A list of points on the boundary/contour of the planar region. */
+  typename pcl::PointCloud<PointT>::VectorType contour_;
+
+  /** \brief A list of model coefficients (a,b,c,d). */
+  Eigen::Vector4f coefficients_;
+
+public:
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+};
+} // namespace pcl
diff --git a/geometry/include/pcl/geometry/polygon_mesh.h b/geometry/include/pcl/geometry/polygon_mesh.h
index 027e9264..f1e5e939 100644
--- a/geometry/include/pcl/geometry/polygon_mesh.h
+++ b/geometry/include/pcl/geometry/polygon_mesh.h
@@ -40,162 +40,170 @@
 
 #pragma once
 
+#include <pcl/geometry/mesh_base.h>
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
-#include <pcl/geometry/mesh_base.h>
 
-namespace pcl
-{
-  namespace geometry
+namespace pcl {
+namespace geometry {
+/** \brief Tag describing the type of the mesh. */
+struct PolygonMeshTag {};
+
+/** \brief General half-edge mesh that can store any polygon with a minimum number of
+ * vertices of 3.
+ * \tparam MeshTraitsT Please have a look at
+ * pcl::geometry::DefaultMeshTraits.
+ * \author Martin Saelzle
+ * \ingroup geometry
+ */
+template <class MeshTraitsT>
+class PolygonMesh : public pcl::geometry::MeshBase<PolygonMesh<MeshTraitsT>,
+                                                   MeshTraitsT,
+                                                   PolygonMeshTag> {
+public:
+  using Base =
+      pcl::geometry::MeshBase<PolygonMesh<MeshTraitsT>, MeshTraitsT, PolygonMeshTag>;
+
+  using Self = PolygonMesh<MeshTraitsT>;
+  using Ptr = shared_ptr<Self>;
+  using ConstPtr = shared_ptr<const Self>;
+
+  using VertexData = typename Base::VertexData;
+  using HalfEdgeData = typename Base::HalfEdgeData;
+  using EdgeData = typename Base::EdgeData;
+  using FaceData = typename Base::FaceData;
+  using IsManifold = typename Base::IsManifold;
+  using MeshTag = typename Base::MeshTag;
+
+  using HasVertexData = typename Base::HasVertexData;
+  using HasHalfEdgeData = typename Base::HasHalfEdgeData;
+  using HasEdgeData = typename Base::HasEdgeData;
+  using HasFaceData = typename Base::HasFaceData;
+
+  using VertexDataCloud = typename Base::VertexDataCloud;
+  using HalfEdgeDataCloud = typename Base::HalfEdgeDataCloud;
+  using EdgeDataCloud = typename Base::EdgeDataCloud;
+  using FaceDataCloud = typename Base::FaceDataCloud;
+
+  // Indices
+  using VertexIndex = typename Base::VertexIndex;
+  using HalfEdgeIndex = typename Base::HalfEdgeIndex;
+  using EdgeIndex = typename Base::EdgeIndex;
+  using FaceIndex = typename Base::FaceIndex;
+
+  using VertexIndices = typename Base::VertexIndices;
+  using HalfEdgeIndices = typename Base::HalfEdgeIndices;
+  using EdgeIndices = typename Base::EdgeIndices;
+  using FaceIndices = typename Base::FaceIndices;
+
+  // Circulators
+  using VertexAroundVertexCirculator = typename Base::VertexAroundVertexCirculator;
+  using OutgoingHalfEdgeAroundVertexCirculator =
+      typename Base::OutgoingHalfEdgeAroundVertexCirculator;
+  using IncomingHalfEdgeAroundVertexCirculator =
+      typename Base::IncomingHalfEdgeAroundVertexCirculator;
+  using FaceAroundVertexCirculator = typename Base::FaceAroundVertexCirculator;
+  using VertexAroundFaceCirculator = typename Base::VertexAroundFaceCirculator;
+  using InnerHalfEdgeAroundFaceCirculator =
+      typename Base::InnerHalfEdgeAroundFaceCirculator;
+  using OuterHalfEdgeAroundFaceCirculator =
+      typename Base::OuterHalfEdgeAroundFaceCirculator;
+  using FaceAroundFaceCirculator = typename Base::FaceAroundFaceCirculator;
+
+  /** \brief Constructor. */
+  PolygonMesh() : Base(), add_triangle_(3), add_quad_(4) {}
+
+  /** \brief The base method of addFace is hidden because of the overloads in this
+   * class. */
+  using Base::addFace;
+
+  /** \brief Add a triangle to the mesh. Data is only added if it is associated with the
+   * elements. The last vertex is connected with the first one.
+   * \param[in] idx_v_0 Index
+   * to the first vertex.
+   * \param[in] idx_v_1        Index to the second vertex.
+   * \param[in] idx_v_2        Index to the third vertex.
+   * \param[in] face_data      Data that is set for the face.
+   * \param[in] half_edge_data Data that is set for all added half-edges.
+   * \param[in] edge_data      Data that is set for all added edges.
+   * \return Index to the new face. Failure is signaled by returning an invalid face
+   * index.
+   * \warning The vertices must be valid and unique (each vertex may be contained
+   * only once). Not complying with this requirement results in undefined behavior!
+   */
+  inline FaceIndex
+  addFace(const VertexIndex& idx_v_0,
+          const VertexIndex& idx_v_1,
+          const VertexIndex& idx_v_2,
+          const FaceData& face_data = FaceData(),
+          const EdgeData& edge_data = EdgeData(),
+          const HalfEdgeData& half_edge_data = HalfEdgeData())
+  {
+    add_triangle_[0] = idx_v_0;
+    add_triangle_[1] = idx_v_1;
+    add_triangle_[2] = idx_v_2;
+
+    return (this->addFaceImplBase(add_triangle_, face_data, edge_data, half_edge_data));
+  }
+
+  /** \brief Add a quad to the mesh. Data is only added if it is associated with the
+   * elements. The last vertex is connected with the first one.
+   * \param[in] idx_v_0 Index to the first vertex.
+   * \param[in] idx_v_1        Index to the second vertex.
+   * \param[in] idx_v_2        Index to the third vertex.
+   * \param[in] idx_v_3        Index to the fourth vertex.
+   * \param[in] face_data      Data that is set for the face.
+   * \param[in] half_edge_data Data that is set for all added half-edges.
+   * \param[in] edge_data      Data that is set for all added edges.
+   * \return Index to the new face. Failure is signaled by returning an invalid face
+   * index.
+   * \warning The vertices must be valid and unique (each vertex may be contained
+   * only once). Not complying with this requirement results in undefined behavior!
+   */
+  inline FaceIndex
+  addFace(const VertexIndex& idx_v_0,
+          const VertexIndex& idx_v_1,
+          const VertexIndex& idx_v_2,
+          const VertexIndex& idx_v_3,
+          const FaceData& face_data = FaceData(),
+          const EdgeData& edge_data = EdgeData(),
+          const HalfEdgeData& half_edge_data = HalfEdgeData())
+  {
+    add_quad_[0] = idx_v_0;
+    add_quad_[1] = idx_v_1;
+    add_quad_[2] = idx_v_2;
+    add_quad_[3] = idx_v_3;
+
+    return (this->addFaceImplBase(add_quad_, face_data, edge_data, half_edge_data));
+  }
+
+private:
+  // NOTE: Can't use the typedef of Base as a friend.
+  friend class pcl::geometry::
+      MeshBase<PolygonMesh<MeshTraitsT>, MeshTraitsT, pcl::geometry::PolygonMeshTag>;
+
+  /** \brief addFace for the polygon mesh. */
+  inline FaceIndex
+  addFaceImpl(const VertexIndices& vertices,
+              const FaceData& face_data,
+              const EdgeData& edge_data,
+              const HalfEdgeData& half_edge_data)
   {
-    /** \brief Tag describing the type of the mesh. */
-    struct PolygonMeshTag {};
-
-    /** \brief General half-edge mesh that can store any polygon with a minimum number of vertices of 3.
-      * \tparam MeshTraitsT Please have a look at pcl::geometry::DefaultMeshTraits.
-      * \author Martin Saelzle
-      * \ingroup geometry
-      */
-    template <class MeshTraitsT>
-    class PolygonMesh : public pcl::geometry::MeshBase <PolygonMesh <MeshTraitsT>, MeshTraitsT, PolygonMeshTag>
-    {
-      public:
-
-        using Base = pcl::geometry::MeshBase <PolygonMesh <MeshTraitsT>, MeshTraitsT, PolygonMeshTag>;
-
-        using Self = PolygonMesh<MeshTraitsT>;
-        using Ptr = shared_ptr<Self>;
-        using ConstPtr = shared_ptr<const Self>;
-
-        using VertexData = typename Base::VertexData;
-        using HalfEdgeData = typename Base::HalfEdgeData;
-        using EdgeData = typename Base::EdgeData;
-        using FaceData = typename Base::FaceData;
-        using IsManifold = typename Base::IsManifold;
-        using MeshTag = typename Base::MeshTag;
-
-        using HasVertexData = typename Base::HasVertexData;
-        using HasHalfEdgeData = typename Base::HasHalfEdgeData;
-        using HasEdgeData = typename Base::HasEdgeData;
-        using HasFaceData = typename Base::HasFaceData;
-
-        using VertexDataCloud = typename Base::VertexDataCloud;
-        using HalfEdgeDataCloud = typename Base::HalfEdgeDataCloud;
-        using EdgeDataCloud = typename Base::EdgeDataCloud;
-        using FaceDataCloud = typename Base::FaceDataCloud;
-
-        // Indices
-        using VertexIndex = typename Base::VertexIndex;
-        using HalfEdgeIndex = typename Base::HalfEdgeIndex;
-        using EdgeIndex = typename Base::EdgeIndex;
-        using FaceIndex = typename Base::FaceIndex;
-
-        using VertexIndices = typename Base::VertexIndices;
-        using HalfEdgeIndices = typename Base::HalfEdgeIndices;
-        using EdgeIndices = typename Base::EdgeIndices;
-        using FaceIndices = typename Base::FaceIndices;
-
-        // Circulators
-        using VertexAroundVertexCirculator = typename Base::VertexAroundVertexCirculator;
-        using OutgoingHalfEdgeAroundVertexCirculator = typename Base::OutgoingHalfEdgeAroundVertexCirculator;
-        using IncomingHalfEdgeAroundVertexCirculator = typename Base::IncomingHalfEdgeAroundVertexCirculator;
-        using FaceAroundVertexCirculator = typename Base::FaceAroundVertexCirculator;
-        using VertexAroundFaceCirculator = typename Base::VertexAroundFaceCirculator;
-        using InnerHalfEdgeAroundFaceCirculator = typename Base::InnerHalfEdgeAroundFaceCirculator;
-        using OuterHalfEdgeAroundFaceCirculator = typename Base::OuterHalfEdgeAroundFaceCirculator;
-        using FaceAroundFaceCirculator = typename Base::FaceAroundFaceCirculator;
-
-        /** \brief Constructor. */
-        PolygonMesh ()
-          : Base (),
-            add_triangle_ (3),
-            add_quad_ (4)
-        {
-        }
-
-        /** \brief The base method of addFace is hidden because of the overloads in this class. */
-        using Base::addFace;
-
-        /** \brief Add a triangle to the mesh. Data is only added if it is associated with the elements. The last vertex is connected with the first one.
-          * \param[in] idx_v_0        Index to the first vertex.
-          * \param[in] idx_v_1        Index to the second vertex.
-          * \param[in] idx_v_2        Index to the third vertex.
-          * \param[in] face_data      Data that is set for the face.
-          * \param[in] half_edge_data Data that is set for all added half-edges.
-          * \param[in] edge_data      Data that is set for all added edges.
-          * \return Index to the new face. Failure is signaled by returning an invalid face index.
-          * \warning The vertices must be valid and unique (each vertex may be contained only once). Not complying with this requirement results in undefined behavior!
-          */
-        inline FaceIndex
-        addFace (const VertexIndex&   idx_v_0,
-                 const VertexIndex&   idx_v_1,
-                 const VertexIndex&   idx_v_2,
-                 const FaceData&      face_data      = FaceData (),
-                 const EdgeData&      edge_data      = EdgeData (),
-                 const HalfEdgeData&  half_edge_data = HalfEdgeData ())
-        {
-          add_triangle_ [0] = idx_v_0;
-          add_triangle_ [1] = idx_v_1;
-          add_triangle_ [2] = idx_v_2;
-
-          return (this->addFaceImplBase (add_triangle_, face_data, edge_data, half_edge_data));
-        }
-
-        /** \brief Add a quad to the mesh. Data is only added if it is associated with the elements. The last vertex is connected with the first one.
-          * \param[in] idx_v_0        Index to the first vertex.
-          * \param[in] idx_v_1        Index to the second vertex.
-          * \param[in] idx_v_2        Index to the third vertex.
-          * \param[in] idx_v_3        Index to the fourth vertex.
-          * \param[in] face_data      Data that is set for the face.
-          * \param[in] half_edge_data Data that is set for all added half-edges.
-          * \param[in] edge_data      Data that is set for all added edges.
-          * \return Index to the new face. Failure is signaled by returning an invalid face index.
-          * \warning The vertices must be valid and unique (each vertex may be contained only once). Not complying with this requirement results in undefined behavior!
-          */
-        inline FaceIndex
-        addFace (const VertexIndex&   idx_v_0,
-                 const VertexIndex&   idx_v_1,
-                 const VertexIndex&   idx_v_2,
-                 const VertexIndex&   idx_v_3,
-                 const FaceData&      face_data      = FaceData (),
-                 const EdgeData&      edge_data      = EdgeData (),
-                 const HalfEdgeData&  half_edge_data = HalfEdgeData ())
-        {
-          add_quad_ [0] = idx_v_0;
-          add_quad_ [1] = idx_v_1;
-          add_quad_ [2] = idx_v_2;
-          add_quad_ [3] = idx_v_3;
-
-          return (this->addFaceImplBase (add_quad_, face_data, edge_data, half_edge_data));
-        }
-
-      private:
-
-        // NOTE: Can't use the typedef of Base as a friend.
-        friend class pcl::geometry::MeshBase <PolygonMesh <MeshTraitsT>, MeshTraitsT, pcl::geometry::PolygonMeshTag>;
-
-        /** \brief addFace for the polygon mesh. */
-        inline FaceIndex
-        addFaceImpl (const VertexIndices& vertices,
-                     const FaceData&      face_data,
-                     const EdgeData&      edge_data,
-                     const HalfEdgeData&  half_edge_data)
-        {
-          return (this->addFaceImplBase (vertices, face_data, edge_data, half_edge_data));
-        }
-
-        ////////////////////////////////////////////////////////////////////////
-        // Members
-        ////////////////////////////////////////////////////////////////////////
-
-        /** \brief Storage for adding a triangle. */
-        VertexIndices add_triangle_;
-
-        /** \brief Storage for adding a quad. */
-        VertexIndices add_quad_;
-
-      public:
-        PCL_MAKE_ALIGNED_OPERATOR_NEW
-    };
-  } // End namespace geom
+    return (this->addFaceImplBase(vertices, face_data, edge_data, half_edge_data));
+  }
+
+  ////////////////////////////////////////////////////////////////////////
+  // Members
+  ////////////////////////////////////////////////////////////////////////
+
+  /** \brief Storage for adding a triangle. */
+  VertexIndices add_triangle_;
+
+  /** \brief Storage for adding a quad. */
+  VertexIndices add_quad_;
+
+public:
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+};
+} // namespace geometry
 } // End namespace pcl
diff --git a/geometry/include/pcl/geometry/polygon_operations.h b/geometry/include/pcl/geometry/polygon_operations.h
index 5df66828..56ea0e16 100644
--- a/geometry/include/pcl/geometry/polygon_operations.h
+++ b/geometry/include/pcl/geometry/polygon_operations.h
@@ -37,31 +37,41 @@
 
 #pragma once
 
-#include "planar_polygon.h"
 #include <pcl/point_cloud.h>
 
-namespace pcl
-{
-  /** \brief see approximatePolygon2D
-    * \author Suat Gedikli <gedikli@willowgarage.com>
-    */
-  template <typename PointT>
-  void approximatePolygon (const PlanarPolygon<PointT>& polygon, PlanarPolygon<PointT>& approx_polygon, float threshold, bool refine = false, bool closed = true);
-  
-  /** \brief returns an approximate polygon to given 2D contour. Uses just X and Y values.
-    * \note  if refinement is not turned on, the resulting polygon will contain points from the original contour with their original z values (if any)
-    * \note  if refinement is turned on, the z values of the refined polygon are not valid and should be set to 0 if point contains z attribute. 
-    * \param [in] polygon input polygon
-    * \param [out] approx_polygon approximate polygon
-    * \param [in] threshold maximum allowed distance of an input vertex to an output edge
-    * \param refine
-    * \param [in] closed whether it is a closed polygon or a polyline
-    * \author Suat Gedikli <gedikli@willowgarage.com>
-    */
-  template <typename PointT>
-  void approximatePolygon2D (const typename PointCloud<PointT>::VectorType &polygon, 
-                             typename PointCloud<PointT>::VectorType &approx_polygon, 
-                             float threshold, bool refine = false, bool closed = true);
+#include "planar_polygon.h"
+
+namespace pcl {
+/** \brief see approximatePolygon2D
+ * \author Suat Gedikli <gedikli@willowgarage.com>
+ */
+template <typename PointT>
+void
+approximatePolygon(const PlanarPolygon<PointT>& polygon,
+                   PlanarPolygon<PointT>& approx_polygon,
+                   float threshold,
+                   bool refine = false,
+                   bool closed = true);
+
+/** \brief returns an approximate polygon to given 2D contour. Uses just X and Y values.
+ * \note  if refinement is not turned on, the resulting polygon will contain points from
+ * the original contour with their original z values (if any)
+ * \note  if refinement is turned on, the z values of the refined polygon are not valid
+ * and should be set to 0 if point contains z attribute.
+ * \param [in] polygon input polygon
+ * \param [out] approx_polygon approximate polygon
+ * \param [in] threshold maximum allowed distance of an input vertex to an output edge
+ * \param refine
+ * \param [in] closed whether it is a closed polygon or a polyline
+ * \author Suat Gedikli <gedikli@willowgarage.com>
+ */
+template <typename PointT>
+void
+approximatePolygon2D(const typename PointCloud<PointT>::VectorType& polygon,
+                     typename PointCloud<PointT>::VectorType& approx_polygon,
+                     float threshold,
+                     bool refine = false,
+                     bool closed = true);
 
 } // namespace pcl
 
diff --git a/geometry/include/pcl/geometry/quad_mesh.h b/geometry/include/pcl/geometry/quad_mesh.h
index 22b442db..62927a36 100644
--- a/geometry/include/pcl/geometry/quad_mesh.h
+++ b/geometry/include/pcl/geometry/quad_mesh.h
@@ -40,135 +40,136 @@
 
 #pragma once
 
+#include <pcl/geometry/mesh_base.h>
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
-#include <pcl/geometry/mesh_base.h>
 
-namespace pcl
-{
-  namespace geometry
+namespace pcl {
+namespace geometry {
+/** \brief Tag describing the type of the mesh. */
+struct QuadMeshTag {};
+
+/** \brief Half-edge mesh that can only store quads.
+ * \tparam MeshTraitsT Please have a look at pcl::geometry::DefaultMeshTraits.
+ * \author Martin Saelzle
+ * \ingroup geometry
+ */
+template <class MeshTraitsT>
+class QuadMesh
+: public pcl::geometry::MeshBase<QuadMesh<MeshTraitsT>, MeshTraitsT, QuadMeshTag> {
+public:
+  using Base = pcl::geometry::MeshBase<QuadMesh<MeshTraitsT>, MeshTraitsT, QuadMeshTag>;
+
+  using Self = QuadMesh<MeshTraitsT>;
+  using Ptr = shared_ptr<Self>;
+  using ConstPtr = shared_ptr<const Self>;
+
+  using VertexData = typename Base::VertexData;
+  using HalfEdgeData = typename Base::HalfEdgeData;
+  using EdgeData = typename Base::EdgeData;
+  using FaceData = typename Base::FaceData;
+  using IsManifold = typename Base::IsManifold;
+  using MeshTag = typename Base::MeshTag;
+
+  using HasVertexData = typename Base::HasVertexData;
+  using HasHalfEdgeData = typename Base::HasHalfEdgeData;
+  using HasEdgeData = typename Base::HasEdgeData;
+  using HasFaceData = typename Base::HasFaceData;
+
+  using VertexDataCloud = typename Base::VertexDataCloud;
+  using HalfEdgeDataCloud = typename Base::HalfEdgeDataCloud;
+  using EdgeDataCloud = typename Base::EdgeDataCloud;
+  using FaceDataCloud = typename Base::FaceDataCloud;
+
+  // Indices
+  using VertexIndex = typename Base::VertexIndex;
+  using HalfEdgeIndex = typename Base::HalfEdgeIndex;
+  using EdgeIndex = typename Base::EdgeIndex;
+  using FaceIndex = typename Base::FaceIndex;
+
+  using VertexIndices = typename Base::VertexIndices;
+  using HalfEdgeIndices = typename Base::HalfEdgeIndices;
+  using EdgeIndices = typename Base::EdgeIndices;
+  using FaceIndices = typename Base::FaceIndices;
+
+  // Circulators
+  using VertexAroundVertexCirculator = typename Base::VertexAroundVertexCirculator;
+  using OutgoingHalfEdgeAroundVertexCirculator =
+      typename Base::OutgoingHalfEdgeAroundVertexCirculator;
+  using IncomingHalfEdgeAroundVertexCirculator =
+      typename Base::IncomingHalfEdgeAroundVertexCirculator;
+  using FaceAroundVertexCirculator = typename Base::FaceAroundVertexCirculator;
+  using VertexAroundFaceCirculator = typename Base::VertexAroundFaceCirculator;
+  using InnerHalfEdgeAroundFaceCirculator =
+      typename Base::InnerHalfEdgeAroundFaceCirculator;
+  using OuterHalfEdgeAroundFaceCirculator =
+      typename Base::OuterHalfEdgeAroundFaceCirculator;
+  using FaceAroundFaceCirculator = typename Base::FaceAroundFaceCirculator;
+
+  /** \brief Constructor. */
+  QuadMesh() : Base(), add_quad_(4) {}
+
+  /** \brief The base method of addFace is hidden because of the overloads in this
+   * class. */
+  using Base::addFace;
+
+  /** \brief Add a quad to the mesh. Data is only added if it is associated with the
+   * elements. The last vertex is connected with the first one.
+   * \param[in] idx_v_0 Index to the first vertex.
+   * \param[in] idx_v_1        Index to the second vertex.
+   * \param[in] idx_v_2        Index to the third vertex.
+   * \param[in] idx_v_3        Index to the fourth vertex.
+   * \param[in] face_data      Data that is set for the face.
+   * \param[in] half_edge_data Data that is set for all added half-edges.
+   * \param[in] edge_data      Data that is set for all added edges.
+   * \return Index to the new face. Failure is signaled by returning an invalid face
+   * index.
+   * \warning The vertices must be valid and unique (each vertex may be contained
+   * only once). Not complying with this requirement results in undefined behavior!
+   */
+  inline FaceIndex
+  addFace(const VertexIndex& idx_v_0,
+          const VertexIndex& idx_v_1,
+          const VertexIndex& idx_v_2,
+          const VertexIndex& idx_v_3,
+          const FaceData& face_data = FaceData(),
+          const EdgeData& edge_data = EdgeData(),
+          const HalfEdgeData& half_edge_data = HalfEdgeData())
+  {
+    add_quad_[0] = idx_v_0;
+    add_quad_[1] = idx_v_1;
+    add_quad_[2] = idx_v_2;
+    add_quad_[3] = idx_v_3;
+
+    return (this->addFaceImplBase(add_quad_, face_data, edge_data, half_edge_data));
+  }
+
+private:
+  // NOTE: Can't use the typedef of Base as a friend.
+  friend class pcl::geometry::
+      MeshBase<QuadMesh<MeshTraitsT>, MeshTraitsT, pcl::geometry::QuadMeshTag>;
+
+  /** \brief addFace for the quad mesh. */
+  inline FaceIndex
+  addFaceImpl(const VertexIndices& vertices,
+              const FaceData& face_data,
+              const EdgeData& edge_data,
+              const HalfEdgeData& half_edge_data)
   {
-    /** \brief Tag describing the type of the mesh. */
-    struct QuadMeshTag {};
-
-    /** \brief Half-edge mesh that can only store quads.
-      * \tparam MeshTraitsT Please have a look at pcl::geometry::DefaultMeshTraits.
-      * \author Martin Saelzle
-      * \ingroup geometry
-      */
-    template <class MeshTraitsT>
-    class QuadMesh : public pcl::geometry::MeshBase <QuadMesh <MeshTraitsT>, MeshTraitsT, QuadMeshTag>
-    {
-      public:
-
-        using Base = pcl::geometry::MeshBase <QuadMesh <MeshTraitsT>, MeshTraitsT, QuadMeshTag>;
-
-        using Self = QuadMesh<MeshTraitsT>;
-        using Ptr = shared_ptr<Self>;
-        using ConstPtr = shared_ptr<const Self>;
-
-        using VertexData = typename Base::VertexData;
-        using HalfEdgeData = typename Base::HalfEdgeData;
-        using EdgeData = typename Base::EdgeData;
-        using FaceData = typename Base::FaceData;
-        using IsManifold = typename Base::IsManifold;
-        using MeshTag = typename Base::MeshTag;
-
-        using HasVertexData = typename Base::HasVertexData;
-        using HasHalfEdgeData = typename Base::HasHalfEdgeData;
-        using HasEdgeData = typename Base::HasEdgeData;
-        using HasFaceData = typename Base::HasFaceData;
-
-        using VertexDataCloud = typename Base::VertexDataCloud;
-        using HalfEdgeDataCloud = typename Base::HalfEdgeDataCloud;
-        using EdgeDataCloud = typename Base::EdgeDataCloud;
-        using FaceDataCloud = typename Base::FaceDataCloud;
-
-        // Indices
-        using VertexIndex = typename Base::VertexIndex;
-        using HalfEdgeIndex = typename Base::HalfEdgeIndex;
-        using EdgeIndex = typename Base::EdgeIndex;
-        using FaceIndex = typename Base::FaceIndex;
-
-        using VertexIndices = typename Base::VertexIndices;
-        using HalfEdgeIndices = typename Base::HalfEdgeIndices;
-        using EdgeIndices = typename Base::EdgeIndices;
-        using FaceIndices = typename Base::FaceIndices;
-
-        // Circulators
-        using VertexAroundVertexCirculator = typename Base::VertexAroundVertexCirculator;
-        using OutgoingHalfEdgeAroundVertexCirculator = typename Base::OutgoingHalfEdgeAroundVertexCirculator;
-        using IncomingHalfEdgeAroundVertexCirculator = typename Base::IncomingHalfEdgeAroundVertexCirculator;
-        using FaceAroundVertexCirculator = typename Base::FaceAroundVertexCirculator;
-        using VertexAroundFaceCirculator = typename Base::VertexAroundFaceCirculator;
-        using InnerHalfEdgeAroundFaceCirculator = typename Base::InnerHalfEdgeAroundFaceCirculator;
-        using OuterHalfEdgeAroundFaceCirculator = typename Base::OuterHalfEdgeAroundFaceCirculator;
-        using FaceAroundFaceCirculator = typename Base::FaceAroundFaceCirculator;
-
-        /** \brief Constructor. */
-        QuadMesh ()
-          : Base (),
-            add_quad_ (4)
-        {
-        }
-
-        /** \brief The base method of addFace is hidden because of the overloads in this class. */
-        using Base::addFace;
-
-        /** \brief Add a quad to the mesh. Data is only added if it is associated with the elements. The last vertex is connected with the first one.
-          * \param[in] idx_v_0        Index to the first vertex.
-          * \param[in] idx_v_1        Index to the second vertex.
-          * \param[in] idx_v_2        Index to the third vertex.
-          * \param[in] idx_v_3        Index to the fourth vertex.
-          * \param[in] face_data      Data that is set for the face.
-          * \param[in] half_edge_data Data that is set for all added half-edges.
-          * \param[in] edge_data      Data that is set for all added edges.
-          * \return Index to the new face. Failure is signaled by returning an invalid face index.
-          * \warning The vertices must be valid and unique (each vertex may be contained only once). Not complying with this requirement results in undefined behavior!
-          */
-        inline FaceIndex
-        addFace (const VertexIndex&   idx_v_0,
-                 const VertexIndex&   idx_v_1,
-                 const VertexIndex&   idx_v_2,
-                 const VertexIndex&   idx_v_3,
-                 const FaceData&      face_data      = FaceData (),
-                 const EdgeData&      edge_data      = EdgeData (),
-                 const HalfEdgeData&  half_edge_data = HalfEdgeData ())
-        {
-          add_quad_ [0] = idx_v_0;
-          add_quad_ [1] = idx_v_1;
-          add_quad_ [2] = idx_v_2;
-          add_quad_ [3] = idx_v_3;
-
-          return (this->addFaceImplBase (add_quad_, face_data, edge_data, half_edge_data));
-        }
-
-      private:
-
-        // NOTE: Can't use the typedef of Base as a friend.
-        friend class pcl::geometry::MeshBase <QuadMesh <MeshTraitsT>, MeshTraitsT, pcl::geometry::QuadMeshTag>;
-
-        /** \brief addFace for the quad mesh. */
-        inline FaceIndex
-        addFaceImpl (const VertexIndices& vertices,
-                     const FaceData&      face_data,
-                     const EdgeData&      edge_data,
-                     const HalfEdgeData&  half_edge_data)
-        {
-          if (vertices.size () == 4)
-            return (this->addFaceImplBase (vertices, face_data, edge_data, half_edge_data));
-          return (FaceIndex ());
-        }
-
-        ////////////////////////////////////////////////////////////////////////
-        // Members
-        ////////////////////////////////////////////////////////////////////////
-
-        /** \brief Storage for adding a quad. */
-        VertexIndices add_quad_;
-
-      public:
-        PCL_MAKE_ALIGNED_OPERATOR_NEW
-    };
-  } // End namespace geom
+    if (vertices.size() == 4)
+      return (this->addFaceImplBase(vertices, face_data, edge_data, half_edge_data));
+    return (FaceIndex());
+  }
+
+  ////////////////////////////////////////////////////////////////////////
+  // Members
+  ////////////////////////////////////////////////////////////////////////
+
+  /** \brief Storage for adding a quad. */
+  VertexIndices add_quad_;
+
+public:
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+};
+} // namespace geometry
 } // End namespace pcl
diff --git a/geometry/include/pcl/geometry/triangle_mesh.h b/geometry/include/pcl/geometry/triangle_mesh.h
index d5f7f5bf..713650f7 100644
--- a/geometry/include/pcl/geometry/triangle_mesh.h
+++ b/geometry/include/pcl/geometry/triangle_mesh.h
@@ -40,313 +40,353 @@
 
 #pragma once
 
-#include <utility>
-
+#include <pcl/geometry/mesh_base.h>
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
-#include <pcl/geometry/mesh_base.h>
 
-namespace pcl
-{
-  namespace geometry
+#include <utility>
+
+namespace pcl {
+namespace geometry {
+/** \brief Tag describing the type of the mesh. */
+struct TriangleMeshTag {};
+
+/** \brief Half-edge mesh that can only store triangles.
+ * \tparam MeshTraitsT Please have a look at pcl::geometry::DefaultMeshTraits.
+ * \author Martin Saelzle
+ * \ingroup geometry
+ */
+template <class MeshTraitsT>
+class TriangleMesh : public pcl::geometry::MeshBase<TriangleMesh<MeshTraitsT>,
+                                                    MeshTraitsT,
+                                                    TriangleMeshTag> {
+public:
+  using Base =
+      pcl::geometry::MeshBase<TriangleMesh<MeshTraitsT>, MeshTraitsT, TriangleMeshTag>;
+
+  using Self = TriangleMesh<MeshTraitsT>;
+  using Ptr = shared_ptr<Self>;
+  using ConstPtr = shared_ptr<const Self>;
+
+  using VertexData = typename Base::VertexData;
+  using HalfEdgeData = typename Base::HalfEdgeData;
+  using EdgeData = typename Base::EdgeData;
+  using FaceData = typename Base::FaceData;
+  using IsManifold = typename Base::IsManifold;
+  using MeshTag = typename Base::MeshTag;
+
+  using HasVertexData = typename Base::HasVertexData;
+  using HasHalfEdgeData = typename Base::HasHalfEdgeData;
+  using HasEdgeData = typename Base::HasEdgeData;
+  using HasFaceData = typename Base::HasFaceData;
+
+  using VertexDataCloud = typename Base::VertexDataCloud;
+  using HalfEdgeDataCloud = typename Base::HalfEdgeDataCloud;
+  using EdgeDataCloud = typename Base::EdgeDataCloud;
+  using FaceDataCloud = typename Base::FaceDataCloud;
+
+  // Indices
+  using VertexIndex = typename Base::VertexIndex;
+  using HalfEdgeIndex = typename Base::HalfEdgeIndex;
+  using EdgeIndex = typename Base::EdgeIndex;
+  using FaceIndex = typename Base::FaceIndex;
+  using FaceIndexPair = std::pair<FaceIndex, FaceIndex>;
+
+  using VertexIndices = typename Base::VertexIndices;
+  using HalfEdgeIndices = typename Base::HalfEdgeIndices;
+  using EdgeIndices = typename Base::EdgeIndices;
+  using FaceIndices = typename Base::FaceIndices;
+
+  // Circulators
+  using VertexAroundVertexCirculator = typename Base::VertexAroundVertexCirculator;
+  using OutgoingHalfEdgeAroundVertexCirculator =
+      typename Base::OutgoingHalfEdgeAroundVertexCirculator;
+  using IncomingHalfEdgeAroundVertexCirculator =
+      typename Base::IncomingHalfEdgeAroundVertexCirculator;
+  using FaceAroundVertexCirculator = typename Base::FaceAroundVertexCirculator;
+  using VertexAroundFaceCirculator = typename Base::VertexAroundFaceCirculator;
+  using InnerHalfEdgeAroundFaceCirculator =
+      typename Base::InnerHalfEdgeAroundFaceCirculator;
+  using OuterHalfEdgeAroundFaceCirculator =
+      typename Base::OuterHalfEdgeAroundFaceCirculator;
+  using FaceAroundFaceCirculator = typename Base::FaceAroundFaceCirculator;
+
+  /** \brief Constructor. */
+  TriangleMesh() : Base(), add_triangle_(3), inner_he_atp_(4), is_new_atp_(4) {}
+
+  /** \brief The base method of addFace is hidden because of the overloads in this
+   * class. */
+  using Base::addFace;
+
+  /** \brief Add a triangle to the mesh. Data is only added if it is associated with the
+   * elements. The last vertex is connected with the first one.
+   * \param[in] idx_v_0 Index to the first vertex.
+   * \param[in] idx_v_1        Index to the second vertex.
+   * \param[in] idx_v_2        Index to the third vertex.
+   * \param[in] face_data      Data that is set for the face.
+   * \param[in] half_edge_data Data that is set for all added half-edges.
+   * \param[in] edge_data      Data that is set for all added edges.
+   * \return Index to the new face. Failure is signaled by returning an invalid face
+   * index.
+   * \warning The vertices must be valid and unique (each vertex may be contained
+   * only once). Not complying with this requirement results in undefined behavior!
+   */
+  inline FaceIndex
+  addFace(const VertexIndex& idx_v_0,
+          const VertexIndex& idx_v_1,
+          const VertexIndex& idx_v_2,
+          const FaceData& face_data = FaceData(),
+          const EdgeData& edge_data = EdgeData(),
+          const HalfEdgeData& half_edge_data = HalfEdgeData())
+  {
+    add_triangle_[0] = idx_v_0;
+    add_triangle_[1] = idx_v_1;
+    add_triangle_[2] = idx_v_2;
+
+    return (this->addFaceImplBase(add_triangle_, face_data, edge_data, half_edge_data));
+  }
+
+  /** \brief Add two triangles for the four given input vertices. When using a manifold
+   * triangle mesh it is not possible to connect two bounded regions without going
+   * through a non-manifold intermediate step. This method first tries to add the
+   * triangles individually and if this fails connects the whole configuration at once
+   * (if possible).
+   * \param[in] vertices       Indices to the vertices of the new face.  (The size
+   * must be equal to four).
+   * \param[in] face_data      Data that is set for the face.
+   * \param[in] half_edge_data Data that is set for all added half-edges.
+   * \param[in] edge_data      Data that is set for all added edges.
+   * \return Pair of face indices. The first index is valid if one triangle was added.
+   * Both indices are valid if two triangles were added.
+   * \warning The vertices must be valid and unique (each vertex may be contained only
+   * once). Not complying with this requirement results in undefined behavior!
+   */
+  FaceIndexPair
+  addTrianglePair(const VertexIndices& vertices,
+                  const FaceData& face_data = FaceData(),
+                  const EdgeData& edge_data = EdgeData(),
+                  const HalfEdgeData& half_edge_data = HalfEdgeData())
+  {
+    if (vertices.size() != 4) {
+      return (std::make_pair(FaceIndex(), FaceIndex()));
+    }
+    return (this->addTrianglePair(vertices[0],
+                                  vertices[1],
+                                  vertices[2],
+                                  vertices[3],
+                                  face_data,
+                                  edge_data,
+                                  half_edge_data));
+  }
+
+  /** \brief Add two triangles for the four given input vertices. When using a manifold
+   * triangle mesh it is not possible to connect two bounded regions without going
+   * through a non-manifold intermediate step. This method first tries to add the
+   * triangles individually and if this fails connects the whole configuration at once
+   * (if possible).
+   * \param[in] idx_v_0        Index to the first vertex.
+   * \param[in] idx_v_1        Index to the second vertex.
+   * \param[in] idx_v_2        Index to the third vertex.
+   * \param[in] idx_v_3        Index to the fourth vertex.
+   * \param[in] face_data      Data that is set for the face.
+   * \param[in] half_edge_data Data that is set for all added half-edges.
+   * \param[in] edge_data      Data that is set for all added edges.
+   * \return Pair of face indices. The first index is valid if one triangle was added.
+   * Both indices are valid if two triangles were added.
+   * \warning The vertices must be valid and unique (each vertex may be contained only
+   * once). Not complying with this requirement results in undefined behavior!
+   */
+  inline FaceIndexPair
+  addTrianglePair(const VertexIndex& idx_v_0,
+                  const VertexIndex& idx_v_1,
+                  const VertexIndex& idx_v_2,
+                  const VertexIndex& idx_v_3,
+                  const FaceData& face_data = FaceData(),
+                  const EdgeData& edge_data = EdgeData(),
+                  const HalfEdgeData& half_edge_data = HalfEdgeData())
+  {
+    // Try to add two faces
+    // 3 - 2
+    // | / |
+    // 0 - 1
+    FaceIndex idx_face_0 = this->addFace(idx_v_0, idx_v_1, idx_v_2, face_data);
+    FaceIndex idx_face_1 = this->addFace(idx_v_0, idx_v_2, idx_v_3, face_data);
+
+    if (idx_face_0.isValid()) {
+      return (std::make_pair(idx_face_0, idx_face_1));
+    }
+    if (idx_face_1.isValid()) {
+      idx_face_0 = this->addFace(
+          idx_v_0, idx_v_1, idx_v_2, face_data); // might be possible to add now
+      return (std::make_pair(idx_face_1, idx_face_0));
+    }
+
+    // Try to add two faces
+    // 3 - 2
+    // | \ |
+    // 0 - 1
+    idx_face_0 = this->addFace(idx_v_1, idx_v_2, idx_v_3, face_data);
+    idx_face_1 = this->addFace(idx_v_0, idx_v_1, idx_v_3, face_data);
+
+    if (idx_face_0.isValid()) {
+      return (std::make_pair(idx_face_0, idx_face_1));
+    }
+    if (idx_face_1.isValid()) {
+      idx_face_0 = this->addFace(
+          idx_v_1, idx_v_2, idx_v_3, face_data); // might be possible to add now
+      return (std::make_pair(idx_face_1, idx_face_0));
+    }
+
+    if (!IsManifold::value) {
+      return (std::make_pair(FaceIndex(), FaceIndex()));
+    }
+
+    // Check manifoldness
+    if (!Base::checkTopology1(
+            idx_v_0, idx_v_1, inner_he_atp_[0], is_new_atp_[0], IsManifold()) ||
+        !Base::checkTopology1(
+            idx_v_1, idx_v_2, inner_he_atp_[1], is_new_atp_[1], IsManifold()) ||
+        !Base::checkTopology1(
+            idx_v_2, idx_v_3, inner_he_atp_[2], is_new_atp_[2], IsManifold()) ||
+        !Base::checkTopology1(
+            idx_v_3, idx_v_0, inner_he_atp_[3], is_new_atp_[3], IsManifold())) {
+      return (std::make_pair(FaceIndex(), FaceIndex()));
+    }
+
+    // Connect the triangle pair
+    if (!is_new_atp_[0] && is_new_atp_[1] && !is_new_atp_[2] && is_new_atp_[3]) {
+      return (this->connectTrianglePair(inner_he_atp_[0],
+                                        inner_he_atp_[2],
+                                        idx_v_0,
+                                        idx_v_1,
+                                        idx_v_2,
+                                        idx_v_3,
+                                        face_data,
+                                        edge_data,
+                                        half_edge_data));
+    }
+    if (is_new_atp_[0] && !is_new_atp_[1] && is_new_atp_[2] && !is_new_atp_[3]) {
+      return (this->connectTrianglePair(inner_he_atp_[1],
+                                        inner_he_atp_[3],
+                                        idx_v_1,
+                                        idx_v_2,
+                                        idx_v_3,
+                                        idx_v_0,
+                                        face_data,
+                                        edge_data,
+                                        half_edge_data));
+    }
+    return (std::make_pair(FaceIndex(), FaceIndex()));
+  }
+
+private:
+  // NOTE: Can't use the typedef of Base as a friend.
+  friend class pcl::geometry::
+      MeshBase<TriangleMesh<MeshTraitsT>, MeshTraitsT, pcl::geometry::TriangleMeshTag>;
+
+  /** \brief addFace for the triangular mesh. */
+  inline FaceIndex
+  addFaceImpl(const VertexIndices& vertices,
+              const FaceData& face_data,
+              const EdgeData& edge_data,
+              const HalfEdgeData& half_edge_data)
+  {
+    if (vertices.size() == 3)
+      return (this->addFaceImplBase(vertices, face_data, edge_data, half_edge_data));
+    return (FaceIndex());
+  }
+
+  /** \brief Connect the triangles a-b-c and a-c-d. The edges a-b and c-d must be old
+   * and the edges b-c and d-a must be new. */
+  // d - c
+  // | / |
+  // a - b
+  FaceIndexPair
+  connectTrianglePair(const HalfEdgeIndex& idx_he_ab,
+                      const HalfEdgeIndex& idx_he_cd,
+                      const VertexIndex& idx_v_a,
+                      const VertexIndex& idx_v_b,
+                      const VertexIndex& idx_v_c,
+                      const VertexIndex& idx_v_d,
+                      const FaceData& face_data,
+                      const EdgeData& edge_data,
+                      const HalfEdgeData& he_data)
   {
-    /** \brief Tag describing the type of the mesh. */
-    struct TriangleMeshTag {};
-
-    /** \brief Half-edge mesh that can only store triangles.
-      * \tparam MeshTraitsT Please have a look at pcl::geometry::DefaultMeshTraits.
-      * \author Martin Saelzle
-      * \ingroup geometry
-      */
-    template <class MeshTraitsT>
-    class TriangleMesh : public pcl::geometry::MeshBase <TriangleMesh <MeshTraitsT>, MeshTraitsT, TriangleMeshTag>
-    {
-      public:
-
-        using Base = pcl::geometry::MeshBase <TriangleMesh <MeshTraitsT>, MeshTraitsT, TriangleMeshTag>;
-
-        using Self = TriangleMesh<MeshTraitsT>;
-        using Ptr = shared_ptr<Self>;
-        using ConstPtr = shared_ptr<const Self>;
-
-        using VertexData = typename Base::VertexData;
-        using HalfEdgeData = typename Base::HalfEdgeData;
-        using EdgeData = typename Base::EdgeData;
-        using FaceData = typename Base::FaceData;
-        using IsManifold = typename Base::IsManifold;
-        using MeshTag = typename Base::MeshTag;
-
-        using HasVertexData = typename Base::HasVertexData;
-        using HasHalfEdgeData = typename Base::HasHalfEdgeData;
-        using HasEdgeData = typename Base::HasEdgeData;
-        using HasFaceData = typename Base::HasFaceData;
-
-        using VertexDataCloud = typename Base::VertexDataCloud;
-        using HalfEdgeDataCloud = typename Base::HalfEdgeDataCloud;
-        using EdgeDataCloud = typename Base::EdgeDataCloud;
-        using FaceDataCloud = typename Base::FaceDataCloud;
-
-        // Indices
-        using VertexIndex = typename Base::VertexIndex;
-        using HalfEdgeIndex = typename Base::HalfEdgeIndex;
-        using EdgeIndex = typename Base::EdgeIndex;
-        using FaceIndex = typename Base::FaceIndex;
-        using FaceIndexPair = std::pair <FaceIndex, FaceIndex>;
-
-        using VertexIndices = typename Base::VertexIndices;
-        using HalfEdgeIndices = typename Base::HalfEdgeIndices;
-        using EdgeIndices = typename Base::EdgeIndices;
-        using FaceIndices = typename Base::FaceIndices;
-
-        // Circulators
-        using VertexAroundVertexCirculator = typename Base::VertexAroundVertexCirculator;
-        using OutgoingHalfEdgeAroundVertexCirculator = typename Base::OutgoingHalfEdgeAroundVertexCirculator;
-        using IncomingHalfEdgeAroundVertexCirculator = typename Base::IncomingHalfEdgeAroundVertexCirculator;
-        using FaceAroundVertexCirculator = typename Base::FaceAroundVertexCirculator;
-        using VertexAroundFaceCirculator = typename Base::VertexAroundFaceCirculator;
-        using InnerHalfEdgeAroundFaceCirculator = typename Base::InnerHalfEdgeAroundFaceCirculator;
-        using OuterHalfEdgeAroundFaceCirculator = typename Base::OuterHalfEdgeAroundFaceCirculator;
-        using FaceAroundFaceCirculator = typename Base::FaceAroundFaceCirculator;
-
-        /** \brief Constructor. */
-        TriangleMesh ()
-          : Base (),
-            add_triangle_ (3),
-            inner_he_atp_ (4),
-            is_new_atp_   (4)
-        {
-        }
-
-        /** \brief The base method of addFace is hidden because of the overloads in this class. */
-        using Base::addFace;
-
-        /** \brief Add a triangle to the mesh. Data is only added if it is associated with the elements. The last vertex is connected with the first one.
-          * \param[in] idx_v_0        Index to the first vertex.
-          * \param[in] idx_v_1        Index to the second vertex.
-          * \param[in] idx_v_2        Index to the third vertex.
-          * \param[in] face_data      Data that is set for the face.
-          * \param[in] half_edge_data Data that is set for all added half-edges.
-          * \param[in] edge_data      Data that is set for all added edges.
-          * \return Index to the new face. Failure is signaled by returning an invalid face index.
-          * \warning The vertices must be valid and unique (each vertex may be contained only once). Not complying with this requirement results in undefined behavior!
-          */
-        inline FaceIndex
-        addFace (const VertexIndex&   idx_v_0,
-                 const VertexIndex&   idx_v_1,
-                 const VertexIndex&   idx_v_2,
-                 const FaceData&      face_data      = FaceData (),
-                 const EdgeData&      edge_data      = EdgeData (),
-                 const HalfEdgeData&  half_edge_data = HalfEdgeData ())
-        {
-          add_triangle_ [0] = idx_v_0;
-          add_triangle_ [1] = idx_v_1;
-          add_triangle_ [2] = idx_v_2;
-
-          return (this->addFaceImplBase (add_triangle_, face_data, edge_data, half_edge_data));
-        }
-
-        /** \brief Add two triangles for the four given input vertices. When using a manifold triangle mesh it is not possible to connect two bounded regions without going through a non-manifold intermediate step. This method first tries to add the triangles individually and if this fails connects the whole configuration at once (if possible).
-          * \param[in] vertices       Indices to the vertices of the new face. (The size must be equal to four).
-          * \param[in] face_data      Data that is set for the face.
-          * \param[in] half_edge_data Data that is set for all added half-edges.
-          * \param[in] edge_data      Data that is set for all added edges.
-          * \return Pair of face indices. The first index is valid if one triangle was added. Both indices are valid if two triangles were added.
-          * \warning The vertices must be valid and unique (each vertex may be contained only once). Not complying with this requirement results in undefined behavior!
-          */
-        FaceIndexPair
-        addTrianglePair (const VertexIndices& vertices,
-                         const FaceData&      face_data      = FaceData (),
-                         const EdgeData&      edge_data      = EdgeData (),
-                         const HalfEdgeData&  half_edge_data = HalfEdgeData ())
-        {
-          if (vertices.size () != 4)
-          {
-            return (std::make_pair (FaceIndex (), FaceIndex ()));
-          }
-          return (this->addTrianglePair (vertices [0], vertices [1], vertices [2], vertices [3], face_data, edge_data, half_edge_data));
-        }
-
-        /** \brief Add two triangles for the four given input vertices. When using a manifold triangle mesh it is not possible to connect two bounded regions without going through a non-manifold intermediate step. This method first tries to add the triangles individually and if this fails connects the whole configuration at once (if possible).
-          * \param[in] idx_v_0        Index to the first vertex.
-          * \param[in] idx_v_1        Index to the second vertex.
-          * \param[in] idx_v_2        Index to the third vertex.
-          * \param[in] idx_v_3        Index to the fourth vertex.
-          * \param[in] face_data      Data that is set for the face.
-          * \param[in] half_edge_data Data that is set for all added half-edges.
-          * \param[in] edge_data      Data that is set for all added edges.
-          * \return Pair of face indices. The first index is valid if one triangle was added. Both indices are valid if two triangles were added.
-          * \warning The vertices must be valid and unique (each vertex may be contained only once). Not complying with this requirement results in undefined behavior!
-          */
-        inline FaceIndexPair
-        addTrianglePair (const VertexIndex&   idx_v_0,
-                         const VertexIndex&   idx_v_1,
-                         const VertexIndex&   idx_v_2,
-                         const VertexIndex&   idx_v_3,
-                         const FaceData&      face_data      = FaceData (),
-                         const EdgeData&      edge_data      = EdgeData (),
-                         const HalfEdgeData&  half_edge_data = HalfEdgeData ())
-        {
-          // Try to add two faces
-          // 3 - 2
-          // | / |
-          // 0 - 1
-          FaceIndex idx_face_0 = this->addFace (idx_v_0, idx_v_1, idx_v_2, face_data);
-          FaceIndex idx_face_1 = this->addFace (idx_v_0, idx_v_2, idx_v_3, face_data);
-
-          if (idx_face_0.isValid ())
-          {
-            return (std::make_pair (idx_face_0, idx_face_1));
-          }
-          if (idx_face_1.isValid ())
-          {
-            idx_face_0 = this->addFace (idx_v_0, idx_v_1, idx_v_2, face_data); // might be possible to add now
-            return (std::make_pair (idx_face_1, idx_face_0));
-          }
-
-          // Try to add two faces
-          // 3 - 2
-          // | \ |
-          // 0 - 1
-          idx_face_0 = this->addFace (idx_v_1, idx_v_2, idx_v_3, face_data);
-          idx_face_1 = this->addFace (idx_v_0, idx_v_1, idx_v_3, face_data);
-
-          if (idx_face_0.isValid ())
-          {
-            return (std::make_pair (idx_face_0, idx_face_1));
-          }
-          if (idx_face_1.isValid ())
-          {
-            idx_face_0 = this->addFace (idx_v_1, idx_v_2, idx_v_3, face_data); // might be possible to add now
-            return (std::make_pair (idx_face_1, idx_face_0));
-          }
-
-          if (!IsManifold::value)
-          {
-            return (std::make_pair (FaceIndex (), FaceIndex ()));
-          }
-
-          // Check manifoldness
-          if (!Base::checkTopology1 (idx_v_0,idx_v_1, inner_he_atp_ [0], is_new_atp_ [0], IsManifold ()) ||
-              !Base::checkTopology1 (idx_v_1,idx_v_2, inner_he_atp_ [1], is_new_atp_ [1], IsManifold ()) ||
-              !Base::checkTopology1 (idx_v_2,idx_v_3, inner_he_atp_ [2], is_new_atp_ [2], IsManifold ()) ||
-              !Base::checkTopology1 (idx_v_3,idx_v_0, inner_he_atp_ [3], is_new_atp_ [3], IsManifold ()))
-          {
-            return (std::make_pair (FaceIndex (), FaceIndex ()));
-          }
-
-          // Connect the triangle pair
-          if (!is_new_atp_ [0] && is_new_atp_ [1] && !is_new_atp_ [2] && is_new_atp_ [3])
-          {
-            return (this->connectTrianglePair (inner_he_atp_ [0], inner_he_atp_ [2], idx_v_0, idx_v_1, idx_v_2, idx_v_3, face_data, edge_data, half_edge_data));
-          }
-          if (is_new_atp_ [0] && !is_new_atp_ [1] && is_new_atp_ [2] && !is_new_atp_ [3])
-          {
-            return (this->connectTrianglePair (inner_he_atp_ [1], inner_he_atp_ [3], idx_v_1, idx_v_2, idx_v_3, idx_v_0, face_data, edge_data, half_edge_data));
-          }
-          return (std::make_pair (FaceIndex (), FaceIndex ()));
-        }
-
-      private:
-
-        // NOTE: Can't use the typedef of Base as a friend.
-        friend class pcl::geometry::MeshBase <TriangleMesh <MeshTraitsT>, MeshTraitsT, pcl::geometry::TriangleMeshTag>;
-
-        /** \brief addFace for the triangular mesh. */
-        inline FaceIndex
-        addFaceImpl (const VertexIndices& vertices,
-                     const FaceData&      face_data,
-                     const EdgeData&      edge_data,
-                     const HalfEdgeData&  half_edge_data)
-        {
-          if (vertices.size () == 3)
-            return (this->addFaceImplBase (vertices, face_data, edge_data, half_edge_data));
-          return (FaceIndex ());
-        }
-
-        /** \brief Connect the triangles a-b-c and a-c-d. The edges a-b and c-d must be old and the edges b-c and d-a must be new. */
-        // d - c
-        // | / |
-        // a - b
-        FaceIndexPair
-        connectTrianglePair (const HalfEdgeIndex& idx_he_ab,
-                             const HalfEdgeIndex& idx_he_cd,
-                             const VertexIndex&   idx_v_a,
-                             const VertexIndex&   idx_v_b,
-                             const VertexIndex&   idx_v_c,
-                             const VertexIndex&   idx_v_d,
-                             const FaceData&      face_data,
-                             const EdgeData&      edge_data,
-                             const HalfEdgeData&  he_data)
-        {
-          // Add new half-edges
-          const HalfEdgeIndex idx_he_bc = Base::addEdge (idx_v_b, idx_v_c, he_data, edge_data);
-          const HalfEdgeIndex idx_he_da = Base::addEdge (idx_v_d, idx_v_a, he_data, edge_data);
-          const HalfEdgeIndex idx_he_ca = Base::addEdge (idx_v_c, idx_v_a, he_data, edge_data);
-
-          const HalfEdgeIndex idx_he_cb = Base::getOppositeHalfEdgeIndex (idx_he_bc);
-          const HalfEdgeIndex idx_he_ad = Base::getOppositeHalfEdgeIndex (idx_he_da);
-          const HalfEdgeIndex idx_he_ac = Base::getOppositeHalfEdgeIndex (idx_he_ca);
-
-          // Get the existing half-edges
-          const HalfEdgeIndex idx_he_ab_prev = Base::getPrevHalfEdgeIndex (idx_he_ab); // No reference!
-          const HalfEdgeIndex idx_he_ab_next = Base::getNextHalfEdgeIndex (idx_he_ab); // No reference!
-
-          const HalfEdgeIndex idx_he_cd_prev = Base::getPrevHalfEdgeIndex (idx_he_cd); // No reference!
-          const HalfEdgeIndex idx_he_cd_next = Base::getNextHalfEdgeIndex (idx_he_cd); // No reference!
-
-          // Connect the outer half-edges
-          Base::connectPrevNext (idx_he_ab_prev, idx_he_ad     );
-          Base::connectPrevNext (idx_he_ad     , idx_he_cd_next);
-          Base::connectPrevNext (idx_he_cd_prev, idx_he_cb     );
-          Base::connectPrevNext (idx_he_cb     , idx_he_ab_next);
-
-          // Connect the inner half-edges
-          Base::connectPrevNext (idx_he_ab, idx_he_bc);
-          Base::connectPrevNext (idx_he_bc, idx_he_ca);
-          Base::connectPrevNext (idx_he_ca, idx_he_ab);
-
-          Base::connectPrevNext (idx_he_ac, idx_he_cd);
-          Base::connectPrevNext (idx_he_cd, idx_he_da);
-          Base::connectPrevNext (idx_he_da, idx_he_ac);
-
-          // Connect the vertices to the boundary half-edges
-          Base::setOutgoingHalfEdgeIndex (idx_v_a, idx_he_ad     );
-          Base::setOutgoingHalfEdgeIndex (idx_v_b, idx_he_ab_next);
-          Base::setOutgoingHalfEdgeIndex (idx_v_c, idx_he_cb     );
-          Base::setOutgoingHalfEdgeIndex (idx_v_d, idx_he_cd_next);
-
-          // Add and connect the faces
-          HalfEdgeIndices inner_he_abc; inner_he_abc.reserve (3);
-          inner_he_abc.push_back (idx_he_ab);
-          inner_he_abc.push_back (idx_he_bc);
-          inner_he_abc.push_back (idx_he_ca);
-
-          HalfEdgeIndices inner_he_acd; inner_he_acd.reserve (3);
-          inner_he_acd.push_back (idx_he_ac);
-          inner_he_acd.push_back (idx_he_cd);
-          inner_he_acd.push_back (idx_he_da);
-
-          const FaceIndex idx_f_abc = Base::connectFace (inner_he_abc, face_data);
-          const FaceIndex idx_f_acd = Base::connectFace (inner_he_acd, face_data);
-
-          return (std::make_pair (idx_f_abc, idx_f_acd));
-        }
-
-        ////////////////////////////////////////////////////////////////////////
-        // Members
-        ////////////////////////////////////////////////////////////////////////
-
-        /** \brief Storage for adding a triangle. */
-        VertexIndices add_triangle_;
-
-        /** \brief Storage for addTrianglePair. */
-        HalfEdgeIndices inner_he_atp_;
-
-        /** \brief Storage for addTrianglePair. */
-        std::vector <bool> is_new_atp_;
-
-      public:
-        PCL_MAKE_ALIGNED_OPERATOR_NEW
-    };
-  } // End namespace geom
+    // Add new half-edges
+    const HalfEdgeIndex idx_he_bc = Base::addEdge(idx_v_b, idx_v_c, he_data, edge_data);
+    const HalfEdgeIndex idx_he_da = Base::addEdge(idx_v_d, idx_v_a, he_data, edge_data);
+    const HalfEdgeIndex idx_he_ca = Base::addEdge(idx_v_c, idx_v_a, he_data, edge_data);
+
+    const HalfEdgeIndex idx_he_cb = Base::getOppositeHalfEdgeIndex(idx_he_bc);
+    const HalfEdgeIndex idx_he_ad = Base::getOppositeHalfEdgeIndex(idx_he_da);
+    const HalfEdgeIndex idx_he_ac = Base::getOppositeHalfEdgeIndex(idx_he_ca);
+
+    // Get the existing half-edges
+    const HalfEdgeIndex idx_he_ab_prev =
+        Base::getPrevHalfEdgeIndex(idx_he_ab); // No reference!
+    const HalfEdgeIndex idx_he_ab_next =
+        Base::getNextHalfEdgeIndex(idx_he_ab); // No reference!
+
+    const HalfEdgeIndex idx_he_cd_prev =
+        Base::getPrevHalfEdgeIndex(idx_he_cd); // No reference!
+    const HalfEdgeIndex idx_he_cd_next =
+        Base::getNextHalfEdgeIndex(idx_he_cd); // No reference!
+
+    // Connect the outer half-edges
+    Base::connectPrevNext(idx_he_ab_prev, idx_he_ad);
+    Base::connectPrevNext(idx_he_ad, idx_he_cd_next);
+    Base::connectPrevNext(idx_he_cd_prev, idx_he_cb);
+    Base::connectPrevNext(idx_he_cb, idx_he_ab_next);
+
+    // Connect the inner half-edges
+    Base::connectPrevNext(idx_he_ab, idx_he_bc);
+    Base::connectPrevNext(idx_he_bc, idx_he_ca);
+    Base::connectPrevNext(idx_he_ca, idx_he_ab);
+
+    Base::connectPrevNext(idx_he_ac, idx_he_cd);
+    Base::connectPrevNext(idx_he_cd, idx_he_da);
+    Base::connectPrevNext(idx_he_da, idx_he_ac);
+
+    // Connect the vertices to the boundary half-edges
+    Base::setOutgoingHalfEdgeIndex(idx_v_a, idx_he_ad);
+    Base::setOutgoingHalfEdgeIndex(idx_v_b, idx_he_ab_next);
+    Base::setOutgoingHalfEdgeIndex(idx_v_c, idx_he_cb);
+    Base::setOutgoingHalfEdgeIndex(idx_v_d, idx_he_cd_next);
+
+    // Add and connect the faces
+    HalfEdgeIndices inner_he_abc;
+    inner_he_abc.reserve(3);
+    inner_he_abc.push_back(idx_he_ab);
+    inner_he_abc.push_back(idx_he_bc);
+    inner_he_abc.push_back(idx_he_ca);
+
+    HalfEdgeIndices inner_he_acd;
+    inner_he_acd.reserve(3);
+    inner_he_acd.push_back(idx_he_ac);
+    inner_he_acd.push_back(idx_he_cd);
+    inner_he_acd.push_back(idx_he_da);
+
+    const FaceIndex idx_f_abc = Base::connectFace(inner_he_abc, face_data);
+    const FaceIndex idx_f_acd = Base::connectFace(inner_he_acd, face_data);
+
+    return (std::make_pair(idx_f_abc, idx_f_acd));
+  }
+
+  ////////////////////////////////////////////////////////////////////////
+  // Members
+  ////////////////////////////////////////////////////////////////////////
+
+  /** \brief Storage for adding a triangle. */
+  VertexIndices add_triangle_;
+
+  /** \brief Storage for addTrianglePair. */
+  HalfEdgeIndices inner_he_atp_;
+
+  /** \brief Storage for addTrianglePair. */
+  std::vector<bool> is_new_atp_;
+
+public:
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+};
+} // namespace geometry
 } // End namespace pcl
diff --git a/gpu/CMakeLists.txt b/gpu/CMakeLists.txt
index 99f36bf6..45daad5c 100644
--- a/gpu/CMakeLists.txt
+++ b/gpu/CMakeLists.txt
@@ -11,7 +11,11 @@ endif()
 if(CMAKE_COMPILER_IS_GNUCXX)
   string(REPLACE "-Wold-style-cast" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
   string(REPLACE "-Wno-invalid-offsetof" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-conversion -Wno-unused-parameter -Wno-unused-variable -Wno-unused-function -Wno-unused-but-set-variable")
+  string(APPEND CMAKE_CXX_FLAGS " -Wno-conversion -Wno-unused-parameter -Wno-unused-variable -Wno-unused-function -Wno-unused-but-set-variable")
+  # allow deprecation warnings in Eigen(3.3.7)/Core, see here: https://gitlab.kitware.com/vtk/vtk/-/issues/17661
+  string(APPEND CMAKE_CXX_FLAGS " -Wno-error=cpp")
+  # allow maybe-uninitialized warnings from thrust library.
+  string(APPEND CMAKE_CXX_FLAGS " -Wno-error=maybe-uninitialized")
 endif()
 
 collect_subproject_directory_names("${CMAKE_CURRENT_SOURCE_DIR}" "CMakeLists.txt" PCL_GPU_MODULES_NAMES PCL_GPU_MODULES_DIRS)
diff --git a/gpu/containers/include/pcl/gpu/containers/device_array.h b/gpu/containers/include/pcl/gpu/containers/device_array.h
index dec8b415..74dfcbe8 100644
--- a/gpu/containers/include/pcl/gpu/containers/device_array.h
+++ b/gpu/containers/include/pcl/gpu/containers/device_array.h
@@ -36,233 +36,286 @@
 
 #pragma once
 
-#include <pcl/pcl_exports.h>
 #include <pcl/gpu/containers/device_memory.h>
+#include <pcl/pcl_exports.h>
 
 #include <vector>
 
-namespace pcl
-{
-    namespace gpu
-    {                   
-        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-        /** \brief @b DeviceArray class
-          * 
-          * \note Typed container for GPU memory with reference counting.          
-          *          
-          * \author Anatoly Baksheev
-          */
-        template<class T> 
-        class PCL_EXPORTS DeviceArray : public DeviceMemory
-        {
-        public:
-            /** \brief Element type. */
-            using type = T;
-
-            /** \brief Element size. */
-            enum { elem_size = sizeof(T) };
-
-            /** \brief Empty constructor. */
-            DeviceArray();
-
-            /** \brief Allocates internal buffer in GPU memory
-              * \param size number of elements to allocate
-              * */
-            DeviceArray(std::size_t size);
-            
-            /** \brief Initializes with user allocated buffer. Reference counting is disabled in this case.
-              * \param ptr pointer to buffer
-              * \param size elements number
-              * */
-            DeviceArray(T *ptr, std::size_t size);
-
-            /** \brief Copy constructor. Just increments reference counter. */
-            DeviceArray(const DeviceArray& other);
-
-            /** \brief Assignment operator. Just increments reference counter. */
-            DeviceArray& operator = (const DeviceArray& other);
-
-            /** \brief Allocates internal buffer in GPU memory. If internal buffer was created before the function recreates it with new size. If new and old sizes are equal it does nothing.               
-              * \param size elements number
-              * */
-            void create(std::size_t size);
-
-            /** \brief Decrements reference counter and releases internal buffer if needed. */
-            void release();  
-
-            /** \brief Performs data copying. If destination size differs it will be reallocated.
-              * \param other destination container
-              * */
-            void copyTo(DeviceArray& other) const;
-
-            /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to ensure that intenal buffer size is enough.
-              * \param host_ptr pointer to buffer to upload               
-              * \param size elements number
-              * */
-            void upload(const T *host_ptr, std::size_t size);
-
-            /** \brief Downloads data from internal buffer to CPU memory
-              * \param host_ptr pointer to buffer to download               
-              * */
-            void download(T *host_ptr) const;
-            
-            /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to ensure that intenal buffer size is enough.
-              * \param data host vector to upload from              
-              * */
-            template<class A>
-            void upload(const std::vector<T, A>& data);
-
-             /** \brief Downloads data from internal buffer to CPU memory
-               * \param data  host vector to download to                 
-               * */
-            template<typename A>
-            void download(std::vector<T, A>& data) const;
-
-            /** \brief Performs swap of data pointed with another device array. 
-              * \param other_arg device array to swap with   
-              * */
-            void swap(DeviceArray& other_arg);
-
-            /** \brief Returns pointer for internal buffer in GPU memory. */
-            T* ptr(); 
-
-            /** \brief Returns const pointer for internal buffer in GPU memory. */
-            const T* ptr() const;
-
-            //using DeviceMemory::ptr;
-            
-            /** \brief Returns pointer for internal buffer in GPU memory. */
-            operator T*();
-
-            /** \brief Returns const pointer for internal buffer in GPU memory. */
-            operator const T*() const;
-
-            /** \brief Returns size in elements. */
-            std::size_t size() const;            
-        };
-        
-        
-        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-        /** \brief @b DeviceArray2D class
-          * 
-          * \note Typed container for pitched GPU memory with reference counting.          
-          *          
-          * \author Anatoly Baksheev
-          */
-        template<class T> 
-        class PCL_EXPORTS DeviceArray2D : public DeviceMemory2D
-        {
-        public:
-            /** \brief Element type. */
-            using type = T;
-
-            /** \brief Element size. */
-            enum { elem_size = sizeof(T) };
-
-            /** \brief Empty constructor. */
-            DeviceArray2D();
-            
-            /** \brief Allocates internal buffer in GPU memory
-              * \param rows number of rows to allocate
-              * \param cols number of elements in each row
-              * */
-            DeviceArray2D(int rows, int cols);
-
-             /** \brief Initializes with user allocated buffer. Reference counting is disabled in this case.
-              * \param rows number of rows
-              * \param cols number of elements in each row
-              * \param data pointer to buffer
-              * \param stepBytes stride between two consecutive rows in bytes
-              * */
-            DeviceArray2D(int rows, int cols, void *data, std::size_t stepBytes);
-
-            /** \brief Copy constructor. Just increments reference counter. */
-            DeviceArray2D(const DeviceArray2D& other);
-
-            /** \brief Assignment operator. Just increments reference counter. */
-            DeviceArray2D& operator = (const DeviceArray2D& other);
-
-            /** \brief Allocates internal buffer in GPU memory. If internal buffer was created before the function recreates it with new size. If new and old sizes are equal it does nothing.
-               * \param rows number of rows to allocate
-               * \param cols number of elements in each row
-               * */
-            void create(int rows, int cols);
-
-            /** \brief Decrements reference counter and releases internal buffer if needed. */
-            void release();
-            
-            /** \brief Performs data copying. If destination size differs it will be reallocated.
-              * \param other destination container
-              * */
-            void copyTo(DeviceArray2D& other) const;
-
-            /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to ensure that intenal buffer size is enough.
-              * \param host_ptr pointer to host buffer to upload               
-              * \param host_step stride between two consecutive rows in bytes for host buffer
-              * \param rows number of rows to upload
-              * \param cols number of elements in each row
-              * */
-            void upload(const void *host_ptr, std::size_t host_step, int rows, int cols);
-
-            /** \brief Downloads data from internal buffer to CPU memory. User is responsible for correct host buffer size.
-              * \param host_ptr pointer to host buffer to download               
-              * \param host_step stride between two consecutive rows in bytes for host buffer             
-              * */
-            void download(void *host_ptr, std::size_t host_step) const;
-
-            /** \brief Performs swap of data pointed with another device array. 
-              * \param other_arg device array to swap with   
-              * */
-            void swap(DeviceArray2D& other_arg);
-
-            /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to ensure that intenal buffer size is enough.
-              * \param data host vector to upload from              
-              * \param cols stride in elements between two consecutive rows for host buffer
-              * */
-            template<class A>
-            void upload(const std::vector<T, A>& data, int cols);
-
-            /** \brief Downloads data from internal buffer to CPU memory
-               * \param data host vector to download to                 
-               * \param cols Output stride in elements between two consecutive rows for host vector.
-               * */
-            template<class A>
-            void download(std::vector<T, A>& data, int& cols) const;
-                                      
-            /** \brief Returns pointer to given row in internal buffer. 
-              * \param y row index   
-              * */
-            T* ptr(int y = 0);             
-
-            /** \brief Returns const pointer to given row in internal buffer. 
-              * \param y row index   
-              * */
-            const T* ptr(int y = 0) const;            
-            
-            //using DeviceMemory2D::ptr;            
-
-            /** \brief Returns pointer for internal buffer in GPU memory. */
-            operator T*();
-
-            /** \brief Returns const pointer for internal buffer in GPU memory. */
-            operator const T*() const;                        
-            
-            /** \brief Returns number of elements in each row. */
-            int cols() const;
-
-            /** \brief Returns number of rows. */
-            int rows() const;
-
-            /** \brief Returns step in elements. */
-            std::size_t elem_step() const;
-        };        
-    }
-
-    namespace device
-    {
-        using pcl::gpu::DeviceArray;
-        using pcl::gpu::DeviceArray2D;
-    }
-}
+namespace pcl {
+namespace gpu {
+//////////////////////////////////////////////////////////////////////////////
+/** \brief @b DeviceArray class
+ *
+ * \note Typed container for GPU memory with reference counting.
+ *
+ * \author Anatoly Baksheev
+ */
+template <class T>
+class PCL_EXPORTS DeviceArray : public DeviceMemory {
+public:
+  /** \brief Element type. */
+  using type = T;
+
+  /** \brief Element size. */
+  enum { elem_size = sizeof(T) };
+
+  /** \brief Empty constructor. */
+  DeviceArray();
+
+  /** \brief Allocates internal buffer in GPU memory
+   * \param size number of elements to allocate
+   * */
+  DeviceArray(std::size_t size);
+
+  /** \brief Initializes with user allocated buffer. Reference counting is disabled in
+   * this case.
+   * \param ptr pointer to buffer
+   * \param size elements number
+   * */
+  DeviceArray(T* ptr, std::size_t size);
+
+  /** \brief Copy constructor. Just increments reference counter. */
+  DeviceArray(const DeviceArray& other);
+
+  /** \brief Assignment operator. Just increments reference counter. */
+  DeviceArray&
+  operator=(const DeviceArray& other);
+
+  /** \brief Allocates internal buffer in GPU memory. If internal buffer was created
+   * before the function recreates it with new size. If new and old sizes are equal it
+   * does nothing.
+   * \param size elements number
+   * */
+  void
+  create(std::size_t size);
+
+  /** \brief Decrements reference counter and releases internal buffer if needed. */
+  void
+  release();
+
+  /** \brief Performs data copying. If destination size differs it will be reallocated.
+   * \param other destination container
+   * */
+  void
+  copyTo(DeviceArray& other) const;
+
+  /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to
+   * ensure that intenal buffer size is enough.
+   * \param host_ptr pointer to buffer to upload
+   * \param size elements number
+   * */
+  void
+  upload(const T* host_ptr, std::size_t size);
+
+  /** \brief Uploads data from CPU memory to internal buffer.
+   * \return true if upload successful
+   * \note In contrast to the other upload function, this function
+   * never allocates memory.
+   * \param host_ptr pointer to buffer to upload
+   * \param device_begin_offset begin upload
+   * \param num_elements number of elements from device_bein_offset
+   * */
+  bool
+  upload(const T* host_ptr, std::size_t device_begin_offset, std::size_t num_elements);
+
+  /** \brief Downloads data from internal buffer to CPU memory
+   * \param host_ptr pointer to buffer to download
+   * */
+  void
+  download(T* host_ptr) const;
+
+  /** \brief Downloads data from internal buffer to CPU memory.
+   * \return true if download successful
+   * \param host_ptr pointer to buffer to download
+   * \param device_begin_offset begin download location
+   * \param num_elements number of elements from device_begin_offset
+   * */
+  bool
+  download(T* host_ptr,
+           std::size_t device_begin_offset,
+           std::size_t num_elements) const;
+
+  /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to
+   * ensure that intenal buffer size is enough.
+   * \param data host vector to upload from
+   * */
+  template <class A>
+  void
+  upload(const std::vector<T, A>& data);
+
+  /** \brief Downloads data from internal buffer to CPU memory
+   * \param data  host vector to download to
+   * */
+  template <typename A>
+  void
+  download(std::vector<T, A>& data) const;
+
+  /** \brief Performs swap of data pointed with another device array.
+   * \param other_arg device array to swap with
+   * */
+  void
+  swap(DeviceArray& other_arg);
+
+  /** \brief Returns pointer for internal buffer in GPU memory. */
+  T*
+  ptr();
+
+  /** \brief Returns const pointer for internal buffer in GPU memory. */
+  const T*
+  ptr() const;
+
+  // using DeviceMemory::ptr;
+
+  /** \brief Returns pointer for internal buffer in GPU memory. */
+  operator T*();
+
+  /** \brief Returns const pointer for internal buffer in GPU memory. */
+  operator const T*() const;
+
+  /** \brief Returns size in elements. */
+  std::size_t
+  size() const;
+};
+
+///////////////////////////////////////////////////////////////////////////////
+/** \brief @b DeviceArray2D class
+ *
+ * \note Typed container for pitched GPU memory with reference counting.
+ *
+ * \author Anatoly Baksheev
+ */
+template <class T>
+class PCL_EXPORTS DeviceArray2D : public DeviceMemory2D {
+public:
+  /** \brief Element type. */
+  using type = T;
+
+  /** \brief Element size. */
+  enum { elem_size = sizeof(T) };
+
+  /** \brief Empty constructor. */
+  DeviceArray2D();
+
+  /** \brief Allocates internal buffer in GPU memory
+   * \param rows number of rows to allocate
+   * \param cols number of elements in each row
+   * */
+  DeviceArray2D(int rows, int cols);
+
+  /** \brief Initializes with user allocated buffer. Reference counting is disabled in
+   * this case.
+   * \param rows number of rows
+   * \param cols number of elements in each row
+   * \param data pointer to buffer
+   * \param stepBytes stride between two consecutive rows in bytes
+   * */
+  DeviceArray2D(int rows, int cols, void* data, std::size_t stepBytes);
+
+  /** \brief Copy constructor. Just increments reference counter. */
+  DeviceArray2D(const DeviceArray2D& other);
+
+  /** \brief Assignment operator. Just increments reference counter. */
+  DeviceArray2D&
+  operator=(const DeviceArray2D& other);
+
+  /** \brief Allocates internal buffer in GPU memory. If internal buffer was created
+   * before the function recreates it with new size. If new and old sizes are equal it
+   * does nothing.
+   * \param rows number of rows to allocate
+   * \param cols number of elements in each row
+   * */
+  void
+  create(int rows, int cols);
+
+  /** \brief Decrements reference counter and releases internal buffer if needed. */
+  void
+  release();
+
+  /** \brief Performs data copying. If destination size differs it will be reallocated.
+   * \param other destination container
+   * */
+  void
+  copyTo(DeviceArray2D& other) const;
+
+  /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to
+   * ensure that intenal buffer size is enough.
+   * \param host_ptr pointer to host buffer to upload
+   * \param host_step stride between two consecutive rows in bytes for host buffer
+   * \param rows number of rows to upload
+   * \param cols number of elements in each row
+   * */
+  void
+  upload(const void* host_ptr, std::size_t host_step, int rows, int cols);
+
+  /** \brief Downloads data from internal buffer to CPU memory. User is responsible for
+   * correct host buffer size.
+   * \param host_ptr pointer to host buffer to download
+   * \param host_step stride between two consecutive rows in bytes for host buffer
+   * */
+  void
+  download(void* host_ptr, std::size_t host_step) const;
+
+  /** \brief Performs swap of data pointed with another device array.
+   * \param other_arg device array to swap with
+   * */
+  void
+  swap(DeviceArray2D& other_arg);
+
+  /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to
+   * ensure that intenal buffer size is enough.
+   * \param data host vector to upload from
+   * \param cols stride in elements between two consecutive rows for host buffer
+   * */
+  template <class A>
+  void
+  upload(const std::vector<T, A>& data, int cols);
+
+  /** \brief Downloads data from internal buffer to CPU memory
+   * \param data host vector to download to
+   * \param cols Output stride in elements between two consecutive rows for host vector.
+   * */
+  template <class A>
+  void
+  download(std::vector<T, A>& data, int& cols) const;
+
+  /** \brief Returns pointer to given row in internal buffer.
+   * \param y row index
+   * */
+  T*
+  ptr(int y = 0);
+
+  /** \brief Returns const pointer to given row in internal buffer.
+   * \param y row index
+   * */
+  const T*
+  ptr(int y = 0) const;
+
+  // using DeviceMemory2D::ptr;
+
+  /** \brief Returns pointer for internal buffer in GPU memory. */
+  operator T*();
+
+  /** \brief Returns const pointer for internal buffer in GPU memory. */
+  operator const T*() const;
+
+  /** \brief Returns number of elements in each row. */
+  int
+  cols() const;
+
+  /** \brief Returns number of rows. */
+  int
+  rows() const;
+
+  /** \brief Returns step in elements. */
+  std::size_t
+  elem_step() const;
+};
+} // namespace gpu
+
+namespace device {
+using pcl::gpu::DeviceArray;
+using pcl::gpu::DeviceArray2D;
+} // namespace device
+} // namespace pcl
 
 #include <pcl/gpu/containers/impl/device_array.hpp>
diff --git a/gpu/containers/include/pcl/gpu/containers/device_memory.h b/gpu/containers/include/pcl/gpu/containers/device_memory.h
index b7bbc2ee..c0833941 100644
--- a/gpu/containers/include/pcl/gpu/containers/device_memory.h
+++ b/gpu/containers/include/pcl/gpu/containers/device_memory.h
@@ -36,222 +36,286 @@
 
 #pragma once
 
-#include <pcl/pcl_exports.h>
 #include <pcl/gpu/containers/kernel_containers.h>
+#include <pcl/pcl_exports.h>
+
+namespace pcl {
+namespace gpu {
+///////////////////////////////////////////////////////////////////////////////
+/** \brief @b DeviceMemory class
+ *
+ * \note This is a BLOB container class with reference counting for GPU memory.
+ *
+ * \author Anatoly Baksheev
+ */
+
+class PCL_EXPORTS DeviceMemory {
+public:
+  /** \brief Empty constructor. */
+  DeviceMemory();
+
+  /** \brief Destructor. */
+  ~DeviceMemory();
+
+  /** \brief Allocates internal buffer in GPU memory
+   * \param sizeBytes_arg amount of memory to allocate
+   * */
+  DeviceMemory(std::size_t sizeBytes_arg);
+
+  /** \brief Initializes with user allocated buffer. Reference counting is disabled in
+   * this case.
+   * \param ptr_arg pointer to buffer
+   * \param sizeBytes_arg buffer size
+   * */
+  DeviceMemory(void* ptr_arg, std::size_t sizeBytes_arg);
+
+  /** \brief Copy constructor. Just increments reference counter. */
+  DeviceMemory(const DeviceMemory& other_arg);
+
+  /** \brief Assignment operator. Just increments reference counter. */
+  DeviceMemory&
+  operator=(const DeviceMemory& other_arg);
+
+  /** \brief Allocates internal buffer in GPU memory. If internal buffer was created
+   * before the function recreates it with new size. If new and old sizes are equal it
+   * does nothing.
+   * \param sizeBytes_arg buffer size
+   * */
+  void
+  create(std::size_t sizeBytes_arg);
+
+  /** \brief Decrements reference counter and releases internal buffer if needed. */
+  void
+  release();
+
+  /** \brief Performs data copying. If destination size differs it will be reallocated.
+   * \param other destination container
+   * */
+  void
+  copyTo(DeviceMemory& other) const;
+
+  /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to
+   * ensure that intenal buffer size is enough.
+   * \param host_ptr_arg pointer to buffer to upload
+   * \param sizeBytes_arg buffer size
+   * */
+  void
+  upload(const void* host_ptr_arg, std::size_t sizeBytes_arg);
+
+  /** \brief Uploads data from CPU memory to device array.
+   * \note This overload never allocates memory in contrast to the
+   * other upload function.
+   * \return true if upload successful
+   * \param host_ptr_arg pointer to buffer to upload
+   * \param device_begin_byte_offset first byte position to upload to
+   * \param num_bytes number of bytes to upload
+   * */
+  bool
+  upload(const void* host_ptr,
+         std::size_t device_begin_byte_offset,
+         std::size_t num_bytes);
+
+  /** \brief Downloads data from internal buffer to CPU memory
+   * \param host_ptr_arg pointer to buffer to download
+   * */
+  void
+  download(void* host_ptr_arg) const;
+
+  /** \brief Downloads data from internal buffer to CPU memory.
+   * \return true if download successful
+   * \param host_ptr_arg pointer to buffer to download
+   * \param device_begin_byte_offset first byte position to download
+   * \param num_bytes number of bytes to download
+   * */
+  bool
+  download(void* host_ptr,
+           std::size_t device_begin_byte_offset,
+           std::size_t num_bytes) const;
+
+  /** \brief Performs swap of data pointed with another device memory.
+   * \param other_arg device memory to swap with
+   * */
+  void
+  swap(DeviceMemory& other_arg);
+
+  /** \brief Returns pointer for internal buffer in GPU memory. */
+  template <class T>
+  T*
+  ptr();
+
+  /** \brief Returns constant pointer for internal buffer in GPU memory. */
+  template <class T>
+  const T*
+  ptr() const;
+
+  /** \brief Conversion to PtrSz for passing to kernel functions. */
+  template <class U>
+  operator PtrSz<U>() const;
+
+  /** \brief Returns true if unallocated otherwise false. */
+  bool
+  empty() const;
+
+  std::size_t
+  sizeBytes() const;
+
+private:
+  /** \brief Device pointer. */
+  void* data_;
+
+  /** \brief Allocated size in bytes. */
+  std::size_t sizeBytes_;
+
+  /** \brief Pointer to reference counter in CPU memory. */
+  int* refcount_;
+};
+
+///////////////////////////////////////////////////////////////////////////////
+/** \brief @b DeviceMemory2D class
+ *
+ * \note This is a BLOB container class with reference counting for pitched GPU memory.
+ *
+ * \author Anatoly Baksheev
+ */
 
-namespace pcl
-{
-    namespace gpu
-    {              
-        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-        /** \brief @b DeviceMemory class
-          * 
-          * \note This is a BLOB container class with reference counting for GPU memory.          
-          *          
-          * \author Anatoly Baksheev
-          */
-
-        class PCL_EXPORTS DeviceMemory
-        {
-        public:
-            /** \brief Empty constructor. */
-            DeviceMemory();
-
-            /** \brief Destructor. */
-            ~DeviceMemory();            
-
-            /** \brief Allocates internal buffer in GPU memory
-              * \param sizeBytes_arg amount of memory to allocate
-              * */
-            DeviceMemory(std::size_t sizeBytes_arg);
-
-            /** \brief Initializes with user allocated buffer. Reference counting is disabled in this case.
-              * \param ptr_arg pointer to buffer
-              * \param sizeBytes_arg buffer size
-              * */
-            DeviceMemory(void *ptr_arg, std::size_t sizeBytes_arg);
-
-            /** \brief Copy constructor. Just increments reference counter. */
-            DeviceMemory(const DeviceMemory& other_arg);
-
-            /** \brief Assignment operator. Just increments reference counter. */
-            DeviceMemory& operator=(const DeviceMemory& other_arg);
-
-             /** \brief Allocates internal buffer in GPU memory. If internal buffer was created before the function recreates it with new size. If new and old sizes are equal it does nothing.               
-               * \param sizeBytes_arg buffer size
-               * */
-            void create(std::size_t sizeBytes_arg);
-
-            /** \brief Decrements reference counter and releases internal buffer if needed. */
-            void release();
-
-            /** \brief Performs data copying. If destination size differs it will be reallocated.
-              * \param other destination container
-              * */
-            void copyTo(DeviceMemory& other) const;
-
-            /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to ensure that intenal buffer size is enough.
-              * \param host_ptr_arg pointer to buffer to upload               
-              * \param sizeBytes_arg buffer size
-              * */
-            void upload(const void *host_ptr_arg, std::size_t sizeBytes_arg);
-            
-            /** \brief Downloads data from internal buffer to CPU memory
-              * \param host_ptr_arg pointer to buffer to download               
-              * */
-            void download(void *host_ptr_arg) const;
-
-            /** \brief Performs swap of data pointed with another device memory. 
-              * \param other_arg device memory to swap with   
-              * */
-            void swap(DeviceMemory& other_arg);
-            
-            /** \brief Returns pointer for internal buffer in GPU memory. */
-            template<class T> T* ptr();
-
-            /** \brief Returns constant pointer for internal buffer in GPU memory. */            
-            template<class T> const T* ptr() const;
-
-            /** \brief Conversion to PtrSz for passing to kernel functions. */
-            template <class U> operator PtrSz<U>() const;            
-           
-            /** \brief Returns true if unallocated otherwise false. */
-            bool empty() const;
-            
-            std::size_t sizeBytes() const;
-        
-        private:
-            /** \brief Device pointer. */
-            void *data_;
-
-            /** \brief Allocated size in bytes. */
-            std::size_t sizeBytes_;
-        
-            /** \brief Pointer to reference counter in CPU memory. */
-            int* refcount_;
-        };
-
-        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-        /** \brief @b DeviceMemory2D class
-          * 
-          * \note This is a BLOB container class with reference counting for pitched GPU memory.          
-          *          
-          * \author Anatoly Baksheev
-          */
-
-        class PCL_EXPORTS DeviceMemory2D
-        {
-        public:
-            /** \brief Empty constructor. */
-            DeviceMemory2D();
-
-            /** \brief Destructor. */
-            ~DeviceMemory2D();            
-
-            /** \brief Allocates internal buffer in GPU memory
-              * \param rows_arg number of rows to allocate
-              * \param colsBytes_arg width of the buffer in bytes
-              * */
-            DeviceMemory2D(int rows_arg, int colsBytes_arg);
-
-
-            /** \brief Initializes with user allocated buffer. Reference counting is disabled in this case.
-              * \param rows_arg number of rows
-              * \param colsBytes_arg width of the buffer in bytes
-              * \param data_arg pointer to buffer
-              * \param step_arg stride between two consecutive rows in bytes
-              * */
-            DeviceMemory2D(int rows_arg, int colsBytes_arg, void *data_arg, std::size_t step_arg);
-
-            /** \brief Copy constructor. Just increments reference counter. */
-            DeviceMemory2D(const DeviceMemory2D& other_arg);
-
-            /** \brief Assignment operator. Just increments reference counter. */
-            DeviceMemory2D& operator=(const DeviceMemory2D& other_arg);
-
-            /** \brief Allocates internal buffer in GPU memory. If internal buffer was created before the function recreates it with new size. If new and old sizes are equal it does nothing.
-               * \param rows_arg number of rows to allocate
-               * \param colsBytes_arg width of the buffer in bytes
-               * */
-            void create(int rows_arg, int colsBytes_arg);
-
-            /** \brief Decrements reference counter and releases internal buffer if needed. */
-            void release();
-
-            /** \brief Performs data copying. If destination size differs it will be reallocated.
-              * \param other destination container
-              * */
-            void copyTo(DeviceMemory2D& other) const;
-
-            /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to ensure that intenal buffer size is enough.
-              * \param host_ptr_arg pointer to host buffer to upload               
-              * \param host_step_arg stride between two consecutive rows in bytes for host buffer
-              * \param rows_arg number of rows to upload
-              * \param colsBytes_arg width of host buffer in bytes
-              * */
-            void upload(const void *host_ptr_arg, std::size_t host_step_arg, int rows_arg, int colsBytes_arg);
-
-            /** \brief Downloads data from internal buffer to CPU memory. User is responsible for correct host buffer size.
-              * \param host_ptr_arg pointer to host buffer to download               
-              * \param host_step_arg stride between two consecutive rows in bytes for host buffer             
-              * */
-            void download(void *host_ptr_arg, std::size_t host_step_arg) const;
-
-            /** \brief Performs swap of data pointed with another device memory. 
-              * \param other_arg device memory to swap with   
-              * */
-            void swap(DeviceMemory2D& other_arg);
-            
-            /** \brief Returns pointer to given row in internal buffer. 
-              * \param y_arg row index   
-              * */
-            template<class T> T* ptr(int y_arg = 0);
-
-            /** \brief Returns constant pointer to given row in internal buffer. 
-              * \param y_arg row index   
-              * */
-            template<class T> const T* ptr(int y_arg = 0) const;
-
-             /** \brief Conversion to PtrStep for passing to kernel functions. */
-            template <class U> operator PtrStep<U>() const;            
-
-            /** \brief Conversion to PtrStepSz for passing to kernel functions. */
-            template <class U> operator PtrStepSz<U>() const;
-
-            /** \brief Returns true if unallocated otherwise false. */
-            bool empty() const;
-
-            /** \brief Returns number of bytes in each row. */
-            int colsBytes() const;
-
-            /** \brief Returns number of rows. */
-            int rows() const;
-
-            /** \brief Returns stride between two consecutive rows in bytes for internal buffer. Step is stored always and everywhere in bytes!!! */
-            std::size_t step() const;
-        private:
-            /** \brief Device pointer. */
-            void *data_;
-
-            /** \brief Stride between two consecutive rows in bytes for internal buffer. Step is stored always and everywhere in bytes!!! */
-            std::size_t step_;
-
-            /** \brief Width of the buffer in bytes. */
-            int colsBytes_;
-
-            /** \brief Number of rows. */
-            int rows_;
-
-            /** \brief Pointer to reference counter in CPU memory. */
-            int* refcount_;
-        };      
-    }
-
-    namespace device
-    {
-        using pcl::gpu::DeviceMemory;
-        using pcl::gpu::DeviceMemory2D;
-    }
-}
+class PCL_EXPORTS DeviceMemory2D {
+public:
+  /** \brief Empty constructor. */
+  DeviceMemory2D();
+
+  /** \brief Destructor. */
+  ~DeviceMemory2D();
+
+  /** \brief Allocates internal buffer in GPU memory
+   * \param rows_arg number of rows to allocate
+   * \param colsBytes_arg width of the buffer in bytes
+   * */
+  DeviceMemory2D(int rows_arg, int colsBytes_arg);
+
+  /** \brief Initializes with user allocated buffer. Reference counting is disabled in
+   * this case.
+   * \param rows_arg number of rows
+   * \param colsBytes_arg width of the buffer in bytes
+   * \param data_arg pointer to buffer
+   * \param step_arg stride between two consecutive rows in bytes
+   * */
+  DeviceMemory2D(int rows_arg, int colsBytes_arg, void* data_arg, std::size_t step_arg);
+
+  /** \brief Copy constructor. Just increments reference counter. */
+  DeviceMemory2D(const DeviceMemory2D& other_arg);
+
+  /** \brief Assignment operator. Just increments reference counter. */
+  DeviceMemory2D&
+  operator=(const DeviceMemory2D& other_arg);
+
+  /** \brief Allocates internal buffer in GPU memory. If internal buffer was created
+   * before the function recreates it with new size. If new and old sizes are equal it
+   * does nothing.
+   * \param rows_arg number of rows to allocate
+   * \param colsBytes_arg width of the buffer in bytes
+   * */
+  void
+  create(int rows_arg, int colsBytes_arg);
+
+  /** \brief Decrements reference counter and releases internal buffer if needed. */
+  void
+  release();
+
+  /** \brief Performs data copying. If destination size differs it will be reallocated.
+   * \param other destination container
+   * */
+  void
+  copyTo(DeviceMemory2D& other) const;
+
+  /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to
+   * ensure that intenal buffer size is enough.
+   * \param host_ptr_arg pointer to host buffer to upload
+   * \param host_step_arg stride between two consecutive rows in bytes for host buffer
+   * \param rows_arg number of rows to upload
+   * \param colsBytes_arg width of host buffer in bytes
+   * */
+  void
+  upload(const void* host_ptr_arg,
+         std::size_t host_step_arg,
+         int rows_arg,
+         int colsBytes_arg);
+
+  /** \brief Downloads data from internal buffer to CPU memory. User is responsible for
+   * correct host buffer size.
+   * \param host_ptr_arg pointer to host buffer to download
+   * \param host_step_arg stride between two consecutive rows in bytes for host buffer
+   * */
+  void
+  download(void* host_ptr_arg, std::size_t host_step_arg) const;
+
+  /** \brief Performs swap of data pointed with another device memory.
+   * \param other_arg device memory to swap with
+   * */
+  void
+  swap(DeviceMemory2D& other_arg);
+
+  /** \brief Returns pointer to given row in internal buffer.
+   * \param y_arg row index
+   * */
+  template <class T>
+  T*
+  ptr(int y_arg = 0);
+
+  /** \brief Returns constant pointer to given row in internal buffer.
+   * \param y_arg row index
+   * */
+  template <class T>
+  const T*
+  ptr(int y_arg = 0) const;
+
+  /** \brief Conversion to PtrStep for passing to kernel functions. */
+  template <class U>
+  operator PtrStep<U>() const;
+
+  /** \brief Conversion to PtrStepSz for passing to kernel functions. */
+  template <class U>
+  operator PtrStepSz<U>() const;
+
+  /** \brief Returns true if unallocated otherwise false. */
+  bool
+  empty() const;
+
+  /** \brief Returns number of bytes in each row. */
+  int
+  colsBytes() const;
+
+  /** \brief Returns number of rows. */
+  int
+  rows() const;
+
+  /** \brief Returns stride between two consecutive rows in bytes for internal buffer.
+   * Step is stored always and everywhere in bytes!!! */
+  std::size_t
+  step() const;
+
+private:
+  /** \brief Device pointer. */
+  void* data_;
+
+  /** \brief Stride between two consecutive rows in bytes for internal buffer. Step is
+   * stored always and everywhere in bytes!!! */
+  std::size_t step_;
+
+  /** \brief Width of the buffer in bytes. */
+  int colsBytes_;
+
+  /** \brief Number of rows. */
+  int rows_;
+
+  /** \brief Pointer to reference counter in CPU memory. */
+  int* refcount_;
+};
+} // namespace gpu
+
+namespace device {
+using pcl::gpu::DeviceMemory;
+using pcl::gpu::DeviceMemory2D;
+} // namespace device
+} // namespace pcl
 
 #include <pcl/gpu/containers/impl/device_memory.hpp>
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 cac65148..5ada5e07 100644
--- a/gpu/containers/include/pcl/gpu/containers/impl/device_array.hpp
+++ b/gpu/containers/include/pcl/gpu/containers/impl/device_array.hpp
@@ -34,92 +34,299 @@
  *  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 <class T>
+inline DeviceArray<T>::DeviceArray()
+{}
+
+template <class T>
+inline DeviceArray<T>::DeviceArray(std::size_t size) : DeviceMemory(size * elem_size)
+{}
+
+template <class T>
+inline DeviceArray<T>::DeviceArray(T* ptr, std::size_t size)
+: DeviceMemory(ptr, size * elem_size)
+{}
+
+template <class T>
+inline DeviceArray<T>::DeviceArray(const DeviceArray& other) : DeviceMemory(other)
+{}
+
+template <class T>
+inline DeviceArray<T>&
+DeviceArray<T>::operator=(const DeviceArray& other)
+{
+  DeviceMemory::operator=(other);
+  return *this;
+}
+
+template <class T>
+inline void
+DeviceArray<T>::create(std::size_t size)
+{
+  DeviceMemory::create(size * elem_size);
+}
+
+template <class T>
+inline void
+DeviceArray<T>::release()
+{
+  DeviceMemory::release();
+}
+
+template <class T>
+inline void
+DeviceArray<T>::copyTo(DeviceArray& other) const
+{
+  DeviceMemory::copyTo(other);
+}
+
+template <class T>
+inline void
+DeviceArray<T>::upload(const T* host_ptr, std::size_t size)
+{
+  DeviceMemory::upload(host_ptr, size * elem_size);
+}
+
+template <class T>
+inline bool
+DeviceArray<T>::upload(const T* host_ptr,
+                       std::size_t device_begin_offset,
+                       std::size_t num_elements)
+{
+  std::size_t begin_byte_offset = device_begin_offset * sizeof(T);
+  std::size_t num_bytes = num_elements * sizeof(T);
+  return DeviceMemory::upload(host_ptr, begin_byte_offset, num_bytes);
+}
+
+template <class T>
+inline void
+DeviceArray<T>::download(T* host_ptr) const
+{
+  DeviceMemory::download(host_ptr);
+}
+
+template <class T>
+inline bool
+DeviceArray<T>::download(T* host_ptr,
+                         std::size_t device_begin_offset,
+                         std::size_t num_elements) const
+{
+  std::size_t begin_byte_offset = device_begin_offset * sizeof(T);
+  std::size_t num_bytes = num_elements * sizeof(T);
+  return DeviceMemory::download(host_ptr, begin_byte_offset, num_bytes);
+}
+
+template <class T>
+void
+DeviceArray<T>::swap(DeviceArray& other_arg)
+{
+  DeviceMemory::swap(other_arg);
+}
+
+template <class T>
+inline DeviceArray<T>::operator T*()
+{
+  return ptr();
+}
+
+template <class T>
+inline DeviceArray<T>::operator const T*() const
+{
+  return ptr();
+}
 
-namespace pcl
+template <class T>
+inline std::size_t
+DeviceArray<T>::size() const
 {
+  return sizeBytes() / elem_size;
+}
 
-namespace gpu
+template <class T>
+inline T*
+DeviceArray<T>::ptr()
 {
+  return DeviceMemory::ptr<T>();
+}
 
-/////////////////////  Inline implementations of DeviceArray ////////////////////////////////////////////
+template <class T>
+inline const T*
+DeviceArray<T>::ptr() const
+{
+  return DeviceMemory::ptr<T>();
+}
 
-template<class T> inline DeviceArray<T>::DeviceArray() {}
-template<class T> inline DeviceArray<T>::DeviceArray(std::size_t size) : DeviceMemory(size * elem_size) {}
-template<class T> inline DeviceArray<T>::DeviceArray(T *ptr, std::size_t size) : DeviceMemory(ptr, size * elem_size) {}
-template<class T> inline DeviceArray<T>::DeviceArray(const DeviceArray& other) : DeviceMemory(other) {}
-template<class T> inline DeviceArray<T>& DeviceArray<T>::operator=(const DeviceArray& other)
-{ DeviceMemory::operator=(other); return *this; }
+template <class T>
+template <class A>
+inline void
+DeviceArray<T>::upload(const std::vector<T, A>& data)
+{
+  upload(&data[0], data.size());
+}
 
-template<class T> inline void DeviceArray<T>::create(std::size_t size)
-{ DeviceMemory::create(size * elem_size); }
-template<class T> inline void DeviceArray<T>::release()
-{ DeviceMemory::release(); }
+template <class T>
+template <class A>
+inline void
+DeviceArray<T>::download(std::vector<T, A>& data) const
+{
+  data.resize(size());
+  if (!data.empty())
+    download(&data[0]);
+}
 
-template<class T> inline void DeviceArray<T>::copyTo(DeviceArray& other) const
-{ DeviceMemory::copyTo(other); }
-template<class T> inline void DeviceArray<T>::upload(const T *host_ptr, std::size_t size)
-{ DeviceMemory::upload(host_ptr, size * elem_size); }
-template<class T> inline void DeviceArray<T>::download(T *host_ptr) const
-{ DeviceMemory::download( host_ptr ); }
+///////////////////  Inline implementations of DeviceArray2D //////////////////
 
-template<class T> void DeviceArray<T>::swap(DeviceArray& other_arg) { DeviceMemory::swap(other_arg); }
+template <class T>
+inline DeviceArray2D<T>::DeviceArray2D()
+{}
 
-template<class T> inline DeviceArray<T>::operator T*() { return ptr(); }
-template<class T> inline DeviceArray<T>::operator const T*() const { return ptr(); }
-template<class T> inline std::size_t DeviceArray<T>::size() const { return sizeBytes() / elem_size; }
+template <class T>
+inline DeviceArray2D<T>::DeviceArray2D(int rows, int cols)
+: DeviceMemory2D(rows, cols * elem_size)
+{}
 
-template<class T> inline       T* DeviceArray<T>::ptr()       { return DeviceMemory::ptr<T>(); }
-template<class T> inline const T* DeviceArray<T>::ptr() const { return DeviceMemory::ptr<T>(); }
+template <class T>
+inline DeviceArray2D<T>::DeviceArray2D(int rows,
+                                       int cols,
+                                       void* data,
+                                       std::size_t stepBytes)
+: DeviceMemory2D(rows, cols * elem_size, data, stepBytes)
+{}
 
-template<class T> template<class A> inline void DeviceArray<T>::upload(const std::vector<T, A>& data) { upload(&data[0], data.size()); }
-template<class T> template<class A> inline void DeviceArray<T>::download(std::vector<T, A>& data) const { data.resize(size()); if (!data.empty()) download(&data[0]); }
+template <class T>
+inline DeviceArray2D<T>::DeviceArray2D(const DeviceArray2D& other)
+: DeviceMemory2D(other)
+{}
 
-/////////////////////  Inline implementations of DeviceArray2D ////////////////////////////////////////////
+template <class T>
+inline DeviceArray2D<T>&
+DeviceArray2D<T>::operator=(const DeviceArray2D& other)
+{
+  DeviceMemory2D::operator=(other);
+  return *this;
+}
 
-template<class T> inline DeviceArray2D<T>::DeviceArray2D() {}
-template<class T> inline DeviceArray2D<T>::DeviceArray2D(int rows, int cols) : DeviceMemory2D(rows, cols * elem_size) {}
-template<class T> inline DeviceArray2D<T>::DeviceArray2D(int rows, int cols, void *data, std::size_t stepBytes) : DeviceMemory2D(rows, cols * elem_size, data, stepBytes) {}
-template<class T> inline DeviceArray2D<T>::DeviceArray2D(const DeviceArray2D& other) : DeviceMemory2D(other) {}
-template<class T> inline DeviceArray2D<T>& DeviceArray2D<T>::operator=(const DeviceArray2D& other)
-{ DeviceMemory2D::operator=(other); return *this; }
+template <class T>
+inline void
+DeviceArray2D<T>::create(int rows, int cols)
+{
+  DeviceMemory2D::create(rows, cols * elem_size);
+}
 
-template<class T> inline void DeviceArray2D<T>::create(int rows, int cols)
-{ DeviceMemory2D::create(rows, cols * elem_size); }
-template<class T> inline void DeviceArray2D<T>::release()
-{ DeviceMemory2D::release(); }
+template <class T>
+inline void
+DeviceArray2D<T>::release()
+{
+  DeviceMemory2D::release();
+}
 
-template<class T> inline void DeviceArray2D<T>::copyTo(DeviceArray2D& other) const
-{ DeviceMemory2D::copyTo(other); }
-template<class T> inline void DeviceArray2D<T>::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<class T> inline void DeviceArray2D<T>::download(void *host_ptr, std::size_t host_step) const
-{ DeviceMemory2D::download( host_ptr, host_step ); }
+template <class T>
+inline void
+DeviceArray2D<T>::copyTo(DeviceArray2D& other) const
+{
+  DeviceMemory2D::copyTo(other);
+}
 
-template<class T> template<class A> inline void DeviceArray2D<T>::upload(const std::vector<T, A>& data, int cols)
-{ upload(&data[0], cols * elem_size, data.size()/cols, cols); }
+template <class T>
+inline void
+DeviceArray2D<T>::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<class T> template<class A> inline void DeviceArray2D<T>::download(std::vector<T, A>& data, int& elem_step) const
-{ elem_step = cols(); data.resize(cols() * rows()); if (!data.empty()) download(&data[0], colsBytes());  }
+template <class T>
+inline void
+DeviceArray2D<T>::download(void* host_ptr, std::size_t host_step) const
+{
+  DeviceMemory2D::download(host_ptr, host_step);
+}
 
-template<class T> void  DeviceArray2D<T>::swap(DeviceArray2D& other_arg) { DeviceMemory2D::swap(other_arg); }
+template <class T>
+template <class A>
+inline void
+DeviceArray2D<T>::upload(const std::vector<T, A>& data, int cols)
+{
+  upload(&data[0], cols * elem_size, data.size() / cols, cols);
+}
 
-template<class T> inline       T* DeviceArray2D<T>::ptr(int y)       { return DeviceMemory2D::ptr<T>(y); }
-template<class T> inline const T* DeviceArray2D<T>::ptr(int y) const { return DeviceMemory2D::ptr<T>(y); }
+template <class T>
+template <class A>
+inline void
+DeviceArray2D<T>::download(std::vector<T, A>& data, int& elem_step) const
+{
+  elem_step = cols();
+  data.resize(cols() * rows());
+  if (!data.empty())
+    download(&data[0], colsBytes());
+}
 
-template<class T> inline DeviceArray2D<T>::operator T*() { return ptr(); }
-template<class T> inline DeviceArray2D<T>::operator const T*() const { return ptr(); }
+template <class T>
+void
+DeviceArray2D<T>::swap(DeviceArray2D& other_arg)
+{
+  DeviceMemory2D::swap(other_arg);
+}
 
-template<class T> inline int DeviceArray2D<T>::cols() const { return DeviceMemory2D::colsBytes()/elem_size; }
-template<class T> inline int DeviceArray2D<T>::rows() const { return DeviceMemory2D::rows(); }
+template <class T>
+inline T*
+DeviceArray2D<T>::ptr(int y)
+{
+  return DeviceMemory2D::ptr<T>(y);
+}
+
+template <class T>
+inline const T*
+DeviceArray2D<T>::ptr(int y) const
+{
+  return DeviceMemory2D::ptr<T>(y);
+}
 
-template<class T> inline std::size_t DeviceArray2D<T>::elem_step() const { return DeviceMemory2D::step()/elem_size; }
+template <class T>
+inline DeviceArray2D<T>::operator T*()
+{
+  return ptr();
+}
+
+template <class T>
+inline DeviceArray2D<T>::operator const T*() const
+{
+  return ptr();
+}
+
+template <class T>
+inline int
+DeviceArray2D<T>::cols() const
+{
+  return DeviceMemory2D::colsBytes() / elem_size;
+}
+
+template <class T>
+inline int
+DeviceArray2D<T>::rows() const
+{
+  return DeviceMemory2D::rows();
+}
+
+template <class T>
+inline std::size_t
+DeviceArray2D<T>::elem_step() const
+{
+  return DeviceMemory2D::step() / elem_size;
+}
 
 } // namespace gpu
 } // namespace pcl
 
 #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 265a8e82..b39fcd1c 100644
--- a/gpu/containers/include/pcl/gpu/containers/impl/device_memory.hpp
+++ b/gpu/containers/include/pcl/gpu/containers/impl/device_memory.hpp
@@ -37,65 +37,67 @@
 #ifndef PCL_GPU_CONTAINER_DEVICE_MEMORY_IMPL_HPP_
 #define PCL_GPU_CONTAINER_DEVICE_MEMORY_IMPL_HPP_
 
-namespace pcl
-{
+namespace pcl {
 
-namespace gpu
-{
+namespace gpu {
 
-/////////////////////  Inline implementations of DeviceMemory ////////////////////////////////////////////
-template<class T> inline T*
+////////////////////  Inline implementations of DeviceMemory //////////////////
+template <class T>
+inline T*
 DeviceMemory::ptr()
 {
   return (T*)data_;
 }
 
-template<class T> inline const T*
+template <class T>
+inline const T*
 DeviceMemory::ptr() const
 {
   return (const T*)data_;
 }
 
-template <class U> inline DeviceMemory::operator
-PtrSz<U>() const
+template <class U>
+inline DeviceMemory::operator PtrSz<U>() const
 {
-    PtrSz<U> result;
-    result.data = (U*)ptr<U>();
-    result.size = sizeBytes_/sizeof(U);
-    return result;
+  PtrSz<U> result;
+  result.data = (U*)ptr<U>();
+  result.size = sizeBytes_ / sizeof(U);
+  return result;
 }
 
-/////////////////////  Inline implementations of DeviceMemory2D ////////////////////////////////////////////
-template<class T> T*
+////////////////////  Inline implementations of DeviceMemory2D ////////////////
+template <class T>
+T*
 DeviceMemory2D::ptr(int y_arg)
 {
   return (T*)((char*)data_ + y_arg * step_);
 }
 
-template<class T> const T*
+template <class T>
+const T*
 DeviceMemory2D::ptr(int y_arg) const
 {
   return (const T*)((const char*)data_ + y_arg * step_);
 }
 
-template <class U> DeviceMemory2D::operator
-PtrStep<U>() const
+template <class U>
+DeviceMemory2D::operator PtrStep<U>() const
 {
-    PtrStep<U> result;
-    result.data = (U*)ptr<U>();
-    result.step = step_;
-    return result;
+  PtrStep<U> result;
+  result.data = (U*)ptr<U>();
+  result.step = step_;
+  return result;
 }
 
-template <class U> DeviceMemory2D::operator
-PtrStepSz<U>() const
+template <class U>
+DeviceMemory2D::operator PtrStepSz<U>() const
 {
-    PtrStepSz<U> result;
-    result.data = (U*)ptr<U>();
-    result.step = step_;
-    result.cols = colsBytes_/sizeof(U);
-    result.rows = rows_;
-    return result;
+  PtrStepSz<U> result;
+  result.data = (U*)ptr<U>();
+  result.step = step_;
+  result.cols = colsBytes_ / sizeof(U);
+  result.rows = rows_;
+  return result;
 }
 
 } // namespace gpu
diff --git a/gpu/containers/include/pcl/gpu/containers/initialization.h b/gpu/containers/include/pcl/gpu/containers/initialization.h
index 191e748a..d90517ca 100644
--- a/gpu/containers/include/pcl/gpu/containers/initialization.h
+++ b/gpu/containers/include/pcl/gpu/containers/initialization.h
@@ -37,37 +37,49 @@
 #pragma once
 
 #include <pcl/pcl_exports.h>
+
 #include <string>
 
-namespace pcl
-{
-    namespace gpu
-    {
-        /** \brief Returns number of Cuda device. */
-        PCL_EXPORTS int getCudaEnabledDeviceCount();
+namespace pcl {
+namespace gpu {
+/** \brief Returns number of Cuda device. */
+PCL_EXPORTS int
+getCudaEnabledDeviceCount();
 
-        /** \brief Sets active device to work with. */
-        PCL_EXPORTS void setDevice(int device);
+/** \brief Sets active device to work with. */
+PCL_EXPORTS void
+setDevice(int device);
 
-        /** \brief Return device name for given device. */
-        PCL_EXPORTS std::string getDeviceName(int device);
+/** \brief Return device name for given device. */
+PCL_EXPORTS std::string
+getDeviceName(int device);
 
-        /** \brief Prints information about given cuda device or about all devices
-         *  \param device: if < 0 prints info for all devices, otherwise the function interprets it as device id.
-         */
-        void PCL_EXPORTS printCudaDeviceInfo(int device = -1);
+/** \brief Prints information about given cuda device or about all devices
+ *  \param device: if < 0 prints info for all devices, otherwise the function interprets
+ * it as device id.
+ */
+void PCL_EXPORTS
+printCudaDeviceInfo(int device = -1);
 
-        /** \brief Prints information about given cuda device or about all devices
-         *  \param device: if < 0 prints info for all devices, otherwise the function interprets it as device id.
-         */
-        void PCL_EXPORTS printShortCudaDeviceInfo(int device = -1);
+/** \brief Prints information about given cuda device or about all devices
+ *  \param device: if < 0 prints info for all devices, otherwise the function interprets
+ * it as device id.
+ */
+void PCL_EXPORTS
+printShortCudaDeviceInfo(int device = -1);
 
-        /** \brief Returns true if pre-Fermi generator GPU. 
-          * \param device: device id to check, if < 0 checks current device.
-          */
-        bool PCL_EXPORTS checkIfPreFermiGPU(int device = -1);
+/** \brief Returns true if pre-Fermi generator GPU.
+ * \param device: device id to check, if < 0 checks current device.
+ */
+bool PCL_EXPORTS
+checkIfPreFermiGPU(int device = -1);
 
-        /** \brief Error handler. All GPU functions call this to report an error. For internal use only */
-        void PCL_EXPORTS error(const char *error_string, const char *file, const int line, const char *func = "");        
-    }
-}
+/** \brief Error handler. All GPU functions call this to report an error. For internal
+ * use only */
+void PCL_EXPORTS
+error(const char* error_string,
+      const char* file,
+      const int line,
+      const char* func = "");
+} // namespace gpu
+} // namespace pcl
diff --git a/gpu/containers/include/pcl/gpu/containers/kernel_containers.h b/gpu/containers/include/pcl/gpu/containers/kernel_containers.h
index 998dba64..3398d518 100644
--- a/gpu/containers/include/pcl/gpu/containers/kernel_containers.h
+++ b/gpu/containers/include/pcl/gpu/containers/kernel_containers.h
@@ -36,73 +36,108 @@
 
 #pragma once
 
-#if defined(__CUDACC__) 
-    #define __PCL_GPU_HOST_DEVICE__ __host__ __device__ __forceinline__ 
+#if defined(__CUDACC__)
+#define __PCL_GPU_HOST_DEVICE__ __host__ __device__ __forceinline__
 #else
-    #define __PCL_GPU_HOST_DEVICE__
-#endif  
+#define __PCL_GPU_HOST_DEVICE__
+#endif
 
 #include <cstddef>
 
-namespace pcl
-{
-    namespace gpu
-    {
-        template<typename T> struct DevPtr
-        {
-            using elem_type = T;
-            const static std::size_t elem_size = sizeof(elem_type);
-
-            T* data;
-
-            __PCL_GPU_HOST_DEVICE__ DevPtr() : data(nullptr) {}
-            __PCL_GPU_HOST_DEVICE__ DevPtr(T* data_arg) : data(data_arg) {}
-
-            __PCL_GPU_HOST_DEVICE__ std::size_t elemSize() const { return elem_size; }
-            __PCL_GPU_HOST_DEVICE__ operator       T*()       { return data; }
-            __PCL_GPU_HOST_DEVICE__ operator const T*() const { return data; }
-        };
-
-        template<typename T> struct PtrSz : public DevPtr<T>
-        {                     
-            __PCL_GPU_HOST_DEVICE__ PtrSz() : size(0) {}
-            __PCL_GPU_HOST_DEVICE__ PtrSz(T* data_arg, std::size_t size_arg) : DevPtr<T>(data_arg), size(size_arg) {}
-
-            std::size_t size;
-        };
-
-        template<typename T>  struct PtrStep : public DevPtr<T>
-        {   
-            __PCL_GPU_HOST_DEVICE__ PtrStep() : step(0) {}
-            __PCL_GPU_HOST_DEVICE__ PtrStep(T* data_arg, std::size_t step_arg) : DevPtr<T>(data_arg), step(step_arg) {}
-
-            /** \brief stride between two consecutive rows in bytes. Step is stored always and everywhere in bytes!!! */
-            std::size_t step;            
-
-            __PCL_GPU_HOST_DEVICE__       T* ptr(int y = 0)       { return (      T*)( (      char*)DevPtr<T>::data + y * step); }
-            __PCL_GPU_HOST_DEVICE__ const T* ptr(int y = 0) const { return (const T*)( (const char*)DevPtr<T>::data + y * step); }
-
-            __PCL_GPU_HOST_DEVICE__       T& operator()(int y, int x)       { return ptr(y)[x]; }
-            __PCL_GPU_HOST_DEVICE__ const T& operator()(int y, int x) const { return ptr(y)[x]; }
-        };
-
-        template <typename T> struct PtrStepSz : public PtrStep<T>
-        {   
-            __PCL_GPU_HOST_DEVICE__ PtrStepSz() : cols(0), rows(0) {}
-            __PCL_GPU_HOST_DEVICE__ PtrStepSz(int rows_arg, int cols_arg, T* data_arg, std::size_t step_arg) 
-                : PtrStep<T>(data_arg, step_arg), cols(cols_arg), rows(rows_arg) {}
-
-            int cols;
-            int rows;                                                                              
-        };
-    }
-
-    namespace device
-    {
-        using pcl::gpu::PtrSz;
-        using pcl::gpu::PtrStep;
-        using pcl::gpu::PtrStepSz;
-    }
-}
+namespace pcl {
+namespace gpu {
+template <typename T>
+struct DevPtr {
+  using elem_type = T;
+  const static std::size_t elem_size = sizeof(elem_type);
+
+  T* data;
+
+  __PCL_GPU_HOST_DEVICE__
+  DevPtr() : data(nullptr) {}
+
+  __PCL_GPU_HOST_DEVICE__
+  DevPtr(T* data_arg) : data(data_arg) {}
+
+  __PCL_GPU_HOST_DEVICE__ std::size_t
+  elemSize() const
+  {
+    return elem_size;
+  }
+
+  __PCL_GPU_HOST_DEVICE__
+  operator T*() { return data; }
+  __PCL_GPU_HOST_DEVICE__ operator const T*() const { return data; }
+};
+
+template <typename T>
+struct PtrSz : public DevPtr<T> {
+  __PCL_GPU_HOST_DEVICE__
+  PtrSz() : size(0) {}
+
+  __PCL_GPU_HOST_DEVICE__
+  PtrSz(T* data_arg, std::size_t size_arg) : DevPtr<T>(data_arg), size(size_arg) {}
+
+  std::size_t size;
+};
+
+template <typename T>
+struct PtrStep : public DevPtr<T> {
+  __PCL_GPU_HOST_DEVICE__
+  PtrStep() : step(0) {}
+
+  __PCL_GPU_HOST_DEVICE__
+  PtrStep(T* data_arg, std::size_t step_arg) : DevPtr<T>(data_arg), step(step_arg) {}
+
+  /** \brief stride between two consecutive rows in bytes. Step is stored always and
+   * everywhere in bytes!!! */
+  std::size_t step;
+
+  __PCL_GPU_HOST_DEVICE__ T*
+  ptr(int y = 0)
+  {
+    return (T*)((char*)DevPtr<T>::data + y * step);
+  }
+
+  __PCL_GPU_HOST_DEVICE__ const T*
+  ptr(int y = 0) const
+  {
+    return (const T*)((const char*)DevPtr<T>::data + y * step);
+  }
+
+  __PCL_GPU_HOST_DEVICE__ T&
+  operator()(int y, int x)
+  {
+    return ptr(y)[x];
+  }
+
+  __PCL_GPU_HOST_DEVICE__ const T&
+  operator()(int y, int x) const
+  {
+    return ptr(y)[x];
+  }
+};
+
+template <typename T>
+struct PtrStepSz : public PtrStep<T> {
+  __PCL_GPU_HOST_DEVICE__
+  PtrStepSz() : cols(0), rows(0) {}
+
+  __PCL_GPU_HOST_DEVICE__
+  PtrStepSz(int rows_arg, int cols_arg, T* data_arg, std::size_t step_arg)
+  : PtrStep<T>(data_arg, step_arg), cols(cols_arg), rows(rows_arg)
+  {}
+
+  int cols;
+  int rows;
+};
+} // namespace gpu
+
+namespace device {
+using pcl::gpu::PtrStep;
+using pcl::gpu::PtrStepSz;
+using pcl::gpu::PtrSz;
+} // namespace device
+} // namespace pcl
 
 #undef __PCL_GPU_HOST_DEVICE__
diff --git a/gpu/containers/src/device_memory.cpp b/gpu/containers/src/device_memory.cpp
index 9f464e8e..065dab7f 100644
--- a/gpu/containers/src/device_memory.cpp
+++ b/gpu/containers/src/device_memory.cpp
@@ -1,4 +1,4 @@
- /*
+/*
  * Software License Agreement (BSD License)
  *
  *  Copyright (c) 2011, Willow Garage, Inc.
@@ -38,6 +38,7 @@
 #include <pcl/gpu/utils/safe_call.hpp>
 
 #include <cuda_runtime_api.h>
+
 #include <cassert>
 
 #define HAVE_CUDA
@@ -45,268 +46,468 @@
 
 #if !defined(HAVE_CUDA)
 
-void throw_nogpu() { throw "PCL 2.0 exception"; }
+void
+throw_nogpu()
+{
+  throw "PCL 2.0 exception";
+}
 
 pcl::gpu::DeviceMemory::DeviceMemory() { throw_nogpu(); }
-pcl::gpu::DeviceMemory::DeviceMemory(void *, std::size_t) { throw_nogpu(); }
+
+pcl::gpu::DeviceMemory::DeviceMemory(void*, std::size_t) { throw_nogpu(); }
+
 pcl::gpu::DeviceMemory::DeviceMemory(std::size_t) { throw_nogpu(); }
+
 pcl::gpu::DeviceMemory::~DeviceMemory() { throw_nogpu(); }
-pcl::gpu::DeviceMemory::DeviceMemory(const DeviceMemory& ) { throw_nogpu(); }
-pcl::gpu::DeviceMemory& pcl::gpu::DeviceMemory::operator=(const pcl::gpu::DeviceMemory&) { throw_nogpu(); return *this;}
+
+pcl::gpu::DeviceMemory::DeviceMemory(const DeviceMemory&) { throw_nogpu(); }
+
+pcl::gpu::DeviceMemory&
+
+pcl::gpu::DeviceMemory::operator=(const pcl::gpu::DeviceMemory&)
+{
+  throw_nogpu();
+  return *this;
+}
+
 void pcl::gpu::DeviceMemory::create(std::size_t) { throw_nogpu(); }
-void pcl::gpu::DeviceMemory::release() { throw_nogpu(); }
-void pcl::gpu::DeviceMemory::copyTo(DeviceMemory&) const { throw_nogpu(); }
-void pcl::gpu::DeviceMemory::upload(const void*, std::size_t) { throw_nogpu(); }
-void pcl::gpu::DeviceMemory::download(void*) const { throw_nogpu(); }
-bool pcl::gpu::DeviceMemory::empty() const { throw_nogpu(); }
+
+void
+pcl::gpu::DeviceMemory::release()
+{
+  throw_nogpu();
+}
+
+void
+pcl::gpu::DeviceMemory::copyTo(DeviceMemory&) const
+{
+  throw_nogpu();
+}
+
+void
+pcl::gpu::DeviceMemory::upload(const void*, std::size_t)
+{
+  throw_nogpu();
+}
+
+void
+pcl::gpu::DeviceMemory::download(void*) const
+{
+  throw_nogpu();
+}
+
+bool
+pcl::gpu::DeviceMemory::empty() const
+{
+  throw_nogpu();
+}
+
 pcl::gpu::DeviceMemory2D::DeviceMemory2D() { throw_nogpu(); }
-pcl::gpu::DeviceMemory2D::DeviceMemory2D(int, int)  { throw_nogpu(); }
-pcl::gpu::DeviceMemory2D::DeviceMemory2D(int, int, void*, std::size_t)  { throw_nogpu(); }
+
+pcl::gpu::DeviceMemory2D::DeviceMemory2D(int, int) { throw_nogpu(); }
+
+pcl::gpu::DeviceMemory2D::DeviceMemory2D(int, int, void*, std::size_t)
+{
+  throw_nogpu();
+}
+
 pcl::gpu::DeviceMemory2D::~DeviceMemory2D() { throw_nogpu(); }
-pcl::gpu::DeviceMemory2D::DeviceMemory2D(const DeviceMemory2D&)  { throw_nogpu(); }
-pcl::gpu::DeviceMemory2D& pcl::gpu::DeviceMemory2D::operator=(const pcl::gpu::DeviceMemory2D&) { throw_nogpu(); return *this;}
-void pcl::gpu::DeviceMemory2D::create(int, int )  { throw_nogpu(); }
-void pcl::gpu::DeviceMemory2D::release()  { throw_nogpu(); }
-void pcl::gpu::DeviceMemory2D::copyTo(DeviceMemory2D&) const  { throw_nogpu(); }
-void pcl::gpu::DeviceMemory2D::upload(const void *, std::size_t, int, int )  { throw_nogpu(); }
-void pcl::gpu::DeviceMemory2D::download(void *, std::size_t ) const  { throw_nogpu(); }
-bool pcl::gpu::DeviceMemory2D::empty() const { throw_nogpu(); }
+
+pcl::gpu::DeviceMemory2D::DeviceMemory2D(const DeviceMemory2D&) { throw_nogpu(); }
+
+pcl::gpu::DeviceMemory2D&
+
+pcl::gpu::DeviceMemory2D::operator=(const pcl::gpu::DeviceMemory2D&)
+{
+  throw_nogpu();
+  return *this;
+}
+
+void
+pcl::gpu::DeviceMemory2D::create(int, int)
+{
+  throw_nogpu();
+}
+
+void
+pcl::gpu::DeviceMemory2D::release()
+{
+  throw_nogpu();
+}
+
+void
+pcl::gpu::DeviceMemory2D::copyTo(DeviceMemory2D&) const
+{
+  throw_nogpu();
+}
+
+void
+pcl::gpu::DeviceMemory2D::upload(const void*, std::size_t, int, int)
+{
+  throw_nogpu();
+}
+
+void
+pcl::gpu::DeviceMemory2D::download(void*, std::size_t) const
+{
+  throw_nogpu();
+}
+
+bool
+pcl::gpu::DeviceMemory2D::empty() const
+{
+  throw_nogpu();
+}
 
 #else
 
 //////////////////////////    XADD    ///////////////////////////////
 
 #ifdef __GNUC__
-  #if !defined WIN32 && (defined __i486__ || defined __i586__ || defined __i686__ || defined __MMX__ || defined __SSE__  || defined __ppc__)
-    #define CV_XADD __sync_fetch_and_add
-  #else
-    #include <ext/atomicity.h>
-    #define CV_XADD __gnu_cxx::__exchange_and_add
-  #endif
+#if !defined WIN32 && (defined __i486__ || defined __i586__ || defined __i686__ ||     \
+                       defined __MMX__ || defined __SSE__ || defined __ppc__)
+#define CV_XADD __sync_fetch_and_add
+#else
+#include <ext/atomicity.h>
+#define CV_XADD __gnu_cxx::__exchange_and_add
+#endif
 #elif defined WIN32 || defined _WIN32
-    #include <intrin.h>
-    #define CV_XADD(addr,delta) _InterlockedExchangeAdd((long volatile*)(addr), (delta))
+#include <intrin.h>
+#define CV_XADD(addr, delta) _InterlockedExchangeAdd((long volatile*)(addr), (delta))
 #else
 
-    template<typename _Tp> static inline _Tp CV_XADD(_Tp* addr, _Tp delta)
-    { int tmp = *addr; *addr += delta; return tmp; }
-    
+template <typename _Tp>
+static inline _Tp
+CV_XADD(_Tp* addr, _Tp delta)
+{
+  int tmp = *addr;
+  *addr += delta;
+  return tmp;
+}
+
 #endif
 
 ////////////////////////    DeviceArray    /////////////////////////////
-    
-pcl::gpu::DeviceMemory::DeviceMemory() : data_(nullptr), sizeBytes_(0), refcount_(nullptr) {}
-pcl::gpu::DeviceMemory::DeviceMemory(void *ptr_arg, std::size_t sizeBytes_arg) : data_(ptr_arg), sizeBytes_(sizeBytes_arg), refcount_(nullptr){}
-pcl::gpu::DeviceMemory::DeviceMemory(std::size_t sizeBtes_arg)  : data_(nullptr), sizeBytes_(0), refcount_(nullptr) { create(sizeBtes_arg); }
+
+pcl::gpu::DeviceMemory::DeviceMemory()
+: data_(nullptr), sizeBytes_(0), refcount_(nullptr)
+{}
+
+pcl::gpu::DeviceMemory::DeviceMemory(void* ptr_arg, std::size_t sizeBytes_arg)
+: data_(ptr_arg), sizeBytes_(sizeBytes_arg), refcount_(nullptr)
+{}
+
+pcl::gpu::DeviceMemory::DeviceMemory(std::size_t sizeBtes_arg)
+: data_(nullptr), sizeBytes_(0), refcount_(nullptr)
+{
+  create(sizeBtes_arg);
+}
+
 pcl::gpu::DeviceMemory::~DeviceMemory() { release(); }
 
-pcl::gpu::DeviceMemory::DeviceMemory(const DeviceMemory& other_arg) 
-    : data_(other_arg.data_), sizeBytes_(other_arg.sizeBytes_), refcount_(other_arg.refcount_)
+pcl::gpu::DeviceMemory::DeviceMemory(const DeviceMemory& other_arg)
+: data_(other_arg.data_)
+, sizeBytes_(other_arg.sizeBytes_)
+, refcount_(other_arg.refcount_)
 {
-    if( refcount_ )
-        CV_XADD(refcount_, 1);
+  if (refcount_)
+    CV_XADD(refcount_, 1);
 }
 
-pcl::gpu::DeviceMemory& pcl::gpu::DeviceMemory::operator = (const pcl::gpu::DeviceMemory& other_arg)
+pcl::gpu::DeviceMemory&
+pcl::gpu::DeviceMemory::operator=(const pcl::gpu::DeviceMemory& other_arg)
 {
-    if( this != &other_arg )
-    {
-        if( other_arg.refcount_ )
-            CV_XADD(other_arg.refcount_, 1);
-        release();
-        
-        data_      = other_arg.data_;
-        sizeBytes_ = other_arg.sizeBytes_;                
-        refcount_  = other_arg.refcount_;
-    }
-    return *this;
+  if (this != &other_arg) {
+    if (other_arg.refcount_)
+      CV_XADD(other_arg.refcount_, 1);
+    release();
+
+    data_ = other_arg.data_;
+    sizeBytes_ = other_arg.sizeBytes_;
+    refcount_ = other_arg.refcount_;
+  }
+  return *this;
 }
 
-void pcl::gpu::DeviceMemory::create(std::size_t sizeBytes_arg)
+void
+pcl::gpu::DeviceMemory::create(std::size_t sizeBytes_arg)
 {
-    if (sizeBytes_arg == sizeBytes_)
-        return;
-            
-    if( sizeBytes_arg > 0)
-    {        
-        if( data_ )
-            release();
+  if (sizeBytes_arg == sizeBytes_)
+    return;
+
+  if (sizeBytes_arg > 0) {
+    if (data_)
+      release();
+
+    sizeBytes_ = sizeBytes_arg;
 
-        sizeBytes_ = sizeBytes_arg;
-                        
-        cudaSafeCall( cudaMalloc(&data_, sizeBytes_) );
-        
-        //refcount_ = (int*)cv::fastMalloc(sizeof(*refcount_));
-        refcount_ = new int;
-        *refcount_ = 1;
-    }
+    cudaSafeCall(cudaMalloc(&data_, sizeBytes_));
+
+    // refcount_ = (int*)cv::fastMalloc(sizeof(*refcount_));
+    refcount_ = new int;
+    *refcount_ = 1;
+  }
 }
 
-void pcl::gpu::DeviceMemory::copyTo(DeviceMemory& other) const
+void
+pcl::gpu::DeviceMemory::copyTo(DeviceMemory& other) const
 {
-    if (empty())
-        other.release();
-    else
-    {    
-        other.create(sizeBytes_);    
-        cudaSafeCall( cudaMemcpy(other.data_, data_, sizeBytes_, cudaMemcpyDeviceToDevice) );
-        cudaSafeCall( cudaDeviceSynchronize() );
-    }
+  if (empty())
+    other.release();
+  else {
+    other.create(sizeBytes_);
+    cudaSafeCall(cudaMemcpy(other.data_, data_, sizeBytes_, cudaMemcpyDeviceToDevice));
+    cudaSafeCall(cudaDeviceSynchronize());
+  }
 }
 
-void pcl::gpu::DeviceMemory::release()
+void
+pcl::gpu::DeviceMemory::release()
 {
-    if( refcount_ && CV_XADD(refcount_, -1) == 1 )
-    {
-        //cv::fastFree(refcount);
-        delete refcount_;
-        cudaSafeCall( cudaFree(data_) );
-    }
-    data_ = nullptr;
-    sizeBytes_ = 0;
-    refcount_ = nullptr;
+  if (refcount_ && CV_XADD(refcount_, -1) == 1) {
+    // cv::fastFree(refcount);
+    delete refcount_;
+    cudaSafeCall(cudaFree(data_));
+  }
+  data_ = nullptr;
+  sizeBytes_ = 0;
+  refcount_ = nullptr;
 }
 
-void pcl::gpu::DeviceMemory::upload(const void *host_ptr_arg, std::size_t sizeBytes_arg)
+void
+pcl::gpu::DeviceMemory::upload(const void* host_ptr_arg, std::size_t sizeBytes_arg)
 {
-    create(sizeBytes_arg);
-    cudaSafeCall( cudaMemcpy(data_, host_ptr_arg, sizeBytes_, cudaMemcpyHostToDevice) );
-    cudaSafeCall( cudaDeviceSynchronize() );
+  create(sizeBytes_arg);
+  cudaSafeCall(cudaMemcpy(data_, host_ptr_arg, sizeBytes_, cudaMemcpyHostToDevice));
+  cudaSafeCall(cudaDeviceSynchronize());
 }
 
-void pcl::gpu::DeviceMemory::download(void *host_ptr_arg) const
-{    
-    cudaSafeCall( cudaMemcpy(host_ptr_arg, data_, sizeBytes_, cudaMemcpyDeviceToHost) );
-    cudaSafeCall( cudaDeviceSynchronize() );
-}          
+bool
+pcl::gpu::DeviceMemory::upload(const void* host_ptr_arg,
+                               std::size_t device_begin_byte_offset,
+                               std::size_t num_bytes)
+{
+  if (device_begin_byte_offset + num_bytes > sizeBytes_) {
+    return false;
+  }
+  void* begin = static_cast<char*>(data_) + device_begin_byte_offset;
+  cudaSafeCall(cudaMemcpy(begin, host_ptr_arg, num_bytes, cudaMemcpyHostToDevice));
+  cudaSafeCall(cudaDeviceSynchronize());
+  return true;
+}
 
-void pcl::gpu::DeviceMemory::swap(DeviceMemory& other_arg)
+void
+pcl::gpu::DeviceMemory::download(void* host_ptr_arg) const
 {
-    std::swap(data_, other_arg.data_);
-    std::swap(sizeBytes_, other_arg.sizeBytes_);
-    std::swap(refcount_, other_arg.refcount_);
+  cudaSafeCall(cudaMemcpy(host_ptr_arg, data_, sizeBytes_, cudaMemcpyDeviceToHost));
+  cudaSafeCall(cudaDeviceSynchronize());
 }
 
-bool pcl::gpu::DeviceMemory::empty() const { return !data_; }
-size_t pcl::gpu::DeviceMemory::sizeBytes() const { return sizeBytes_; }
+bool
+pcl::gpu::DeviceMemory::download(void* host_ptr_arg,
+                                 std::size_t device_begin_byte_offset,
+                                 std::size_t num_bytes) const
+{
+  if (device_begin_byte_offset + num_bytes > sizeBytes_) {
+    return false;
+  }
+  const void* begin = static_cast<char*>(data_) + device_begin_byte_offset;
+  cudaSafeCall(cudaMemcpy(host_ptr_arg, begin, num_bytes, cudaMemcpyDeviceToHost));
+  cudaSafeCall(cudaDeviceSynchronize());
+  return true;
+}
 
+void
+pcl::gpu::DeviceMemory::swap(DeviceMemory& other_arg)
+{
+  std::swap(data_, other_arg.data_);
+  std::swap(sizeBytes_, other_arg.sizeBytes_);
+  std::swap(refcount_, other_arg.refcount_);
+}
+
+bool
+pcl::gpu::DeviceMemory::empty() const
+{
+  return !data_;
+}
+size_t
+pcl::gpu::DeviceMemory::sizeBytes() const
+{
+  return sizeBytes_;
+}
 
 ////////////////////////    DeviceArray2D    /////////////////////////////
 
-pcl::gpu::DeviceMemory2D::DeviceMemory2D() : data_(nullptr), step_(0), colsBytes_(0), rows_(0), refcount_(nullptr) {}
+pcl::gpu::DeviceMemory2D::DeviceMemory2D()
+: data_(nullptr), step_(0), colsBytes_(0), rows_(0), refcount_(nullptr)
+{}
 
-pcl::gpu::DeviceMemory2D::DeviceMemory2D(int rows_arg, int colsBytes_arg) 
-    : data_(nullptr), step_(0), colsBytes_(0), rows_(0), refcount_(nullptr)
-{ 
-    create(rows_arg, colsBytes_arg); 
+pcl::gpu::DeviceMemory2D::DeviceMemory2D(int rows_arg, int colsBytes_arg)
+: data_(nullptr), step_(0), colsBytes_(0), rows_(0), refcount_(nullptr)
+{
+  create(rows_arg, colsBytes_arg);
 }
 
-pcl::gpu::DeviceMemory2D::DeviceMemory2D(int rows_arg, int colsBytes_arg, void *data_arg, std::size_t step_arg) 
-    :  data_(data_arg), step_(step_arg), colsBytes_(colsBytes_arg), rows_(rows_arg), refcount_(nullptr) {}
+pcl::gpu::DeviceMemory2D::DeviceMemory2D(int rows_arg,
+                                         int colsBytes_arg,
+                                         void* data_arg,
+                                         std::size_t step_arg)
+: data_(data_arg)
+, step_(step_arg)
+, colsBytes_(colsBytes_arg)
+, rows_(rows_arg)
+, refcount_(nullptr)
+{}
 
 pcl::gpu::DeviceMemory2D::~DeviceMemory2D() { release(); }
 
-
-pcl::gpu::DeviceMemory2D::DeviceMemory2D(const DeviceMemory2D& other_arg) : 
-    data_(other_arg.data_), step_(other_arg.step_), colsBytes_(other_arg.colsBytes_), rows_(other_arg.rows_), refcount_(other_arg.refcount_)
+pcl::gpu::DeviceMemory2D::DeviceMemory2D(const DeviceMemory2D& other_arg)
+: data_(other_arg.data_)
+, step_(other_arg.step_)
+, colsBytes_(other_arg.colsBytes_)
+, rows_(other_arg.rows_)
+, refcount_(other_arg.refcount_)
 {
-    if( refcount_ )
-        CV_XADD(refcount_, 1);
+  if (refcount_)
+    CV_XADD(refcount_, 1);
 }
 
-pcl::gpu::DeviceMemory2D& pcl::gpu::DeviceMemory2D::operator = (const pcl::gpu::DeviceMemory2D& other_arg)
+pcl::gpu::DeviceMemory2D&
+pcl::gpu::DeviceMemory2D::operator=(const pcl::gpu::DeviceMemory2D& other_arg)
 {
-    if( this != &other_arg )
-    {
-        if( other_arg.refcount_ )
-            CV_XADD(other_arg.refcount_, 1);
-        release();
-        
-        colsBytes_ = other_arg.colsBytes_;
-        rows_ = other_arg.rows_;
-        data_ = other_arg.data_;
-        step_ = other_arg.step_;
-                
-        refcount_ = other_arg.refcount_;
-    }
-    return *this;
+  if (this != &other_arg) {
+    if (other_arg.refcount_)
+      CV_XADD(other_arg.refcount_, 1);
+    release();
+
+    colsBytes_ = other_arg.colsBytes_;
+    rows_ = other_arg.rows_;
+    data_ = other_arg.data_;
+    step_ = other_arg.step_;
+
+    refcount_ = other_arg.refcount_;
+  }
+  return *this;
 }
 
-void pcl::gpu::DeviceMemory2D::create(int rows_arg, int colsBytes_arg)
+void
+pcl::gpu::DeviceMemory2D::create(int rows_arg, int colsBytes_arg)
 {
-    if (colsBytes_ == colsBytes_arg && rows_ == rows_arg)
-        return;
-            
-    if( rows_arg > 0 && colsBytes_arg > 0)
-    {        
-        if( data_ )
-            release();
-              
-        colsBytes_ = colsBytes_arg;
-        rows_ = rows_arg;
-                        
-        cudaSafeCall( cudaMallocPitch( (void**)&data_, &step_, colsBytes_, rows_) );        
+  if (colsBytes_ == colsBytes_arg && rows_ == rows_arg)
+    return;
+
+  if (rows_arg > 0 && colsBytes_arg > 0) {
+    if (data_)
+      release();
+
+    colsBytes_ = colsBytes_arg;
+    rows_ = rows_arg;
+
+    cudaSafeCall(cudaMallocPitch((void**)&data_, &step_, colsBytes_, rows_));
+
+    // refcount = (int*)cv::fastMalloc(sizeof(*refcount));
+    refcount_ = new int;
+    *refcount_ = 1;
+  }
+}
 
-        //refcount = (int*)cv::fastMalloc(sizeof(*refcount));
-        refcount_ = new int;
-        *refcount_ = 1;
-    }
+void
+pcl::gpu::DeviceMemory2D::release()
+{
+  if (refcount_ && CV_XADD(refcount_, -1) == 1) {
+    // cv::fastFree(refcount);
+    delete refcount_;
+    cudaSafeCall(cudaFree(data_));
+  }
+
+  colsBytes_ = 0;
+  rows_ = 0;
+  data_ = nullptr;
+  step_ = 0;
+  refcount_ = nullptr;
 }
 
-void pcl::gpu::DeviceMemory2D::release()
+void
+pcl::gpu::DeviceMemory2D::copyTo(DeviceMemory2D& other) const
 {
-    if( refcount_ && CV_XADD(refcount_, -1) == 1 )
-    {
-        //cv::fastFree(refcount);
-        delete refcount_;
-        cudaSafeCall( cudaFree(data_) );
-    }
+  if (empty())
+    other.release();
+  else {
+    other.create(rows_, colsBytes_);
+    cudaSafeCall(cudaMemcpy2D(other.data_,
+                              other.step_,
+                              data_,
+                              step_,
+                              colsBytes_,
+                              rows_,
+                              cudaMemcpyDeviceToDevice));
+    cudaSafeCall(cudaDeviceSynchronize());
+  }
+}
 
-    colsBytes_ = 0;
-    rows_ = 0;    
-    data_ = nullptr;    
-    step_ = 0;
-    refcount_ = nullptr;
+void
+pcl::gpu::DeviceMemory2D::upload(const void* host_ptr_arg,
+                                 std::size_t host_step_arg,
+                                 int rows_arg,
+                                 int colsBytes_arg)
+{
+  create(rows_arg, colsBytes_arg);
+  cudaSafeCall(cudaMemcpy2D(data_,
+                            step_,
+                            host_ptr_arg,
+                            host_step_arg,
+                            colsBytes_,
+                            rows_,
+                            cudaMemcpyHostToDevice));
+  cudaSafeCall(cudaDeviceSynchronize());
 }
 
-void pcl::gpu::DeviceMemory2D::copyTo(DeviceMemory2D& other) const
+void
+pcl::gpu::DeviceMemory2D::download(void* host_ptr_arg, std::size_t host_step_arg) const
 {
-    if (empty())
-        other.release();
-    else
-    {
-        other.create(rows_, colsBytes_);    
-        cudaSafeCall( cudaMemcpy2D(other.data_, other.step_, data_, step_, colsBytes_, rows_, cudaMemcpyDeviceToDevice) );
-        cudaSafeCall( cudaDeviceSynchronize() );
-    }
+  cudaSafeCall(cudaMemcpy2D(host_ptr_arg,
+                            host_step_arg,
+                            data_,
+                            step_,
+                            colsBytes_,
+                            rows_,
+                            cudaMemcpyDeviceToHost));
+  cudaSafeCall(cudaDeviceSynchronize());
 }
 
-void pcl::gpu::DeviceMemory2D::upload(const void *host_ptr_arg, std::size_t host_step_arg, int rows_arg, int colsBytes_arg)
+void
+pcl::gpu::DeviceMemory2D::swap(DeviceMemory2D& other_arg)
 {
-    create(rows_arg, colsBytes_arg);
-    cudaSafeCall( cudaMemcpy2D(data_, step_, host_ptr_arg, host_step_arg, colsBytes_, rows_, cudaMemcpyHostToDevice) );        
-    cudaSafeCall( cudaDeviceSynchronize() );
+  std::swap(data_, other_arg.data_);
+  std::swap(step_, other_arg.step_);
+
+  std::swap(colsBytes_, other_arg.colsBytes_);
+  std::swap(rows_, other_arg.rows_);
+  std::swap(refcount_, other_arg.refcount_);
 }
 
-void pcl::gpu::DeviceMemory2D::download(void *host_ptr_arg, std::size_t host_step_arg) const
-{    
-    cudaSafeCall( cudaMemcpy2D(host_ptr_arg, host_step_arg, data_, step_, colsBytes_, rows_, cudaMemcpyDeviceToHost) );
-    cudaSafeCall( cudaDeviceSynchronize() );
-}      
+bool
+pcl::gpu::DeviceMemory2D::empty() const
+{
+  return !data_;
+}
 
-void pcl::gpu::DeviceMemory2D::swap(DeviceMemory2D& other_arg)
-{    
-    std::swap(data_, other_arg.data_);
-    std::swap(step_, other_arg.step_);
+int
+pcl::gpu::DeviceMemory2D::colsBytes() const
+{
+  return colsBytes_;
+}
 
-    std::swap(colsBytes_, other_arg.colsBytes_);
-    std::swap(rows_, other_arg.rows_);
-    std::swap(refcount_, other_arg.refcount_);                 
+int
+pcl::gpu::DeviceMemory2D::rows() const
+{
+  return rows_;
 }
 
-bool pcl::gpu::DeviceMemory2D::empty() const { return !data_; }
-int pcl::gpu::DeviceMemory2D::colsBytes() const { return colsBytes_; }
-int pcl::gpu::DeviceMemory2D::rows() const { return rows_; }
-size_t pcl::gpu::DeviceMemory2D::step() const { return step_; }
+size_t
+pcl::gpu::DeviceMemory2D::step() const
+{
+  return step_;
+}
 
 #endif
diff --git a/gpu/containers/src/error.cpp b/gpu/containers/src/error.cpp
index a944e591..b570ae39 100644
--- a/gpu/containers/src/error.cpp
+++ b/gpu/containers/src/error.cpp
@@ -36,11 +36,15 @@
 
 #include <pcl/gpu/containers/initialization.h>
 
-#include <iostream>
 #include <cstdlib>
+#include <iostream>
 
-void pcl::gpu::error(const char *error_string, const char *file, const int line, const char *func)
-{	
-    std::cout << "Error: " << error_string << "\t" << file << ":" << line << std::endl;
-    exit(EXIT_FAILURE);
+void
+pcl::gpu::error(const char* error_string,
+                const char* file,
+                const int line,
+                const char* func)
+{
+  std::cout << "Error: " << error_string << "\t" << file << ":" << line << std::endl;
+  exit(EXIT_FAILURE);
 }
diff --git a/gpu/containers/src/initialization.cpp b/gpu/containers/src/initialization.cpp
index 4a8ac185..f5b0d077 100644
--- a/gpu/containers/src/initialization.cpp
+++ b/gpu/containers/src/initialization.cpp
@@ -1,247 +1,355 @@
 /*
-* Software License Agreement (BSD License)
-*
-*  Copyright (c) 2011, Willow Garage, Inc.
-*  All rights reserved.
-*
-*  Redistribution and use in source and binary forms, with or without
-*  modification, are permitted provided that the following conditions
-*  are met:
-*
-*   * Redistributions of source code must retain the above copyright
-*     notice, this list of conditions and the following disclaimer.
-*   * Redistributions in binary form must reproduce the above
-*     copyright notice, this list of conditions and the following
-*     disclaimer in the documentation and/or other materials provided
-*     with the distribution.
-*   * Neither the name of Willow Garage, Inc. nor the names of its
-*     contributors may be used to endorse or promote products derived
-*     from this software without specific prior written permission.
-*
-*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-*  POSSIBILITY OF SUCH DAMAGE.
-*
-*  Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
-*/
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
 
 #include <pcl/gpu/containers/initialization.h>
 #include <pcl/gpu/utils/safe_call.hpp>
 
 #include "cuda.h"
+
+#include <array> // replace c-style array with std::array
 #include <cstdio>
 
 #define HAVE_CUDA
 //#include <pcl_config.h>
 
-
 #if !defined(HAVE_CUDA)
 
-void throw_nogpu() { throw "PCL 2.0 exception"; }
-int  pcl::gpu::getCudaEnabledDeviceCount() { return 0; }
-void pcl::gpu::setDevice(int /*device*/) { throw_nogpu(); }
-std::string pcl::gpu::getDeviceName(int /*device*/) { throw_nogpu(); }
-void pcl::gpu::printCudaDeviceInfo(int /*device*/){ throw_nogpu(); }
-void pcl::gpu::printShortCudaDeviceInfo(int /*device*/) { throw_nogpu(); }
+void
+throw_nogpu()
+{
+  throw "PCL 2.0 exception";
+}
 
-#else
+int
+pcl::gpu::getCudaEnabledDeviceCount()
+{
+  return 0;
+}
+
+void
+pcl::gpu::setDevice(int /*device*/)
+{
+  throw_nogpu();
+}
 
+std::string
+pcl::gpu::getDeviceName(int /*device*/)
+{
+  throw_nogpu();
+}
 
-int pcl::gpu::getCudaEnabledDeviceCount()
+void
+pcl::gpu::printCudaDeviceInfo(int /*device*/)
 {
-    int count;
-    cudaError_t error = cudaGetDeviceCount( &count );
+  throw_nogpu();
+}
 
-    if (error == cudaErrorInsufficientDriver)
-        return -1;
+void
+pcl::gpu::printShortCudaDeviceInfo(int /*device*/)
+{
+  throw_nogpu();
+}
+
+#else
 
-    if (error == cudaErrorNoDevice)
-        return 0;
+int
+pcl::gpu::getCudaEnabledDeviceCount()
+{
+  int count;
+  cudaError_t error = cudaGetDeviceCount(&count);
 
-    cudaSafeCall(error);
-    return count;  
+  if (error == cudaErrorInsufficientDriver)
+    return -1;
+
+  if (error == cudaErrorNoDevice)
+    return 0;
+
+  cudaSafeCall(error);
+  return count;
 }
 
-void pcl::gpu::setDevice(int device)
+void
+pcl::gpu::setDevice(int device)
 {
-    cudaSafeCall( cudaSetDevice( device ) );
+  cudaSafeCall(cudaSetDevice(device));
 }
 
-std::string pcl::gpu::getDeviceName(int device)
+std::string
+pcl::gpu::getDeviceName(int device)
 {
-    cudaDeviceProp prop;
-    cudaSafeCall( cudaGetDeviceProperties(&prop, device) );
+  cudaDeviceProp prop;
+  cudaSafeCall(cudaGetDeviceProperties(&prop, device));
 
-    return prop.name;
+  return prop.name;
 }
 
-bool pcl::gpu::checkIfPreFermiGPU(int device)
+bool
+pcl::gpu::checkIfPreFermiGPU(int device)
 {
   if (device < 0)
-    cudaSafeCall( cudaGetDevice(&device) );
+    cudaSafeCall(cudaGetDevice(&device));
 
   cudaDeviceProp prop;
-  cudaSafeCall( cudaGetDeviceProperties(&prop, device) );
+  cudaSafeCall(cudaGetDeviceProperties(&prop, device));
   return prop.major < 2; // CC == 1.x
 }
 
-namespace 
+namespace {
+template <class T>
+inline void
+getCudaAttribute(T* attribute, CUdevice_attribute device_attribute, int device)
 {
-    template <class T> inline void getCudaAttribute(T *attribute, CUdevice_attribute device_attribute, int device)
-    {
-        *attribute = T();
-        CUresult error = CUDA_SUCCESS;// = cuDeviceGetAttribute( attribute, device_attribute, device );
-        if( CUDA_SUCCESS == error ) 
-            return;        
-
-        printf("Driver API error = %04d\n", error);
-        pcl::gpu::error("driver API error", __FILE__, __LINE__);    
-    }
-
-    inline int convertSMVer2Cores(int major, int minor)
-    {
-        // Defines for GPU Architecture types (using the SM version to determine the # of cores per SM
-        struct SMtoCores {
-            int SM; // 0xMm (hexadecimal notation), M = SM Major version, and m = SM minor version
-            int Cores;
-        };
-
-        SMtoCores gpuArchCoresPerSM[] = {
-            {0x10,   8}, {0x11,   8}, {0x12,   8}, {0x13,  8}, {0x20,  32}, {0x21, 48}, {0x30, 192},
-            {0x35, 192}, {0x50, 128}, {0x52, 128}, {0x53, 128}, {0x60, 64}, {0x61, 128}, {-1, -1}
-        };
-        int index = 0;
-        while (gpuArchCoresPerSM[index].SM != -1) 
-        {
-            if (gpuArchCoresPerSM[index].SM == ((major << 4) + minor) ) 
-                return gpuArchCoresPerSM[index].Cores;
-            index++;
-        }
-        printf("\nCan't determine number of cores. Unknown SM version %d.%d!\n", major, minor);
-        return 0;
-    }
+  *attribute = T();
+  CUresult error =
+      CUDA_SUCCESS; // = cuDeviceGetAttribute( attribute, device_attribute, device );
+  if (CUDA_SUCCESS == error)
+    return;
+
+  printf("Driver API error = %04d\n", error);
+  pcl::gpu::error("driver API error", __FILE__, __LINE__);
 }
 
-void pcl::gpu::printCudaDeviceInfo(int device)
+inline int
+convertSMVer2Cores(int major, int minor)
 {
-    int count = getCudaEnabledDeviceCount();
-    bool valid = (device >= 0) && (device < count);
-
-    int beg = valid ? device   : 0;
-    int end = valid ? device+1 : count;
-
-    printf("*** CUDA Device Query (Runtime API) version (CUDART static linking) *** \n\n");
-    printf("Device count: %d\n", count);
-
-    int driverVersion = 0, runtimeVersion = 0;
-    cudaSafeCall( cudaDriverGetVersion(&driverVersion) );
-    cudaSafeCall( cudaRuntimeGetVersion(&runtimeVersion) );
-
-    const char *computeMode[] = {
-        "Default (multiple host threads can use ::cudaSetDevice() with device simultaneously)",
-        "Exclusive (only one host thread in one process is able to use ::cudaSetDevice() with this device)",
-        "Prohibited (no host thread can use ::cudaSetDevice() with this device)",
-        "Exclusive Process (many threads in one process is able to use ::cudaSetDevice() with this device)",
-        "Unknown",
-        nullptr
-    };
-
-    for(int dev = beg; dev < end; ++dev)
-    {                
-        cudaDeviceProp prop;
-        cudaSafeCall( cudaGetDeviceProperties(&prop, dev) );
-
-        int sm_cores = convertSMVer2Cores(prop.major, prop.minor);
-
-        printf("\nDevice %d: \"%s\"\n", dev, prop.name);        
-        printf("  CUDA Driver Version / Runtime Version          %d.%d / %d.%d\n", driverVersion/1000, driverVersion%100, runtimeVersion/1000, runtimeVersion%100);
-        printf("  CUDA Capability Major/Minor version number:    %d.%d\n", prop.major, prop.minor);        
-        printf("  Total amount of global memory:                 %.0f MBytes (%llu bytes)\n", (float)prop.totalGlobalMem/1048576.0f, (unsigned long long) prop.totalGlobalMem);            
-        printf("  (%2d) Multiprocessors x (%2d) CUDA Cores/MP:     %d CUDA Cores\n", prop.multiProcessorCount, sm_cores, sm_cores * prop.multiProcessorCount);
-        printf("  GPU Clock Speed:                               %.2f GHz\n", prop.clockRate * 1e-6f);
-
-        // This is not available in the CUDA Runtime API, so we make the necessary calls the driver API to support this for output
-        int memoryClock, memBusWidth, L2CacheSize;
-        getCudaAttribute<int>( &memoryClock, CU_DEVICE_ATTRIBUTE_MEMORY_CLOCK_RATE, dev );        
-        getCudaAttribute<int>( &memBusWidth, CU_DEVICE_ATTRIBUTE_GLOBAL_MEMORY_BUS_WIDTH, dev );                
-        getCudaAttribute<int>( &L2CacheSize, CU_DEVICE_ATTRIBUTE_L2_CACHE_SIZE, dev );
-
-        printf("  Memory Clock rate:                             %.2f Mhz\n", memoryClock * 1e-3f);
-        printf("  Memory Bus Width:                              %d-bit\n", memBusWidth);
-        if (L2CacheSize)
-            printf("  L2 Cache Size:                                 %d bytes\n", L2CacheSize);
-        
-        printf("  Max Texture Dimension Size (x,y,z)             1D=(%d), 2D=(%d,%d), 3D=(%d,%d,%d)\n",
-            prop.maxTexture1D, prop.maxTexture2D[0], prop.maxTexture2D[1],
-            prop.maxTexture3D[0], prop.maxTexture3D[1], prop.maxTexture3D[2]);
-        printf("  Max Layered Texture Size (dim) x layers        1D=(%d) x %d, 2D=(%d,%d) x %d\n",
-            prop.maxTexture1DLayered[0], prop.maxTexture1DLayered[1],
-            prop.maxTexture2DLayered[0], prop.maxTexture2DLayered[1], prop.maxTexture2DLayered[2]);
-        printf("  Total amount of constant memory:               %u bytes\n", (int)prop.totalConstMem);
-        printf("  Total amount of shared memory per block:       %u bytes\n", (int)prop.sharedMemPerBlock);
-        printf("  Total number of registers available per block: %d\n", prop.regsPerBlock);
-        printf("  Warp size:                                     %d\n", prop.warpSize);
-        printf("  Maximum number of threads per block:           %d\n", prop.maxThreadsPerBlock);
-        printf("  Maximum sizes of each dimension of a block:    %d x %d x %d\n", prop.maxThreadsDim[0], prop.maxThreadsDim[1], prop.maxThreadsDim[2]);
-        printf("  Maximum sizes of each dimension of a grid:     %d x %d x %d\n", prop.maxGridSize[0], prop.maxGridSize[1],  prop.maxGridSize[2]);
-        printf("  Maximum memory pitch:                          %u bytes\n", (int)prop.memPitch);
-        printf("  Texture alignment:                             %u bytes\n", (int)prop.textureAlignment);
-
-        printf("  Concurrent copy and execution:                 %s with %d copy engine(s)\n", (prop.deviceOverlap ? "Yes" : "No"), prop.asyncEngineCount);
-        printf("  Run time limit on kernels:                     %s\n", prop.kernelExecTimeoutEnabled ? "Yes" : "No");
-        printf("  Integrated GPU sharing Host Memory:            %s\n", prop.integrated ? "Yes" : "No");
-        printf("  Support host page-locked memory mapping:       %s\n", prop.canMapHostMemory ? "Yes" : "No");
-
-        printf("  Concurrent kernel execution:                   %s\n", prop.concurrentKernels ? "Yes" : "No");
-        printf("  Alignment requirement for Surfaces:            %s\n", prop.surfaceAlignment ? "Yes" : "No");
-        printf("  Device has ECC support enabled:                %s\n", prop.ECCEnabled ? "Yes" : "No");
-        printf("  Device is using TCC driver mode:               %s\n", prop.tccDriver ? "Yes" : "No");
-        printf("  Device supports Unified Addressing (UVA):      %s\n", prop.unifiedAddressing ? "Yes" : "No");
-        printf("  Device PCI Bus ID / PCI location ID:           %d / %d\n", prop.pciBusID, prop.pciDeviceID );
-        printf("  Compute Mode:\n");
-        printf("      %s \n", computeMode[prop.computeMode]);
-    }
-    
-    printf("\n");    
-    printf("deviceQuery, CUDA Driver = CUDART");
-    printf(", CUDA Driver Version  = %d.%d", driverVersion / 1000, driverVersion % 100);
-    printf(", CUDA Runtime Version = %d.%d", runtimeVersion/1000, runtimeVersion%100);
-    printf(", NumDevs = %d\n\n", count);                
-    fflush(stdout);
+  // Defines for GPU Architecture types (using the SM version to determine the # of
+  // cores per SM
+  struct SMtoCores {
+    int SM; // 0xMm (hexadecimal notation), M = SM Major version, and m = SM minor
+            // version
+    int Cores;
+  };
+
+  std::array<SMtoCores, 15> gpuArchCoresPerSM = {{{0x30, 192},
+                                                  {0x32, 192},
+                                                  {0x35, 192},
+                                                  {0x37, 192},
+                                                  {0x50, 128},
+                                                  {0x52, 128},
+                                                  {0x53, 128},
+                                                  {0x60, 64},
+                                                  {0x61, 128},
+                                                  {0x62, 128},
+                                                  {0x70, 64},
+                                                  {0x72, 64},
+                                                  {0x75, 64},
+                                                  {0x80, 64},
+                                                  {0x86, 128}}};
+  for (const auto& sm2cores : gpuArchCoresPerSM) {
+    if (sm2cores.SM == ((major << 4) + minor))
+      return sm2cores.Cores;
+  }
+  printf(
+      "\nCan't determine number of cores. Unknown SM version %d.%d!\n", major, minor);
+  return 0;
 }
+} // namespace
 
-void pcl::gpu::printShortCudaDeviceInfo(int device)
+void
+pcl::gpu::printCudaDeviceInfo(int device)
 {
-    int count = getCudaEnabledDeviceCount();
-    bool valid = (device >= 0) && (device < count);
-
-    int beg = valid ? device   : 0;
-    int end = valid ? device+1 : count;
-
-    int driverVersion = 0, runtimeVersion = 0;
-    cudaSafeCall( cudaDriverGetVersion(&driverVersion) );
-    cudaSafeCall( cudaRuntimeGetVersion(&runtimeVersion) );
-
-    for(int dev = beg; dev < end; ++dev)
-    {                
-        cudaDeviceProp prop;
-        cudaSafeCall( cudaGetDeviceProperties(&prop, dev) );
-
-        const char *arch_str = prop.major < 2 ? " (pre-Fermi)" : "";
-        printf("[pcl::gpu::printShortCudaDeviceInfo] : Device %d:  \"%s\"  %.0fMb", dev, prop.name, (float)prop.totalGlobalMem/1048576.0f);
-        printf(", sm_%d%d%s, %d cores", prop.major, prop.minor, arch_str, convertSMVer2Cores(prop.major, prop.minor) * prop.multiProcessorCount);                
-        printf(", Driver/Runtime ver.%d.%d/%d.%d\n", driverVersion/1000, driverVersion%100, runtimeVersion/1000, runtimeVersion%100);
-    }
-    fflush(stdout);
+  int count = getCudaEnabledDeviceCount();
+  bool valid = (device >= 0) && (device < count);
+
+  int beg = valid ? device : 0;
+  int end = valid ? device + 1 : count;
+
+  printf(
+      "*** CUDA Device Query (Runtime API) version (CUDART static linking) *** \n\n");
+  printf("Device count: %d\n", count);
+
+  int driverVersion = 0, runtimeVersion = 0;
+  cudaSafeCall(cudaDriverGetVersion(&driverVersion));
+  cudaSafeCall(cudaRuntimeGetVersion(&runtimeVersion));
+
+  const char* computeMode[] = {
+      "Default (multiple host threads can use ::cudaSetDevice() simultaneously)",
+      "Exclusive (only one host thread in one process can use ::cudaSetDevice())",
+      "Prohibited (no host thread can use ::cudaSetDevice())",
+      "Exclusive Process (many threads in one process can use ::cudaSetDevice())",
+      "Unknown",
+      nullptr};
+
+  for (int dev = beg; dev < end; ++dev) {
+    cudaDeviceProp prop;
+    cudaSafeCall(cudaGetDeviceProperties(&prop, dev));
+
+    int sm_cores = convertSMVer2Cores(prop.major, prop.minor);
+
+    printf("\nDevice %d: \"%s\"\n", dev, prop.name);
+    printf("  CUDA Driver Version / Runtime Version          %d.%d / %d.%d\n",
+           driverVersion / 1000,
+           driverVersion % 100,
+           runtimeVersion / 1000,
+           runtimeVersion % 100);
+    printf("  CUDA Capability Major/Minor version number:    %d.%d\n",
+           prop.major,
+           prop.minor);
+    printf(
+        "  Total amount of global memory:                 %.0f MBytes (%llu bytes)\n",
+        (float)prop.totalGlobalMem / 1048576.0f,
+        (unsigned long long)prop.totalGlobalMem);
+    printf("  (%2d) Multiprocessors x (%2d) CUDA Cores/MP:     %d CUDA Cores\n",
+           prop.multiProcessorCount,
+           sm_cores,
+           sm_cores * prop.multiProcessorCount);
+    printf("  GPU Clock Speed:                               %.2f GHz\n",
+           prop.clockRate * 1e-6f);
+
+    // This is not available in the CUDA Runtime API, so we make the necessary calls the
+    // driver API to support this for output
+    int memoryClock, memBusWidth, L2CacheSize;
+    getCudaAttribute<int>(&memoryClock, CU_DEVICE_ATTRIBUTE_MEMORY_CLOCK_RATE, dev);
+    getCudaAttribute<int>(
+        &memBusWidth, CU_DEVICE_ATTRIBUTE_GLOBAL_MEMORY_BUS_WIDTH, dev);
+    getCudaAttribute<int>(&L2CacheSize, CU_DEVICE_ATTRIBUTE_L2_CACHE_SIZE, dev);
+
+    printf("  Memory Clock rate:                             %.2f Mhz\n",
+           memoryClock * 1e-3f);
+    printf("  Memory Bus Width:                              %d-bit\n", memBusWidth);
+    if (L2CacheSize)
+      printf("  L2 Cache Size:                                 %d bytes\n",
+             L2CacheSize);
+
+    printf("  Max Texture Dimension Size (x,y,z)             1D=(%d), 2D=(%d,%d), "
+           "3D=(%d,%d,%d)\n",
+           prop.maxTexture1D,
+           prop.maxTexture2D[0],
+           prop.maxTexture2D[1],
+           prop.maxTexture3D[0],
+           prop.maxTexture3D[1],
+           prop.maxTexture3D[2]);
+    printf("  Max Layered Texture Size (dim) x layers        1D=(%d) x %d, 2D=(%d,%d) "
+           "x %d\n",
+           prop.maxTexture1DLayered[0],
+           prop.maxTexture1DLayered[1],
+           prop.maxTexture2DLayered[0],
+           prop.maxTexture2DLayered[1],
+           prop.maxTexture2DLayered[2]);
+    printf("  Total amount of constant memory:               %u bytes\n",
+           (int)prop.totalConstMem);
+    printf("  Total amount of shared memory per block:       %u bytes\n",
+           (int)prop.sharedMemPerBlock);
+    printf("  Total number of registers available per block: %d\n", prop.regsPerBlock);
+    printf("  Warp size:                                     %d\n", prop.warpSize);
+    printf("  Maximum number of threads per block:           %d\n",
+           prop.maxThreadsPerBlock);
+    printf("  Maximum sizes of each dimension of a block:    %d x %d x %d\n",
+           prop.maxThreadsDim[0],
+           prop.maxThreadsDim[1],
+           prop.maxThreadsDim[2]);
+    printf("  Maximum sizes of each dimension of a grid:     %d x %d x %d\n",
+           prop.maxGridSize[0],
+           prop.maxGridSize[1],
+           prop.maxGridSize[2]);
+    printf("  Maximum memory pitch:                          %u bytes\n",
+           (int)prop.memPitch);
+    printf("  Texture alignment:                             %u bytes\n",
+           (int)prop.textureAlignment);
+
+    printf(
+        "  Concurrent copy and execution:                 %s with %d copy engine(s)\n",
+        (prop.deviceOverlap ? "Yes" : "No"),
+        prop.asyncEngineCount);
+    printf("  Run time limit on kernels:                     %s\n",
+           prop.kernelExecTimeoutEnabled ? "Yes" : "No");
+    printf("  Integrated GPU sharing Host Memory:            %s\n",
+           prop.integrated ? "Yes" : "No");
+    printf("  Support host page-locked memory mapping:       %s\n",
+           prop.canMapHostMemory ? "Yes" : "No");
+
+    printf("  Concurrent kernel execution:                   %s\n",
+           prop.concurrentKernels ? "Yes" : "No");
+    printf("  Alignment requirement for Surfaces:            %s\n",
+           prop.surfaceAlignment ? "Yes" : "No");
+    printf("  Device has ECC support enabled:                %s\n",
+           prop.ECCEnabled ? "Yes" : "No");
+    printf("  Device is using TCC driver mode:               %s\n",
+           prop.tccDriver ? "Yes" : "No");
+    printf("  Device supports Unified Addressing (UVA):      %s\n",
+           prop.unifiedAddressing ? "Yes" : "No");
+    printf("  Device PCI Bus ID / PCI location ID:           %d / %d\n",
+           prop.pciBusID,
+           prop.pciDeviceID);
+    printf("  Compute Mode:\n");
+    printf("      %s \n", computeMode[prop.computeMode]);
+  }
+
+  printf("\n");
+  printf("deviceQuery, CUDA Driver = CUDART");
+  printf(", CUDA Driver Version  = %d.%d", driverVersion / 1000, driverVersion % 100);
+  printf(", CUDA Runtime Version = %d.%d", runtimeVersion / 1000, runtimeVersion % 100);
+  printf(", NumDevs = %d\n\n", count);
+  fflush(stdout);
+}
+
+void
+pcl::gpu::printShortCudaDeviceInfo(int device)
+{
+  int count = getCudaEnabledDeviceCount();
+  bool valid = (device >= 0) && (device < count);
+
+  int beg = valid ? device : 0;
+  int end = valid ? device + 1 : count;
+
+  int driverVersion = 0, runtimeVersion = 0;
+  cudaSafeCall(cudaDriverGetVersion(&driverVersion));
+  cudaSafeCall(cudaRuntimeGetVersion(&runtimeVersion));
+
+  for (int dev = beg; dev < end; ++dev) {
+    cudaDeviceProp prop;
+    cudaSafeCall(cudaGetDeviceProperties(&prop, dev));
+
+    const char* arch_str = prop.major < 2 ? " (pre-Fermi)" : "";
+    printf("[pcl::gpu::printShortCudaDeviceInfo] : Device %d:  \"%s\"  %.0fMb",
+           dev,
+           prop.name,
+           (float)prop.totalGlobalMem / 1048576.0f);
+    printf(", sm_%d%d%s, %d cores",
+           prop.major,
+           prop.minor,
+           arch_str,
+           convertSMVer2Cores(prop.major, prop.minor) * prop.multiProcessorCount);
+    printf(", Driver/Runtime ver.%d.%d/%d.%d\n",
+           driverVersion / 1000,
+           driverVersion % 100,
+           runtimeVersion / 1000,
+           runtimeVersion % 100);
+  }
+  fflush(stdout);
 }
 
 #endif
diff --git a/gpu/examples/octree/src/octree_search.cpp b/gpu/examples/octree/src/octree_search.cpp
index 7e591d61..2de8d205 100644
--- a/gpu/examples/octree/src/octree_search.cpp
+++ b/gpu/examples/octree/src/octree_search.cpp
@@ -23,7 +23,7 @@ int main (int argc, char** argv)
     {
       pcl::PointXYZ p;
       p.x = w; p.y = h; p.z = 1;
-      cloud.points.push_back(p);
+      cloud.push_back(p);
     }
   }
 
@@ -90,7 +90,7 @@ int main (int argc, char** argv)
 
       for (std::size_t j = 0; j < sizes[i] ; ++j)
       {
-        cloud_result.points.push_back(cloud[data[j + i * max_answers]]);
+        cloud_result.push_back(cloud[data[j + i * max_answers]]);
         std::cout << "INFO: data : " << j << " " << j + i * max_answers << " data " << data[j+ i * max_answers] << std::endl;
       }
       std::stringstream ss;
diff --git a/gpu/examples/segmentation/src/seg.cpp b/gpu/examples/segmentation/src/seg.cpp
index 948774d3..348e27c1 100644
--- a/gpu/examples/segmentation/src/seg.cpp
+++ b/gpu/examples/segmentation/src/seg.cpp
@@ -49,11 +49,11 @@ main (int argc, char** argv)
   printf("CPU Time taken: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC);
 
   int j = 0;
-  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
+  for (const pcl::PointIndices& cluster: cluster_indices)
   {
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
-    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
-      cloud_cluster->push_back ((*cloud_filtered)[*pit]); //*
+    for (const auto& index : (cluster.indices))
+      cloud_cluster->push_back ((*cloud_filtered)[index]); //*
     cloud_cluster->width = cloud_cluster->size ();
     cloud_cluster->height = 1;
     cloud_cluster->is_dense = true;
@@ -94,11 +94,11 @@ main (int argc, char** argv)
   std::cout << "INFO: stopped with the GPU version" << std::endl;
 
   j = 0;
-  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices_gpu.begin (); it != cluster_indices_gpu.end (); ++it)
+  for (const pcl::PointIndices& cluster : cluster_indices_gpu)
   {
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster_gpu (new pcl::PointCloud<pcl::PointXYZ>);
-    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
-      cloud_cluster_gpu->push_back ((*cloud_filtered)[*pit]); //*
+    for (const auto& index : (cluster.indices))
+      cloud_cluster_gpu->push_back ((*cloud_filtered)[index]); //*
     cloud_cluster_gpu->width = cloud_cluster_gpu->size ();
     cloud_cluster_gpu->height = 1;
     cloud_cluster_gpu->is_dense = true;
diff --git a/gpu/features/include/pcl/gpu/features/device/pair_features.hpp b/gpu/features/include/pcl/gpu/features/device/pair_features.hpp
index 6337f239..9ebb17c1 100644
--- a/gpu/features/include/pcl/gpu/features/device/pair_features.hpp
+++ b/gpu/features/include/pcl/gpu/features/device/pair_features.hpp
@@ -191,15 +191,15 @@ namespace pcl
             float3 row1, row2, row3; // == rotation_mg
             AngleAxisf(acos_value, cross_vector_norm, row1, row2, row3);
 
-            float3 traslation;
-            //traslation.x = row1.x * -model_reference_point.x + row1.y * -model_reference_point.y + row1.z * -model_reference_point.z;
-            traslation.y = row2.x * -model_reference_point.x + row2.y * -model_reference_point.y + row2.z * -model_reference_point.z;
-            traslation.z = row3.x * -model_reference_point.x + row3.y * -model_reference_point.y + row3.z * -model_reference_point.z;
+            float3 translation;
+            //translation.x = row1.x * -model_reference_point.x + row1.y * -model_reference_point.y + row1.z * -model_reference_point.z;
+            translation.y = row2.x * -model_reference_point.x + row2.y * -model_reference_point.y + row2.z * -model_reference_point.z;
+            translation.z = row3.x * -model_reference_point.x + row3.y * -model_reference_point.y + row3.z * -model_reference_point.z;
 
             float3 model_point_transformed;// = transform_mg * model_point;
-            //model_point_transformed.x = traslation.x + row1.x * model_point.x + row1.y * model_point.y + row1.z * model_point.z;
-            model_point_transformed.y = traslation.y + row2.x * model_point.x + row2.y * model_point.y + row2.z * model_point.z;
-            model_point_transformed.z = traslation.z + row3.x * model_point.x + row3.y * model_point.y + row3.z * model_point.z;
+            //model_point_transformed.x = translation.x + row1.x * model_point.x + row1.y * model_point.y + row1.z * model_point.z;
+            model_point_transformed.y = translation.y + row2.x * model_point.x + row2.y * model_point.y + row2.z * model_point.z;
+            model_point_transformed.z = translation.z + row3.x * model_point.x + row3.y * model_point.y + row3.z * model_point.z;
 
 
             float angle = std::atan2 ( -model_point_transformed.z, model_point_transformed.y);
diff --git a/gpu/features/src/fpfh.cu b/gpu/features/src/fpfh.cu
index f5105502..d2ae7fda 100644
--- a/gpu/features/src/fpfh.cu
+++ b/gpu/features/src/fpfh.cu
@@ -140,15 +140,17 @@ namespace pcl
                         if (computePairFeatures (current_point[warp_idx], current_nomal[warp_idx], p, n, f1, f2, f3, f4))
                         {                                                          
                             // Normalize the f1, f2, f3 features and push them in the histogram
-                            h_index = std::floor (bins1 * ((f1 + M_PI) * (1.0f / (2.0f * M_PI))));
+                            //Using floorf due to changes to MSVC 16.9. See details here: https://devtalk.blender.org/t/cuda-compile-error-windows-10/17886/4
+                            //floorf is without std:: see why here: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=79700
+                            h_index = floorf (bins1 * ((f1 + M_PI) * (1.0f / (2.0f * M_PI))));
                             h_index = min(bins1 - 1, max(0, h_index));                                                       
                             atomicAdd(shist_b1 + h_index, hist_incr);                            
 
-                            h_index = std::floor (bins2 * ((f2 + 1.0f) * 0.5f));
+                            h_index = floorf (bins2 * ((f2 + 1.0f) * 0.5f));
                             h_index = min(bins2 - 1, max (0, h_index));
                             atomicAdd(shist_b2 + h_index, hist_incr);                          
 
-                            h_index = std::floor (bins3 * ((f3 + 1.0f) * 0.5f));
+                            h_index = floorf (bins3 * ((f3 + 1.0f) * 0.5f));
                             h_index = min(bins3 - 1, max (0, h_index));
 
                             atomicAdd(shist_b3 + h_index, hist_incr); 
diff --git a/gpu/features/src/pfh.cu b/gpu/features/src/pfh.cu
index 2b4bbc83..b5a508b3 100644
--- a/gpu/features/src/pfh.cu
+++ b/gpu/features/src/pfh.cu
@@ -197,13 +197,15 @@ namespace pcl
                         //if (computePairFeatures (pi, ni, pj, nj, f1, f2, f3, f4))
                         {                            
                             // Normalize the f1, f2, f3 features and push them in the histogram
-                            int find0 = std::floor( NR_SPLIT * ((f1 + PI) * (1.f / (2.f * PI))) );                            
+                            //Using floorf due to changes to MSVC 16.9. See details here: https://devtalk.blender.org/t/cuda-compile-error-windows-10/17886/4
+                            //floorf is without std:: see why here: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=79700
+                            int find0 = floorf( NR_SPLIT * ((f1 + PI) * (1.f / (2.f * PI))) );                            
                             find0 = min(NR_SPLIT - 1, max(0, find0));
 
-                            int find1 = std::floor( NR_SPLIT * ( (f2 + 1.f) * 0.5f ) );
+                            int find1 = floorf( NR_SPLIT * ( (f2 + 1.f) * 0.5f ) );
                             find1 = min(NR_SPLIT - 1, max(0, find1));
 
-                            int find2 = std::floor( NR_SPLIT * ( (f3 + 1.f) * 0.5f ) );
+                            int find2 = floorf( NR_SPLIT * ( (f3 + 1.f) * 0.5f ) );
                             find2 = min(NR_SPLIT - 1, max(0, find2));
 
                             int h_index = find0 + NR_SPLIT * find1 + NR_SPLIT_2 * find2;
@@ -218,13 +220,13 @@ namespace pcl
                                 computeRGBPairFeatures_RGBOnly(ci, cj, f5, f6, f7);
 
                                 // color ratios are in [-1, 1]
-                                int find4 = std::floor (NR_SPLIT * ((f5 + 1.f) * 0.5f));
+                                int find4 = floorf(NR_SPLIT * ((f5 + 1.f) * 0.5f));
                                 find4 = min(NR_SPLIT - 1, max(0, find4));
 
-                                int find5 = std::floor (NR_SPLIT * ((f6 + 1.f) * 0.5f));
+                                int find5 = floorf(NR_SPLIT * ((f6 + 1.f) * 0.5f));
                                 find5 = min(NR_SPLIT - 1, max(0, find5));
 
-                                int find6 = std::floor (NR_SPLIT * ((f7 + 1.f) * 0.5f));
+                                int find6 = floorf(NR_SPLIT * ((f7 + 1.f) * 0.5f));
                                 find6 = min(NR_SPLIT - 1, max(0, find6));
 
                                 // and the colors
diff --git a/gpu/features/src/spinimages.cu b/gpu/features/src/spinimages.cu
index 4b9f587d..c763ae3b 100644
--- a/gpu/features/src/spinimages.cu
+++ b/gpu/features/src/spinimages.cu
@@ -197,8 +197,10 @@ namespace pcl
 
 					// bilinear interpolation
 					float beta_bin_size = radial ? (PI*0.5f/image_width) : bin_size;
-					int beta_bin  = std::floor(beta  / beta_bin_size) + image_width;
-					int alpha_bin = std::floor(alpha / bin_size);
+					//Using floorf due to changes to MSVC 16.9. See details here: https://devtalk.blender.org/t/cuda-compile-error-windows-10/17886/4
+					//floorf is without std:: see why here: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=79700
+					int beta_bin  = floorf(beta  / beta_bin_size) + image_width;
+					int alpha_bin = floorf(alpha / bin_size);
 
 					//alpha_bin = min(simage_cols, max(0, alpha_bin));
 					//beta_bin  = min(simage_rows, max(0,  beta_bin));					
diff --git a/gpu/features/src/vfh.cu b/gpu/features/src/vfh.cu
index c221ccf0..2a577157 100644
--- a/gpu/features/src/vfh.cu
+++ b/gpu/features/src/vfh.cu
@@ -136,20 +136,22 @@ namespace pcl
                     if (computePairFeatures(centroid_p, centroid_n, p, n, f1, f2, f3, f4))
                     {
                         // Normalize the f1, f2, f3, f4 features and push them in the histogram
-                        h_index = std::floor (bins1 * ((f1 + M_PI) * (1.f / (2.f * M_PI))));
+                        //Using floorf due to changes to MSVC 16.9. See details here: https://devtalk.blender.org/t/cuda-compile-error-windows-10/17886/4
+                        //floorf is without std:: see why here: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=79700
+                        h_index = floorf (bins1 * ((f1 + M_PI) * (1.f / (2.f * M_PI))));
                         h_index = min(bins1 - 1, max(0, h_index));  
                         atomicAdd(shist_b1 + h_index, hist_incr);    
 
-                        h_index = std::floor (bins2 * ((f2 + 1.f) * 0.5f));
+                        h_index = floorf (bins2 * ((f2 + 1.f) * 0.5f));
                         h_index = min(bins2 - 1, max (0, h_index));                                            
                         atomicAdd(shist_b2 + h_index, hist_incr);  
 
-                        h_index = std::floor (bins3 * ((f3 + 1.f) * 0.5f));
+                        h_index = floorf (bins3 * ((f3 + 1.f) * 0.5f));
                         h_index = min(bins3 - 1, max (0, h_index));
                         atomicAdd(shist_b3 + h_index, hist_incr);
 
                         if (normalize_distances)
-                            h_index = std::floor (bins4 * (f4 * distance_normalization_factor_inv));
+                            h_index = floorf (bins4 * (f4 * distance_normalization_factor_inv));
                         else
                             h_index = __float2int_rn (f4 * 100);
 
@@ -159,7 +161,7 @@ namespace pcl
 
                     // viewpoint component
                     float alfa = ((dot(n, d_vp_p) + 1.f) * 0.5f);                    
-                    h_index = std::floor (bins_vp * alfa);
+                    h_index = floorf (bins_vp * alfa);
                     h_index = min(bins_vp - 1, max (0, h_index));
                     atomicAdd(shist_vp + h_index, hist_incr_vp);
                 
diff --git a/gpu/features/test/test_fpfh.cpp b/gpu/features/test/test_fpfh.cpp
index 301468a0..5d2dbb6e 100644
--- a/gpu/features/test/test_fpfh.cpp
+++ b/gpu/features/test/test_fpfh.cpp
@@ -43,7 +43,6 @@
 #include <pcl/common/common.h>
 #include <pcl/gpu/features/features.hpp>
 #include "data_source.hpp"
-#include <pcl/search/pcl_search.h>
 
 using namespace pcl;
 using namespace pcl::gpu;
diff --git a/gpu/features/test/test_normals.cpp b/gpu/features/test/test_normals.cpp
index 1d3ca2a2..7d6a4923 100644
--- a/gpu/features/test/test_normals.cpp
+++ b/gpu/features/test/test_normals.cpp
@@ -400,7 +400,7 @@ TEST(PCL_FeaturesGPU, normals_nan_gpu)
     const std::size_t N = 5;
 
     PointCloud<PointXYZ> cloud;
-    cloud.points.assign(N, {0.0, 0.0, 0.0});
+    cloud.assign(N, {0.0, 0.0, 0.0});
 
     const float radius_search = 2.0F;
     const int max_results = 500;
diff --git a/gpu/kinfu/src/tsdf_volume.cpp b/gpu/kinfu/src/tsdf_volume.cpp
index 8e8087e1..e763238a 100644
--- a/gpu/kinfu/src/tsdf_volume.cpp
+++ b/gpu/kinfu/src/tsdf_volume.cpp
@@ -149,8 +149,8 @@ pcl::gpu::TsdfVolume::fetchCloudHost (PointCloud<PointType>& cloud, bool connect
   std::vector<int> volume_host;
   volume_.download (volume_host, cols);
 
-  cloud.points.clear ();
-  cloud.points.reserve (10000);
+  cloud.clear ();
+  cloud.reserve (10000);
 
   constexpr int DIVISOR = device::DIVISOR; // SHRT_MAX;
 
@@ -196,7 +196,7 @@ pcl::gpu::TsdfVolume::fetchCloudHost (PointCloud<PointType>& cloud, bool connect
                 xyz.y = point (1);
                 xyz.z = point (2);
 
-                cloud.points.push_back (xyz);
+                cloud.push_back (xyz);
               }
             }
           dz = 0;
@@ -220,7 +220,7 @@ pcl::gpu::TsdfVolume::fetchCloudHost (PointCloud<PointType>& cloud, bool connect
                 xyz.y = point (1);
                 xyz.z = point (2);
 
-                cloud.points.push_back (xyz);
+                cloud.push_back (xyz);
               }
             }
         }
@@ -252,7 +252,7 @@ pcl::gpu::TsdfVolume::fetchCloudHost (PointCloud<PointType>& cloud, bool connect
               xyz.y = point (1);
               xyz.z = point (2);
 
-              cloud.points.push_back (xyz);
+              cloud.push_back (xyz);
             }
           }
         } /* if (connected26) */
@@ -333,4 +333,4 @@ pcl::gpu::TsdfVolume::downloadTsdfAndWeighs (std::vector<float>& tsdf, std::vect
     tsdf[i] = (float)(elem.x)/device::DIVISOR;    
     weights[i] = (short)(elem.y);    
   }
-}
\ No newline at end of file
+}
diff --git a/gpu/kinfu/tools/CMakeLists.txt b/gpu/kinfu/tools/CMakeLists.txt
index 8ef3d920..691b4d87 100644
--- a/gpu/kinfu/tools/CMakeLists.txt
+++ b/gpu/kinfu/tools/CMakeLists.txt
@@ -8,8 +8,6 @@ if(NOT VTK_FOUND)
 else()
   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()
 
diff --git a/gpu/kinfu/tools/kinfu_app.cpp b/gpu/kinfu/tools/kinfu_app.cpp
index cb8e30f9..a18d3dca 100644
--- a/gpu/kinfu/tools/kinfu_app.cpp
+++ b/gpu/kinfu/tools/kinfu_app.cpp
@@ -44,6 +44,7 @@
 #include <pcl/console/parse.h>
 
 #include <boost/filesystem.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp> // for microsec_clock::local_time
 
 #include <pcl/gpu/kinfu/kinfu.h>
 #include <pcl/gpu/kinfu/raycaster.h>
@@ -128,18 +129,17 @@ namespace pcl
         }
               
         /** \brief Obtain the actual color for the input dataset as vtk scalars.
-          * \param[out] scalars the output scalars containing the color for the dataset
-          * \return true if the operation was successful (the handler is capable and 
-          * the input cloud was given as a valid pointer), false otherwise
+          * \return Array of scalars if the operation was successful (the handler is capable and 
+          * the input cloud was given as a valid pointer), nullptr otherwise
           */
-        bool
-        getColor (vtkSmartPointer<vtkDataArray> &scalars) const override
+        vtkSmartPointer<vtkDataArray>
+        getColor () const override
         {
           if (!capable_ || !cloud_)
-            return (false);
+            return nullptr;
          
-          if (!scalars)
-            scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
+          auto scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
+  
           scalars->SetNumberOfComponents (3);
             
           vtkIdType nr_points = vtkIdType (cloud_->size ());
@@ -157,7 +157,7 @@ namespace pcl
               colors[idx + 1] = (*rgb_)[cp].g;
               colors[idx + 2] = (*rgb_)[cp].b;
             }
-          return (true);
+          return scalars;
         }
 
       private:
diff --git a/gpu/kinfu/tools/kinfu_app_sim.cpp b/gpu/kinfu/tools/kinfu_app_sim.cpp
index c6e55f1b..1591c199 100644
--- a/gpu/kinfu/tools/kinfu_app_sim.cpp
+++ b/gpu/kinfu/tools/kinfu_app_sim.cpp
@@ -98,7 +98,6 @@ using ScopeTimeT = pcl::ScopeTime;
 
 #include "../src/internal.h"
 
-#include <Eigen/Dense>
 #include <cmath>
 #include <iostream>
 #include <pcl/memory.h>
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 8bf99b7f..9749eaa3 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
@@ -194,7 +194,7 @@ pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::convertTsdfVectors (const Po
 #pragma omp parallel for \
   default(none) \
   shared(cloud, output) 	
-	for(int i = 0; i < (int) cloud.points.size (); ++i)
+	for(int i = 0; i < (int) cloud.size (); ++i)
 	{
 	  int x = cloud[i].x;
 	  int y = cloud[i].y;
diff --git a/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/impl/world_model.hpp b/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/impl/world_model.hpp
index 9c5495a4..0067db50 100644
--- a/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/impl/world_model.hpp
+++ b/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/impl/world_model.hpp
@@ -40,6 +40,7 @@
 #define PCL_WORLD_MODEL_IMPL_HPP_
 
 #include <pcl/gpu/kinfu_large_scale/world_model.h>
+#include <pcl/common/transforms.h> // for transformPointCloud
 
 template <typename PointT>
 void 
@@ -114,7 +115,7 @@ pcl::kinfuLS::WorldModel<PointT>::getExistingData(const double previous_origin_x
   // apply filter
   condrem.filter (existing_slice);  
  
-  if(!existing_slice.points.empty ())
+  if(!existing_slice.empty ())
   {
 	//transform the slice in new cube coordinates
 	Eigen::Affine3f transformation; 
@@ -146,7 +147,7 @@ pcl::kinfuLS::WorldModel<PointT>::getWorldAsCubes (const double size, std::vecto
 
   // remove nans from world cloud
   world_->is_dense = false;
-  std::vector<int> indices;
+  pcl::Indices indices;
   pcl::removeNaNFromPointCloud ( *world_, *world_, indices);
 
   PCL_INFO("World contains %zu points after nan removal.\n",
diff --git a/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/world_model.h b/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/world_model.h
index 4f7e2a17..d8b696ce 100644
--- a/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/world_model.h
+++ b/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/world_model.h
@@ -41,7 +41,6 @@
 #include <pcl/common/impl/common.hpp>
 #include <pcl/filters/extract_indices.h>
 #include <pcl/filters/filter_indices.h>
-#include <pcl/filters/crop_box.h>
 #include <pcl/filters/conditional_removal.h>
 #include <pcl/point_types.h>
 #include <pcl/io/pcd_io.h>
@@ -138,7 +137,7 @@ namespace pcl
         void cleanWorldFromNans () 
         { 
           world_->is_dense = false;
-          std::vector<int> indices; 
+          pcl::Indices indices;
           pcl::removeNaNFromPointCloud (*world_, *world_, indices);
         }
 
diff --git a/gpu/kinfu_large_scale/src/cyclical_buffer.cpp b/gpu/kinfu_large_scale/src/cyclical_buffer.cpp
index 4d55eb88..745eb56c 100644
--- a/gpu/kinfu_large_scale/src/cyclical_buffer.cpp
+++ b/gpu/kinfu_large_scale/src/cyclical_buffer.cpp
@@ -38,6 +38,7 @@
 
 #include <pcl/gpu/kinfu_large_scale/cyclical_buffer.h>
 #include <pcl/common/distances.h>
+#include <pcl/common/transforms.h> // for transformPoint, transformPointCloud
 #include "internal.h"
 
 
diff --git a/gpu/kinfu_large_scale/src/tsdf_volume.cpp b/gpu/kinfu_large_scale/src/tsdf_volume.cpp
index 19d774a5..591b0da0 100644
--- a/gpu/kinfu_large_scale/src/tsdf_volume.cpp
+++ b/gpu/kinfu_large_scale/src/tsdf_volume.cpp
@@ -160,8 +160,8 @@ pcl::gpu::kinfuLS::TsdfVolume::fetchCloudHost (PointCloud<PointType>& cloud, boo
   std::vector<int> volume_host;
   volume_.download (volume_host, cols);
 
-  cloud.points.clear ();
-  cloud.points.reserve (10000);
+  cloud.clear ();
+  cloud.reserve (10000);
 
   constexpr int DIVISOR = pcl::device::kinfuLS::DIVISOR; // SHRT_MAX;
 
@@ -207,7 +207,7 @@ pcl::gpu::kinfuLS::TsdfVolume::fetchCloudHost (PointCloud<PointType>& cloud, boo
                 xyz.y = point (1);
                 xyz.z = point (2);
 
-                cloud.points.push_back (xyz);
+                cloud.push_back (xyz);
               }
             }
           dz = 0;
@@ -231,7 +231,7 @@ pcl::gpu::kinfuLS::TsdfVolume::fetchCloudHost (PointCloud<PointType>& cloud, boo
                 xyz.y = point (1);
                 xyz.z = point (2);
 
-                cloud.points.push_back (xyz);
+                cloud.push_back (xyz);
               }
             }
         }
@@ -263,7 +263,7 @@ pcl::gpu::kinfuLS::TsdfVolume::fetchCloudHost (PointCloud<PointType>& cloud, boo
               xyz.y = point (1);
               xyz.z = point (2);
 
-              cloud.points.push_back (xyz);
+              cloud.push_back (xyz);
             }
           }
         } /* if (connected26) */
diff --git a/gpu/kinfu_large_scale/tools/CMakeLists.txt b/gpu/kinfu_large_scale/tools/CMakeLists.txt
index 9df2535a..65f63aa8 100644
--- a/gpu/kinfu_large_scale/tools/CMakeLists.txt
+++ b/gpu/kinfu_large_scale/tools/CMakeLists.txt
@@ -8,8 +8,6 @@ if(NOT VTK_FOUND)
 else()
   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()
 
diff --git a/gpu/kinfu_large_scale/tools/color_handler.h b/gpu/kinfu_large_scale/tools/color_handler.h
index 3048b8f9..3478a3be 100644
--- a/gpu/kinfu_large_scale/tools/color_handler.h
+++ b/gpu/kinfu_large_scale/tools/color_handler.h
@@ -63,14 +63,13 @@ namespace pcl
         capable_ = true;
       }
             
-      bool 
-      getColor (vtkSmartPointer<vtkDataArray> &scalars) const override
+      vtkSmartPointer<vtkDataArray>
+      getColor () const override
       {
         if (!capable_)
-          return (false);
+          return nullptr;
       
-        if (!scalars)
-          scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
+        auto scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
         scalars->SetNumberOfComponents (3);
         
         vtkIdType nr_points = static_cast<vtkIdType>(cloud_->size ());
@@ -88,7 +87,7 @@ namespace pcl
             colors[idx + 1] = (*rgb_)[cp].g;
             colors[idx + 2] = (*rgb_)[cp].b;
           }
-        return (true);
+        return scalars;
       }
     
     private:
diff --git a/gpu/kinfu_large_scale/tools/kinfuLS_app.cpp b/gpu/kinfu_large_scale/tools/kinfuLS_app.cpp
index c531f671..d96b6171 100644
--- a/gpu/kinfu_large_scale/tools/kinfuLS_app.cpp
+++ b/gpu/kinfu_large_scale/tools/kinfuLS_app.cpp
@@ -56,6 +56,7 @@ Work in progress: patch by Marco (AUG,19th 2012)
 #include <pcl/console/parse.h>
 
 #include <boost/filesystem.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp> // for microsec_clock::local_time
 
 #include <pcl/gpu/kinfu_large_scale/kinfu.h>
 #include <pcl/gpu/kinfu_large_scale/raycaster.h>
@@ -63,6 +64,7 @@ Work in progress: patch by Marco (AUG,19th 2012)
 #include <pcl/gpu/containers/initialization.h>
 
 #include <pcl/common/time.h>
+#include <pcl/common/transforms.h> // for transformPoint
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl/visualization/image_viewer.h>
@@ -1033,7 +1035,7 @@ struct KinFuLSApp
         c = capture_.registerCallback (func2);
       }
       #else
-      PCL_ERROR ("OpenNI2 is disabled in this PCL. Please build PCL with OpenNI2 feature.");
+      PCL_ERROR ("OpenNI2 is disabled in this PCL. Please build PCL with OpenNI2 feature.\n");
       #endif
     }
     else
@@ -1064,7 +1066,7 @@ struct KinFuLSApp
         c = capture_.registerCallback (func2);
       }
       #else
-      PCL_ERROR ("OpenNI is disabled in this PCL. Please build PCL with OpenNI feature.");
+      PCL_ERROR ("OpenNI is disabled in this PCL. Please build PCL with OpenNI feature.\n");
       #endif
     }
 
diff --git a/gpu/kinfu_large_scale/tools/kinfu_app_sim.cpp b/gpu/kinfu_large_scale/tools/kinfu_app_sim.cpp
index 3e405331..c333b269 100644
--- a/gpu/kinfu_large_scale/tools/kinfu_app_sim.cpp
+++ b/gpu/kinfu_large_scale/tools/kinfu_app_sim.cpp
@@ -96,7 +96,6 @@
 #endif
 using ScopeTimeT = pcl::ScopeTime;
 
-#include <Eigen/Dense>
 #include <cmath>
 #include <iostream>
 #ifdef _WIN32
diff --git a/gpu/octree/include/pcl/gpu/octree/octree.hpp b/gpu/octree/include/pcl/gpu/octree/octree.hpp
index fbb41809..a5e2c734 100644
--- a/gpu/octree/include/pcl/gpu/octree/octree.hpp
+++ b/gpu/octree/include/pcl/gpu/octree/octree.hpp
@@ -144,8 +144,16 @@ namespace pcl
               * \param[in] queries array of centers
               * \param[out] result array of results ( one index for each query ) 
               */
+            PCL_DEPRECATED(1, 14, "use approxNearestSearch() which returns square distances instead")
             void approxNearestSearch(const Queries& queries, NeighborIndices& result) const;
 
+            /** \brief Batch approximate nearest search on GPU
+              * \param[in] queries array of centers
+              * \param[out] result array of results ( one index for each query )
+              * \param[out] sqr_distance corresponding square distances to results from query point
+              */
+            void approxNearestSearch(const Queries& queries, NeighborIndices& result, ResultSqrDists& sqr_distance) const;
+
             /** \brief Batch exact k-nearest search on GPU for k == 1 only!
               * \param[in] queries array of centers
               * \param[in] k number of neighbors (only k == 1 is supported)
@@ -153,6 +161,14 @@ namespace pcl
               */
             void nearestKSearchBatch(const Queries& queries, int k, NeighborIndices& results) const;
 
+            /** \brief Batch exact k-nearest search on GPU for k == 1 only!
+              * \param[in] queries array of centers
+              * \param[in] k number of neighbors (only k == 1 is supported)
+              * \param[out] results array of results
+              * \param[out] sqr_distances square distances to results
+              */
+            void nearestKSearchBatch(const Queries& queries, int k, NeighborIndices& results, ResultSqrDists& sqr_distances) const;
+
             /** \brief Desroys octree and release all resources */
             void clear();            
         private:
diff --git a/gpu/octree/src/cuda/approx_nsearch.cu b/gpu/octree/src/cuda/approx_nsearch.cu
index df7d4240..509fce7e 100644
--- a/gpu/octree/src/cuda/approx_nsearch.cu
+++ b/gpu/octree/src/cuda/approx_nsearch.cu
@@ -34,279 +34,216 @@
  *  Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
  */
 
-#include <limits>
-
 #include "internal.hpp"
 #include "pcl/gpu/utils/device/warp.hpp"
-
-#include "utils/copygen.hpp"
+#include "utils/approx_nearest_utils.hpp"
 #include "utils/boxutils.hpp"
+#include "utils/copygen.hpp"
 #include "utils/scan_block.hpp"
+#include <assert.h>
 
+#include <limits>
+#include <tuple>
+
+namespace pcl {
+namespace device {
+namespace appnearest_search {
+using PointType = OctreeImpl::PointType;
+
+struct Batch {
+  const PointType* queries;
+
+  const int* indices;
+  const float* points;
+  int points_step; // elem step
+
+  OctreeGlobalWithBox octree;
+
+  int queries_num;
+  mutable int* output;
+  mutable float* sqr_distance;
+};
+
+struct KernelPolicy {
+  enum {
+    CTA_SIZE = 512,
+
+    LOG_WARP_SIZE = 5,
+    WARP_SIZE = 1 << LOG_WARP_SIZE,
+    WARPS_COUNT = CTA_SIZE / WARP_SIZE,
+  };
+};
+
+struct Warp_appNearestSearch {
+public:
+  const Batch& batch;
+
+  int query_index;
+  float3 query;
+  int result_idx;
+  float sqr_dist;
+
+  __device__ __forceinline__
+  Warp_appNearestSearch(const Batch& batch_arg, int query_index_arg)
+  : batch(batch_arg), query_index(query_index_arg)
+  {}
+
+  __device__ __forceinline__ void
+  launch(bool active)
+  {
+    int node_idx = -1;
+    if (active) {
+      PointType q = batch.queries[query_index];
+      query = make_float3(q.x, q.y, q.z);
+
+      const float3& minp = batch.octree.minp;
+      const float3& maxp = batch.octree.maxp;
+
+      node_idx = pcl::device::findNode(minp, maxp, query, batch.octree.nodes);
+    }
+
+    processNode(node_idx);
+
+    if (active)
+    {
+      batch.output[query_index] = batch.indices[result_idx];
+      batch.sqr_distance[query_index] = sqr_dist;
+    }
+  }
+
+private:
+  __device__ __forceinline__ void
+  processNode(const int node_idx)
+  {
+    __shared__ volatile int per_warp_buffer[KernelPolicy::WARPS_COUNT];
+
+    int mask = __ballot_sync(0xFFFFFFFF, node_idx != -1);
+
+    while (mask) {
+      const unsigned int laneId = Warp::laneId();
+
+      const int active_lane = __ffs(mask) - 1; //[0..31]
+      mask &= ~(1 << active_lane);
+
+      // broadcast beg and end
+      int fbeg, fend;
+      if (active_lane == laneId) {
+        fbeg = batch.octree.begs[node_idx];
+        fend = batch.octree.ends[node_idx];
+      }
+      const int beg = __shfl_sync(0xFFFFFFFF, fbeg, active_lane);
+      const int end = __shfl_sync(0xFFFFFFFF, fend, active_lane);
+
+      // broadcast warp_query
+      const float3 active_query =
+          make_float3(__shfl_sync(0xFFFFFFFF, query.x, active_lane),
+                      __shfl_sync(0xFFFFFFFF, query.y, active_lane),
+                      __shfl_sync(0xFFFFFFFF, query.z, active_lane));
+
+      const auto nearestPoint = NearestWarpKernel<KernelPolicy::CTA_SIZE>(
+          beg, batch.points_step, end - beg, active_query);
+
+      if (active_lane == laneId)
+      {
+        result_idx = beg + nearestPoint.first;
+        sqr_dist = nearestPoint.second;
+      }
+    }
+  }
+
+  template <int CTA_SIZE>
+  __device__ std::pair<int, float>
+  NearestWarpKernel(const int beg,
+                    const int field_step,
+                    const int length,
+                    const float3& active_query)
+
+  {
+    int index = 0;
+    float dist_squared = std::numeric_limits<float>::max();
+
+    // serial step
+    for (int idx = Warp::laneId(); idx < length; idx += Warp::STRIDE) {
+      const float dx = batch.points[beg + idx] - active_query.x;
+      const float dy = batch.points[beg + idx + field_step] - active_query.y;
+      const float dz = batch.points[beg + idx + field_step * 2] - active_query.z;
+
+      const float d2 = dx * dx + dy * dy + dz * dz;
+
+      if (dist_squared > d2) {
+        dist_squared = d2;
+        index = idx;
+      }
+    }
+
+    // find minimum distance among warp threads
+    constexpr unsigned FULL_MASK = 0xFFFFFFFF;
+    static_assert(KernelPolicy::WARP_SIZE <= 8 * sizeof(FULL_MASK),
+                  "WARP_SIZE exceeds size of bit_offset.");
+
+    for (unsigned int bit_offset = KernelPolicy::WARP_SIZE / 2; bit_offset > 0;
+         bit_offset /= 2) {
+      const float next = __shfl_down_sync(FULL_MASK, dist_squared, bit_offset);
+      const int next_index = __shfl_down_sync(FULL_MASK, index, bit_offset);
+
+      if (dist_squared > next) {
+        dist_squared = next;
+        index = next_index;
+      }
+    }
+
+    // retrieve index and distance
+    index = __shfl_sync(FULL_MASK, index, 0);
+    dist_squared = __shfl_sync(FULL_MASK, dist_squared, 0);
+
+    return {index, dist_squared};
+  }
+};
+
+__global__ void
+KernelAN(const Batch batch)
+{
+  const int query_index = blockIdx.x * blockDim.x + threadIdx.x;
 
-namespace pcl { namespace device { namespace appnearest_search
-{   
-    using PointType = OctreeImpl::PointType;
-	
-	struct Batch
-	{   
-		const PointType* queries;
-
-		const int *indices;
-		const float* points;
-		int points_step; // elem step
-
-		OctreeGlobalWithBox octree;
-	    
-		int queries_num;                
-		mutable int* output;                
-	};
-
-	struct KernelPolicy
-	{
-		enum 
-		{
-			CTA_SIZE = 512,
-
-			LOG_WARP_SIZE = 5,
-			WARP_SIZE = 1 << LOG_WARP_SIZE,
-			WARPS_COUNT = CTA_SIZE/WARP_SIZE,                    
-		};	
-	};
-
-	struct Warp_appNearestSearch
-	{   
-	public:                		
-		const Batch& batch;
-
-		int query_index;        
-		float3 query;  
-		int result_idx;
+  const bool active = query_index < batch.queries_num;
 
-		__device__ __forceinline__ Warp_appNearestSearch(const Batch& batch_arg, int query_index_arg) 
-			: batch(batch_arg), query_index(query_index_arg){}
+  if (__all_sync(0xFFFFFFFF, active == false))
+    return;
 
-		__device__ __forceinline__ void launch(bool active)
-		{              
-			int node_idx = -1;
-			if (active)
-			{
-				PointType q = batch.queries[query_index];
-				query = make_float3(q.x, q.y, q.z);                        
-
-				node_idx = findNode();
-			}           
+  Warp_appNearestSearch search(batch, query_index);
+  search.launch(active);
+}
 
-			processNode(node_idx);                    
-
-			if (active)
-				batch.output[query_index] = batch.indices[result_idx];
-		}    
-
-	private:
+} // namespace appnearest_search
+} // namespace device
+} // namespace pcl
 
-		__device__ __forceinline__ int findNode()
-		{                                             
-			float3 minp = batch.octree.minp;
-			float3 maxp = batch.octree.maxp;
-
-			if(query.x < minp.x || query.y < minp.y ||  query.z < minp.z)
-				return 0;
-
-			if(query.x > maxp.x || query.y > maxp.y ||  query.z > maxp.z)
-				return 0;
-
-			int node_idx = 0;
-			int code = CalcMorton(minp, maxp)(query);
-			int level = 0;
-
-			for(;;)
-			{
-				int mask_pos = 1 << Morton::extractLevelCode(code, level);
-
-				int node = batch.octree.nodes[node_idx];
-				int mask = node & 0xFF;
-
-				if(__popc(mask) == 0)  // leaf
-					return node_idx; 
-
-				if ( (mask & mask_pos) == 0) // no child
-					return node_idx;                         
-
-				node_idx = (node >> 8) + __popc(mask & (mask_pos - 1));
-				++level;
-			}
-		};
-
-		__device__ __forceinline__ void processNode(int node_idx)
-		{   
-            __shared__ volatile int  per_warp_buffer[KernelPolicy::WARPS_COUNT];
-
-			int mask = __ballot_sync(0xFFFFFFFF, node_idx != -1);                        
-
-			while(mask)
-			{                
-				unsigned int laneId = Warp::laneId();
-				unsigned int warpId = threadIdx.x/warpSize;            
-
-				int active_lane = __ffs(mask) - 1; //[0..31]                        
-				mask &= ~(1 << active_lane);   
-
-				volatile int* warp_buffer = &per_warp_buffer[warpId];
-
-				//broadcast beg
-				if (active_lane == laneId)
-					*warp_buffer = batch.octree.begs[node_idx];                    
-				int beg = *warp_buffer;
-
-				//broadcast end
-				if (active_lane == laneId)
-					*warp_buffer = batch.octree.ends[node_idx];
-				int end = *warp_buffer;
-
-				float3 active_query;
-				volatile float* warp_buffer_float = (float*)&per_warp_buffer[warpId];
-
-				//broadcast warp_query
-				if (active_lane == laneId)
-					*warp_buffer_float = query.x;
-				active_query.x = *warp_buffer_float;
-
-				if (active_lane == laneId)
-					*warp_buffer_float = query.y;
-				active_query.y = *warp_buffer_float;
-
-				if (active_lane == laneId)
-					*warp_buffer_float = query.z;
-				active_query.z = *warp_buffer_float;                            
-
-				int offset = NearestWarpKernel<KernelPolicy::CTA_SIZE>(batch.points + beg, batch.points_step, end - beg, active_query);                    
-
-				if (active_lane == laneId)
-					result_idx = beg + offset;
-			}
-		}
-
-        template<int CTA_SIZE>
-		__device__ __forceinline__ int NearestWarpKernel(const float* points, int points_step, int length, const float3& active_query)
-		{                        						
-            __shared__ volatile float dist2[CTA_SIZE];
-            __shared__ volatile int   index[CTA_SIZE];
-			
-            int tid = threadIdx.x;
-			dist2[tid] = std::numeric_limits<float>::max();
-
-			//serial step
-            for (int idx = Warp::laneId(); idx < length; idx += Warp::STRIDE)
-			{
-				float dx = points[idx                  ] - active_query.x;
-				float dy = points[idx + points_step    ] - active_query.y;
-				float dz = points[idx + points_step * 2] - active_query.z;
-
-				float d2 = dx * dx + dy * dy + dz * dz;
-
-				if (dist2[tid] > d2)
-				{
-					dist2[tid] = d2;
-					index[tid] = idx;                            
-				}
-			}
-			//parallel step
-			unsigned int lane = Warp::laneId();
-
-			float mind2 = dist2[tid];
-
-			if (lane < 16)
-			{
-				float next = dist2[tid + 16];
-				if (mind2 > next) 
-				{ 
-					dist2[tid] = mind2 = next; 
-					index[tid] = index[tid + 16]; 
-				}                        
-			}
-
-			if (lane < 8)
-			{
-				float next = dist2[tid + 8];
-				if (mind2 > next) 
-				{ 
-					dist2[tid] = mind2 = next; 
-					index[tid] = index[tid + 8]; 
-				}                        
-			}
-
-			if (lane < 4)
-			{
-				float next = dist2[tid + 4];
-				if (mind2 > next) 
-				{ 
-					dist2[tid] = mind2 = next; 
-					index[tid] = index[tid + 4]; 
-				}                        
-			}
-
-			if (lane < 2)
-			{
-				float next = dist2[tid + 2];
-				if (mind2 > next) 
-				{ 
-					dist2[tid] = mind2 = next; 
-					index[tid] = index[tid + 2]; 
-				}                        
-			}
-
-			if (lane < 1)
-			{
-				float next = dist2[tid + 1];
-				if (mind2 > next) 
-				{ 
-					dist2[tid] = mind2 = next; 
-					index[tid] = index[tid + 1]; 
-				}                        
-			}        
-
-			return index[tid - lane];
-		}
-	};
-	
-	__global__ void KernelAN(const Batch batch) 
-	{         
-		int query_index = blockIdx.x * blockDim.x + threadIdx.x;
-
-		bool active = query_index < batch.queries_num;
-
-		if (__all_sync(0xFFFFFFFF, active == false)) 
-			return;
-
-		Warp_appNearestSearch search(batch, query_index);
-		search.launch(active); 
-	}
-
-} } }
-
-
-void pcl::device::OctreeImpl::approxNearestSearch(const Queries& queries, NeighborIndices& results) const
+void
+pcl::device::OctreeImpl::approxNearestSearch(const Queries& queries,
+                                             NeighborIndices& results,
+                                             BatchResultSqrDists& sqr_distance) const
 {
-    using BatchType = pcl::device::appnearest_search::Batch;
+  using BatchType = pcl::device::appnearest_search::Batch;
 
-    BatchType batch;
-    batch.indices = indices;
-    batch.octree = octreeGlobal;
+  BatchType batch;
+  batch.indices = indices;
+  batch.octree = octreeGlobal;
 
-    batch.queries_num = (int)queries.size();        
-    batch.output = results.data;     
+  batch.queries_num = (int)queries.size();
+  batch.output = results.data;
+  batch.sqr_distance = sqr_distance;
 
-    batch.points = points_sorted;
-    batch.points_step = (int)points_sorted.elem_step();
-    batch.queries = queries;
+  batch.points = points_sorted;
+  batch.points_step = (int)points_sorted.elem_step();
+  batch.queries = queries;
 
-    int block = pcl::device::appnearest_search::KernelPolicy::CTA_SIZE;
-    int grid = (batch.queries_num + block - 1) / block;    
+  int block = pcl::device::appnearest_search::KernelPolicy::CTA_SIZE;
+  int grid = (batch.queries_num + block - 1) / block;
 
-    cudaSafeCall( cudaFuncSetCacheConfig(pcl::device::appnearest_search::KernelAN, cudaFuncCachePreferL1) );
+  cudaSafeCall(cudaFuncSetCacheConfig(pcl::device::appnearest_search::KernelAN,
+                                      cudaFuncCachePreferL1));
 
-    pcl::device::appnearest_search::KernelAN<<<grid, block>>>(batch);
-    cudaSafeCall( cudaGetLastError() );
-    cudaSafeCall( cudaDeviceSynchronize() );
+  pcl::device::appnearest_search::KernelAN<<<grid, block>>>(batch);
+  cudaSafeCall(cudaGetLastError());
+  cudaSafeCall(cudaDeviceSynchronize());
 }
\ No newline at end of file
diff --git a/gpu/octree/src/cuda/knn_search.cu b/gpu/octree/src/cuda/knn_search.cu
index 353fc148..1f26ee58 100644
--- a/gpu/octree/src/cuda/knn_search.cu
+++ b/gpu/octree/src/cuda/knn_search.cu
@@ -68,6 +68,7 @@ namespace pcl { namespace device { namespace knn_search
         int queries_num;                
         mutable int* output;                        
         mutable int* sizes;
+        mutable float* sqr_distances;
     };
 
     struct KernelPolicy
@@ -91,12 +92,13 @@ namespace pcl { namespace device { namespace knn_search
         float3 query;  
         
         float min_distance;
+        float min_distance_squared;
         int min_idx;
 
         OctreeIterator iterator;     
 
         __device__ __forceinline__ Warp_knnSearch(const Batch& batch_arg, const int query_index_arg)
-            : batch(batch_arg), query_index(query_index_arg), min_distance(std::numeric_limits<float>::max()), min_idx(0), iterator(batch.octree) { }
+            : batch(batch_arg), query_index(query_index_arg), min_distance(std::numeric_limits<float>::max()), min_distance_squared(std::numeric_limits<float>::max()), min_idx(0), iterator(batch.octree) { }
 
         __device__ __forceinline__ void launch(bool active)
         {              
@@ -121,7 +123,8 @@ namespace pcl { namespace device { namespace knn_search
             }
             if (query_index != -1)
             {
-				batch.output[query_index] = batch.indices[min_idx];
+                batch.output[query_index] = batch.indices[min_idx];
+                batch.sqr_distances[query_index] = min_distance_squared;
 
                 if (batch.sizes)
                     batch.sizes[query_index]  = 1;
@@ -197,10 +200,11 @@ namespace pcl { namespace device { namespace knn_search
                 const auto nearestPoint = NearestWarpKernel<KernelPolicy::CTA_SIZE>(beg, batch.points_step, end - beg, active_query);
 
                 if (active_lane == laneId)
-                    if (min_distance > nearestPoint.second)
+                    if (min_distance_squared > nearestPoint.second)
                     {
-                       min_distance = nearestPoint.second;
+                        min_distance_squared = nearestPoint.second;
                        min_idx = beg + nearestPoint.first;
+                       min_distance = sqrt(nearestPoint.second);
                     }
             }
         }
@@ -214,7 +218,7 @@ namespace pcl { namespace device { namespace knn_search
 
         {
           int index = 0;
-          float dist2 = std::numeric_limits<float>::max();
+          float dist_squared = std::numeric_limits<float>::max();
 
           // serial step
           for (int idx = Warp::laneId(); idx < length; idx += Warp::STRIDE) {
@@ -224,8 +228,8 @@ namespace pcl { namespace device { namespace knn_search
 
             const float d2 = dx * dx + dy * dy + dz * dz;
 
-            if (dist2 > d2) {
-              dist2 = d2;
+            if (dist_squared > d2) {
+                dist_squared = d2;
               index = idx;
             }
           }
@@ -236,20 +240,20 @@ namespace pcl { namespace device { namespace knn_search
 
           for (unsigned int bit_offset = KernelPolicy::WARP_SIZE / 2; bit_offset > 0;
                bit_offset /= 2) {
-            const float next = __shfl_down_sync(FULL_MASK, dist2, bit_offset);
+            const float next = __shfl_down_sync(FULL_MASK, dist_squared, bit_offset);
             const int next_index = __shfl_down_sync(FULL_MASK, index, bit_offset);
 
-            if (dist2 > next) {
-              dist2 = next;
+            if (dist_squared > next) {
+                dist_squared = next;
               index = next_index;
             }
           }
 
           // retrieve index and distance
           index = __shfl_sync(FULL_MASK, index, 0);
-          const float dist = sqrt(__shfl_sync(FULL_MASK, dist2, 0));
+          dist_squared = __shfl_sync(FULL_MASK, dist_squared, 0);
 
-          return std::make_pair(index, dist);
+          return {index, dist_squared};
         }
     };
     
@@ -269,7 +273,7 @@ namespace pcl { namespace device { namespace knn_search
 } } }
 
 
-void pcl::device::OctreeImpl::nearestKSearchBatch(const Queries& queries, int /*k*/, NeighborIndices& results) const
+void pcl::device::OctreeImpl::nearestKSearchBatch(const Queries& queries, int /*k*/, NeighborIndices& results, BatchResultSqrDists& sqr_distances) const
 {              
     using BatchType = pcl::device::knn_search::Batch;
 
@@ -282,6 +286,7 @@ void pcl::device::OctreeImpl::nearestKSearchBatch(const Queries& queries, int /*
     
     batch.output = results.data;
     batch.sizes  = results.sizes;
+    batch.sqr_distances = sqr_distances;
     
     batch.points = points_sorted;
     batch.points_step = points_sorted.step()/points_sorted.elem_size;
diff --git a/gpu/octree/src/cuda/octree_host.cu b/gpu/octree/src/cuda/octree_host.cu
index 3957bb5c..2230ab3e 100644
--- a/gpu/octree/src/cuda/octree_host.cu
+++ b/gpu/octree/src/cuda/octree_host.cu
@@ -38,10 +38,12 @@
 #include "pcl/gpu/utils/safe_call.hpp"
 
 #include "internal.hpp"
+#include "utils/approx_nearest_utils.hpp"
 #include "utils/boxutils.hpp"
 
 #include<algorithm>
 #include<limits>
+#include <tuple>
 
 using namespace pcl::gpu;
 using namespace pcl::device;
@@ -88,18 +90,6 @@ void pcl::device::OctreeImpl::internalDownload()
 
 namespace 
 {
-    int getBitsNum(int integer)
-    {
-        int count = 0;
-        while(integer > 0)
-        {
-            if (integer & 1)
-                ++count;
-            integer>>=1;
-        }
-        return count;
-    } 
-
     struct OctreeIteratorHost
     {        
         const static int MAX_LEVELS_PLUS_ROOT = 11;
@@ -223,35 +213,11 @@ void pcl::device::OctreeImpl::radiusSearchHost(const PointType& query, float rad
 
 void  pcl::device::OctreeImpl::approxNearestSearchHost(const PointType& query, int& out_index, float& sqr_dist) const
 {
-    float3 minp = octreeGlobal.minp;
-    float3 maxp = octreeGlobal.maxp;
-
-    int node_idx = 0;
+    const float3& minp = octreeGlobal.minp;
+    const float3& maxp = octreeGlobal.maxp;
+    const float3 query_point = make_float3(query.x, query.y, query.z);
 
-    bool out_of_root = query.x < minp.x || query.y < minp.y ||  query.z < minp.z || query.x > maxp.x || query.y > maxp.y ||  query.z > maxp.z;
-
-    if(!out_of_root)        
-    {
-        int code = CalcMorton(minp, maxp)(query);
-        int level = 0;
-
-        for(;;)
-        {            
-            int mask_pos = 1 << Morton::extractLevelCode(code, level);
-
-            int node = host_octree.nodes[node_idx];
-            int mask = node & 0xFF;
-
-            if(getBitsNum(mask) == 0)  // leaf
-                break;
-
-            if ( (mask & mask_pos) == 0) // no child
-                break;
-
-            node_idx = (node >> 8) + getBitsNum(mask & (mask_pos - 1));
-            ++level;
-        }
-    }
+    const int node_idx = pcl::device::findNode(minp, maxp, query_point, host_octree.nodes);
 
     int beg = host_octree.begs[node_idx];
     int end = host_octree.ends[node_idx];
@@ -275,7 +241,7 @@ void  pcl::device::OctreeImpl::approxNearestSearchHost(const PointType& query, i
             sqr_dist = d2;
             out_index = i;
         }
-    }  
+    }
 
     out_index = host_octree.indices[out_index];
 }
diff --git a/gpu/octree/src/internal.hpp b/gpu/octree/src/internal.hpp
index 86ff690c..7951a584 100644
--- a/gpu/octree/src/internal.hpp
+++ b/gpu/octree/src/internal.hpp
@@ -97,9 +97,9 @@ namespace pcl
 
             void radiusSearch(const Queries& queries, const Indices& indices, float radius, NeighborIndices& results);
 
-            void approxNearestSearch(const Queries& queries, NeighborIndices& results) const;
+            void approxNearestSearch(const Queries& queries, NeighborIndices& results, BatchResultSqrDists& sqr_distance) const;
             
-            void nearestKSearchBatch(const Queries& queries, int k, NeighborIndices& results) const;
+            void nearestKSearchBatch(const Queries& queries, int k, NeighborIndices& results, BatchResultSqrDists& sqr_distances) const;
             
             //just reference 
             PointCloud points;
diff --git a/gpu/octree/src/octree.cpp b/gpu/octree/src/octree.cpp
index 8790b361..b642b591 100644
--- a/gpu/octree/src/octree.cpp
+++ b/gpu/octree/src/octree.cpp
@@ -66,6 +66,14 @@ pcl::gpu::Octree::Octree() : cloud_(nullptr), impl(nullptr)
     int bin, ptx;
     OctreeImpl::get_gpu_arch_compiled_for(bin, ptx);
 
+    if (bin < 0 || ptx < 0)
+    {
+        pcl::gpu::error(R"(cudaFuncGetAttributes() returned a value < 0.
+This is likely a build configuration error.
+Ensure that the proper compute capability is specified in the CUDA_ARCH_BIN cmake variable when building for your GPU.)",
+            __FILE__, __LINE__);
+    }
+
     if (bin < 20 && ptx < 20)
         pcl::gpu::error("This must be compiled for compute capability >= 2.0", __FILE__, __LINE__);    
 
@@ -162,25 +170,39 @@ void pcl::gpu::Octree::radiusSearch(const Queries& queries, const Indices& indic
 }
 
 void pcl::gpu::Octree::approxNearestSearch(const Queries& queries, NeighborIndices& results) const
+{
+    ResultSqrDists sqr_distance;
+    approxNearestSearch(queries, results, sqr_distance);
+}
+
+void pcl::gpu::Octree::approxNearestSearch(const Queries& queries, NeighborIndices& results, ResultSqrDists& sqr_distance) const
 {
     assert(queries.size() > 0);    
     results.create(static_cast<int> (queries.size()), 1);
+    sqr_distance.create(queries.size());
     
     const OctreeImpl::Queries& q = (const OctreeImpl::Queries&)queries;
-    static_cast<OctreeImpl*>(impl)->approxNearestSearch(q, results);
+    static_cast<OctreeImpl*>(impl)->approxNearestSearch(q, results, sqr_distance);
 }
 
 void pcl::gpu::Octree::nearestKSearchBatch(const Queries& queries, int k, NeighborIndices& results) const
+{
+    ResultSqrDists sqr_distances;
+    nearestKSearchBatch(queries, k, results, sqr_distances);
+}
+
+void pcl::gpu::Octree::nearestKSearchBatch(const Queries& queries, int k, NeighborIndices& results, ResultSqrDists& sqr_distances) const
 {    
     if (k != 1)
         throw pcl::PCLException("OctreeGPU::knnSearch is supported only for k == 1", __FILE__, "", __LINE__);
     
     assert(queries.size() > 0);
     results.create(static_cast<int> (queries.size()), k);	    
+    sqr_distances.create(queries.size() * k);
 
 	const OctreeImpl::Queries& q = (const OctreeImpl::Queries&)queries;
 
-    static_cast<OctreeImpl*>(impl)->nearestKSearchBatch(q, k, results);
+    static_cast<OctreeImpl*>(impl)->nearestKSearchBatch(q, k, results, sqr_distances);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////
diff --git a/gpu/octree/src/utils/approx_nearest_utils.hpp b/gpu/octree/src/utils/approx_nearest_utils.hpp
new file mode 100644
index 00000000..247eda0d
--- /dev/null
+++ b/gpu/octree/src/utils/approx_nearest_utils.hpp
@@ -0,0 +1,126 @@
+/*
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2020-, Open Perception
+ *
+ *  All rights reserved
+ */
+
+#pragma once
+
+#include "morton.hpp"
+#include <assert.h>
+
+#include <limits>
+#include <tuple>
+#include <bitset>
+
+namespace pcl {
+namespace device {
+
+__device__ __host__ __forceinline__ unsigned
+getBitsNum(const unsigned integer)
+{
+  #ifdef __CUDA_ARCH__
+    return __popc(integer);
+  #else
+    return std::bitset<8*sizeof(integer)> (integer).count();
+  #endif
+}
+
+__host__ __device__ __forceinline__ std::pair<uint3, std::uint8_t>
+nearestVoxel(const float3 query,
+             const unsigned& level,
+             const std::uint8_t& mask,
+             const float3& minp,
+             const float3& maxp,
+             const uint3& index)
+{
+  assert(mask != 0);
+  // identify closest voxel
+  float closest_distance = std::numeric_limits<float>::max();
+  unsigned closest_index = 0;
+  uint3 closest = make_uint3(0, 0, 0);
+
+  for (unsigned i = 0; i < 8; ++i) {
+    if ((mask & (1 << i)) == 0) // no child
+      continue;
+
+    const uint3 child = make_uint3((index.x << 1) + (i & 1),
+                                   (index.y << 1) + ((i >> 1) & 1),
+                                   (index.z << 1) + ((i >> 2) & 1));
+
+    // find center of child cell
+    const unsigned voxel_width_scale_factor = 1 << (level + 2);
+    const float3 voxel_center =
+        make_float3(minp.x + (maxp.x - minp.x) * (2 * child.x + 1) / voxel_width_scale_factor,
+                    minp.y + (maxp.y - minp.y) * (2 * child.y + 1) / voxel_width_scale_factor,
+                    minp.z + (maxp.z - minp.z) * (2 * child.z + 1) / voxel_width_scale_factor);
+
+    // compute distance to centroid
+    const float3 dist = make_float3(
+        voxel_center.x - query.x, voxel_center.y - query.y, voxel_center.z - query.z);
+
+    const float distance_to_query = dist.x * dist.x + dist.y * dist.y + dist.z * dist.z;
+
+    // compare distance
+    if (distance_to_query < closest_distance) {
+      closest_distance = distance_to_query;
+      closest_index = i;
+      closest = child;
+    }
+  }
+
+  return {closest, 1 << closest_index};
+}
+
+#pragma hd_warning_disable
+template <typename T>
+__device__ __host__ int
+findNode(const float3 minp, const float3 maxp, const float3 query, const T nodes)
+{
+  size_t node_idx = 0;
+  const auto code = CalcMorton(minp, maxp)(query);
+  unsigned level = 0;
+
+  bool voxel_traversal = false;
+  uint3 index = Morton::decomposeCode(code);
+  std::uint8_t mask_pos;
+
+  while (true) {
+    const auto node = nodes[node_idx];
+    const std::uint8_t mask = node & 0xFF;
+
+    if (!mask) // leaf
+      return node_idx;
+
+    if (voxel_traversal) // empty voxel already encountered, performing nearest-centroid
+                         // based traversal
+    {
+      const auto nearest_voxel = nearestVoxel(query, level, mask, minp, maxp, index);
+      index = nearest_voxel.first;
+      mask_pos = nearest_voxel.second;
+    }
+    else {
+      mask_pos = 1 << Morton::extractLevelCode(code, level);
+
+      if (!(mask & mask_pos)) // child doesn't exist
+      {
+        const auto remaining_depth = Morton::levels - level;
+        index.x >>= remaining_depth;
+        index.y >>= remaining_depth;
+        index.z >>= remaining_depth;
+
+        voxel_traversal = true;
+        const auto nearest_voxel = nearestVoxel(query, level, mask, minp, maxp, index);
+        index = nearest_voxel.first;
+        mask_pos = nearest_voxel.second;
+      }
+    }
+    node_idx = (node >> 8) + getBitsNum(mask & (mask_pos - 1));
+    ++level;
+  }
+}
+} // namespace device
+} // namespace pcl
diff --git a/gpu/octree/src/utils/morton.hpp b/gpu/octree/src/utils/morton.hpp
index 5e160fd6..943e3deb 100644
--- a/gpu/octree/src/utils/morton.hpp
+++ b/gpu/octree/src/utils/morton.hpp
@@ -88,6 +88,12 @@ namespace pcl
                 cell_z = compactBits(code, 2);        
             }
 
+            __device__ __host__ __forceinline__
+                static uint3 decomposeCode(code_t code)
+            {
+                return make_uint3 (compactBits(code, 0), compactBits(code, 1), compactBits(code, 2));
+            }
+
             __host__ __device__ __forceinline__ 
                 static code_t extractLevelCode(code_t code, int level) 
             {
@@ -116,10 +122,12 @@ namespace pcl
             }			
 
             __device__ __host__ __forceinline__ Morton::code_t operator()(const float3& p) const
-            {			
-                int cellx = min((int)std::floor(depth_mult * (p.x - minp_.x)/dims_.x), depth_mult - 1);
-                int celly = min((int)std::floor(depth_mult * (p.y - minp_.y)/dims_.y), depth_mult - 1);
-                int cellz = min((int)std::floor(depth_mult * (p.z - minp_.z)/dims_.z), depth_mult - 1); 
+            {
+                //Using floorf due to changes to MSVC 16.9. See details here: https://devtalk.blender.org/t/cuda-compile-error-windows-10/17886/4
+                //floorf is without std:: see why here: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=79700
+                const int cellx = min((int)floorf(depth_mult * min(1.f, max(0.f, (p.x - minp_.x)/dims_.x))), depth_mult - 1);
+                const int celly = min((int)floorf(depth_mult * min(1.f, max(0.f, (p.y - minp_.y)/dims_.y))), depth_mult - 1);
+                const int cellz = min((int)floorf(depth_mult * min(1.f, max(0.f, (p.z - minp_.z)/dims_.z))), depth_mult - 1);
 
                 return Morton::createCode(cellx, celly, cellz);
             }	
@@ -145,4 +153,4 @@ namespace pcl
     }
 }
 
-#endif /* PCL_GPU_OCTREE_MORTON_HPP */
\ No newline at end of file
+#endif /* PCL_GPU_OCTREE_MORTON_HPP */
diff --git a/gpu/people/include/pcl/gpu/people/label_tree.h b/gpu/people/include/pcl/gpu/people/label_tree.h
index f9908c46..e5922482 100644
--- a/gpu/people/include/pcl/gpu/people/label_tree.h
+++ b/gpu/people/include/pcl/gpu/people/label_tree.h
@@ -551,7 +551,7 @@ namespace pcl
         const Blob2& blob = sorted[part_label][part_lid];
 
         // iterate over the number of pixels that are part of this label
-        const std::vector<int>& indices = blob.indices.indices;
+        const auto& indices = blob.indices.indices;
         tree.indices.indices.insert(tree.indices.indices.end(), indices.begin(), indices.end());
 
         if(nr_children == 0)
@@ -583,7 +583,7 @@ namespace pcl
         const Blob2& blob = sorted[part_label][part_lid];
 
         // iterate over the number of pixels that are part of this label
-        const std::vector<int>& indices = blob.indices.indices;
+        const auto& indices = blob.indices.indices;
         tree.indices.indices.insert(tree.indices.indices.end(), indices.begin(), indices.end());
         
         if(nr_children == 0)
diff --git a/gpu/people/include/pcl/gpu/people/people_detector.h b/gpu/people/include/pcl/gpu/people/people_detector.h
index 88cbb0d5..315211e2 100644
--- a/gpu/people/include/pcl/gpu/people/people_detector.h
+++ b/gpu/people/include/pcl/gpu/people/people_detector.h
@@ -157,7 +157,7 @@ namespace pcl
           allocate_buffers (int rows = 480, int cols = 640);
 
           void 
-          shs5 (const pcl::PointCloud<PointT> &cloud, const std::vector<int>& indices, unsigned char *mask);
+          shs5 (const pcl::PointCloud<PointT> &cloud, const pcl::Indices& indices, unsigned char *mask);
 
           //!!! only for debug purposes TODO: remove this. 
           friend class PeoplePCDApp;
diff --git a/gpu/people/src/face_detector.cpp b/gpu/people/src/face_detector.cpp
index 8ff27bfd..d330ebb9 100644
--- a/gpu/people/src/face_detector.cpp
+++ b/gpu/people/src/face_detector.cpp
@@ -565,7 +565,7 @@ pcl::gpu::people::FaceDetector::NCVprocess(pcl::PointCloud<pcl::RGB>&
 
   cloud_out.width = input_gray.width;
   cloud_out.height = input_gray.height;
-  cloud_out.points.resize (input_gray.size ());
+  cloud_out.resize (input_gray.size ());
 
   PCL_ASSERT_ERROR_PRINT_RETURN(!gpu_allocator.isCounting(),"retcode=NCV_NULL_PTR", NCV_NULL_PTR);
 
diff --git a/gpu/people/src/organized_plane_detector.cpp b/gpu/people/src/organized_plane_detector.cpp
index 18bad724..d7d21242 100644
--- a/gpu/people/src/organized_plane_detector.cpp
+++ b/gpu/people/src/organized_plane_detector.cpp
@@ -110,7 +110,7 @@ pcl::gpu::people::OrganizedPlaneDetector::process(const PointCloud<PointTC>::Con
   // Fill in the probabilities
   for(const auto &inlier_index : inlier_indices)                           // iterate over all found planes
   {
-    for(const int &index : inlier_index.indices)                           // iterate over all the indices in that plane
+    for(const auto &index : inlier_index.indices)                           // iterate over all the indices in that plane
     {
       P_l_host_[index].probs[pcl::gpu::people::Background] = 1.f;   // set background at max
     }
@@ -124,11 +124,11 @@ pcl::gpu::people::OrganizedPlaneDetector::allocate_buffers(int rows, int cols)
   PCL_DEBUG("[pcl::gpu::people::OrganizedPlaneDetector::allocate_buffers] : (D) : Called\n");
 
   // Create histogram on host
-  P_l_host_.points.resize(rows*cols);
+  P_l_host_.resize(rows*cols);
   P_l_host_.width = cols;
   P_l_host_.height = rows;
 
-  P_l_host_prev_.points.resize(rows*cols);
+  P_l_host_prev_.resize(rows*cols);
   P_l_host_prev_.width = cols;
   P_l_host_prev_.height = rows;
 
diff --git a/gpu/people/src/people_detector.cpp b/gpu/people/src/people_detector.cpp
index adc3513a..430a59a4 100644
--- a/gpu/people/src/people_detector.cpp
+++ b/gpu/people/src/people_detector.cpp
@@ -96,7 +96,7 @@ pcl::gpu::people::PeopleDetector::allocate_buffers(int rows, int cols)
 
   cloud_host_.width  = cols;
   cloud_host_.height = rows;
-  cloud_host_.points.resize(cols * rows);
+  cloud_host_.resize(cols * rows);
   cloud_host_.is_dense = false;
 
   cloud_host_color_.width = cols;
@@ -106,17 +106,17 @@ pcl::gpu::people::PeopleDetector::allocate_buffers(int rows, int cols)
 
   hue_host_.width  = cols;
   hue_host_.height = rows;
-  hue_host_.points.resize(cols * rows);
+  hue_host_.resize(cols * rows);
   hue_host_.is_dense = false;
 
   depth_host_.width  = cols;
   depth_host_.height = rows;
-  depth_host_.points.resize(cols * rows);
+  depth_host_.resize(cols * rows);
   depth_host_.is_dense = false;
 
   flowermat_host_.width  = cols;
   flowermat_host_.height = rows;
-  flowermat_host_.points.resize(cols * rows);
+  flowermat_host_.resize(cols * rows);
   flowermat_host_.is_dense = false;
   
   cloud_device_.create(rows, cols);
@@ -193,9 +193,9 @@ pcl::gpu::people::PeopleDetector::process ()
     Tree2 t;
     buildTree(sorted, cloud_host_, Neck, c, t);
     
-    const std::vector<int>& seed = t.indices.indices;
+    const auto& seed = t.indices.indices;
         
-    std::fill(flowermat_host_.points.begin(), flowermat_host_.points.end(), 0);
+    std::fill(flowermat_host_.begin(), flowermat_host_.end(), 0);
     {
       //ScopeTime time("shs");    
       shs5(cloud_host_, seed, &flowermat_host_[0]);
@@ -332,9 +332,9 @@ pcl::gpu::people::PeopleDetector::processProb ()
     Tree2 t;
     buildTree(sorted, cloud_host_, Neck, c, t, person_attribs_);
 
-    const std::vector<int>& seed = t.indices.indices;
+    const auto& seed = t.indices.indices;
 
-    std::fill(flowermat_host_.points.begin(), flowermat_host_.points.end(), 0);
+    std::fill(flowermat_host_.begin(), flowermat_host_.end(), 0);
     {
       //ScopeTime time("shs");
       shs5(cloud_host_, seed, &flowermat_host_[0]);
@@ -473,7 +473,7 @@ namespace
 }
 
 void 
-pcl::gpu::people::PeopleDetector::shs5(const pcl::PointCloud<PointT> &cloud, const std::vector<int>& indices, unsigned char *mask)
+pcl::gpu::people::PeopleDetector::shs5(const pcl::PointCloud<PointT> &cloud, const pcl::Indices& indices, unsigned char *mask)
 {
   pcl::device::Intr intr(fx_, fy_, cx_, cy_);
   intr.setDefaultPPIfIncorrect(cloud.width, cloud.height);
diff --git a/gpu/people/tools/CMakeLists.txt b/gpu/people/tools/CMakeLists.txt
index 3d45dbc4..83cf9b4b 100644
--- a/gpu/people/tools/CMakeLists.txt
+++ b/gpu/people/tools/CMakeLists.txt
@@ -6,8 +6,6 @@ if(NOT VTK_FOUND)
 else()
   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()
 
diff --git a/gpu/people/tools/people_app.cpp b/gpu/people/tools/people_app.cpp
index 707ded16..acf452ed 100644
--- a/gpu/people/tools/people_app.cpp
+++ b/gpu/people/tools/people_app.cpp
@@ -155,13 +155,13 @@ class PeoplePCDApp
       depth_view_.setPosition (650, 0);
 
       cmap_device_.create(ROWS, COLS);
-      cmap_host_.points.resize(COLS * ROWS);
+      cmap_host_.resize(COLS * ROWS);
       depth_device_.create(ROWS, COLS);
       image_device_.create(ROWS, COLS);
 
-      depth_host_.points.resize(COLS * ROWS);
+      depth_host_.resize(COLS * ROWS);
 
-      rgba_host_.points.resize(COLS * ROWS);
+      rgba_host_.resize(COLS * ROWS);
       rgb_host_.resize(COLS * ROWS * 3);
 
       pcl::gpu::people::uploadColorMap(color_map_);
@@ -177,7 +177,7 @@ class PeoplePCDApp
       int c;
       cmap_host_.width = cmap_device_.cols();
       cmap_host_.height = cmap_device_.rows();
-      cmap_host_.points.resize(cmap_host_.width * cmap_host_.height);
+      cmap_host_.resize(cmap_host_.width * cmap_host_.height);
       cmap_device_.download(cmap_host_.points, c);
 
       final_view_.showRGBImage<pcl::RGB>(cmap_host_);
@@ -187,7 +187,7 @@ class PeoplePCDApp
       {
         depth_host_.width = people_detector_.depth_device1_.cols();
         depth_host_.height = people_detector_.depth_device1_.rows();
-        depth_host_.points.resize(depth_host_.width * depth_host_.height);        
+        depth_host_.resize(depth_host_.width * depth_host_.height);        
         people_detector_.depth_device1_.download(depth_host_.points, c);        
       }      
       
@@ -235,7 +235,7 @@ class PeoplePCDApp
         const unsigned short *data = depth_wrapper->getDepthMetaData().Data();
         depth_device_.upload(data, s, h, w);
 
-        depth_host_.points.resize(w *h);
+        depth_host_.resize(w *h);
         depth_host_.width = w;
         depth_host_.height = h;
         std::copy(data, data + w * h, &depth_host_[0]);
@@ -250,7 +250,7 @@ class PeoplePCDApp
         image_wrapper->fillRGB(w, h, (unsigned char*)&rgb_host_[0]);
 
         // convert to rgba, TODO image_wrapper should be updated to support rgba directly
-        rgba_host_.points.resize(w * h);
+        rgba_host_.resize(w * h);
         rgba_host_.width = w;
         rgba_host_.height = h;
         for(std::size_t i = 0; i < rgba_host_.size(); ++i)
@@ -433,7 +433,7 @@ int main(int argc, char** argv)
   tree_files.resize(num_trees);
   if (num_trees == 0 || num_trees > 4)
   {
-    PCL_ERROR("[Main] : Invalid number of trees");
+    PCL_ERROR("[Main] : Invalid number of trees\n");
     print_help();
     return -1;
   }
diff --git a/gpu/people/tools/people_pcd_prob.cpp b/gpu/people/tools/people_pcd_prob.cpp
index 55dc210f..c29ac771 100644
--- a/gpu/people/tools/people_pcd_prob.cpp
+++ b/gpu/people/tools/people_pcd_prob.cpp
@@ -48,7 +48,6 @@
 #include <pcl/gpu/people/people_detector.h>
 #include <pcl/gpu/people/colormap.h>
 #include <pcl/visualization/image_viewer.h>
-#include <pcl/search/pcl_search.h>
 #include <Eigen/Core>
 
 #include <pcl/io/png_io.h>
@@ -193,7 +192,7 @@ class PeoplePCDApp
         char val = static_cast<char> (value8);
         pcl::RGB p;
         p.r = val; p.b = val; p.g = val;
-        rgb.points.push_back(p);
+        rgb.push_back(p);
       }
       rgb.width = histograms.width;
       rgb.height = histograms.height;
diff --git a/gpu/segmentation/CMakeLists.txt b/gpu/segmentation/CMakeLists.txt
index f8f3cbb5..9a99a620 100644
--- a/gpu/segmentation/CMakeLists.txt
+++ b/gpu/segmentation/CMakeLists.txt
@@ -6,6 +6,7 @@ set(SUBSYS_DEPS common gpu_containers gpu_utils gpu_octree)
 set(build TRUE)
 PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON)
 PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS})
+PCL_SET_SUBSYS_INCLUDE_DIR("${SUBSYS_NAME}" "${SUBSYS_PATH}")
 mark_as_advanced("BUILD_${SUBSYS_NAME}")
 
 PCL_ADD_DOC("${SUBSYS_NAME}")
@@ -33,7 +34,7 @@ set(impl_incs
 set(LIB_NAME "pcl_${SUBSYS_NAME}")
 include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include")
 PCL_ADD_LIBRARY(${LIB_NAME} COMPONENT ${SUBSYS_NAME} SOURCES ${srcs} ${incs} ${impl_incs})
-target_link_libraries("${LIB_NAME}" pcl_gpu_octree pcl_gpu_utils pcl_gpu_containers)
+target_link_libraries("${LIB_NAME}" pcl_common pcl_gpu_octree pcl_gpu_utils pcl_gpu_containers)
 PCL_MAKE_PKGCONFIG(${LIB_NAME} COMPONENT ${SUBSYS_NAME} DESC ${SUBSYS_DESC} PCL_DEPS ${SUBSYS_DEPS})
 
 # Install include files
diff --git a/gpu/segmentation/include/pcl/gpu/segmentation/gpu_extract_clusters.h b/gpu/segmentation/include/pcl/gpu/segmentation/gpu_extract_clusters.h
index 5e2ecde9..3738663f 100644
--- a/gpu/segmentation/include/pcl/gpu/segmentation/gpu_extract_clusters.h
+++ b/gpu/segmentation/include/pcl/gpu/segmentation/gpu_extract_clusters.h
@@ -50,8 +50,8 @@ namespace pcl
 {
   namespace gpu
   {
-    void
-    extractEuclideanClusters (const pcl::PointCloud<pcl::PointXYZ>::Ptr &host_cloud_,
+    template <typename PointT> void
+    extractEuclideanClusters (const typename pcl::PointCloud<PointT>::Ptr &host_cloud_,
                               const pcl::gpu::Octree::Ptr               &tree,
                               float                                     tolerance,
                               std::vector<PointIndices>                 &clusters,
@@ -62,13 +62,13 @@ namespace pcl
     * \author Koen Buys, Radu Bogdan Rusu
     * \ingroup segmentation
     */
+    template <typename PointT>
     class EuclideanClusterExtraction
     {
       public:
-        using PointType = pcl::PointXYZ;
-        using PointCloudHost = pcl::PointCloud<pcl::PointXYZ>;
-        using PointCloudHostPtr = PointCloudHost::Ptr;
-        using PointCloudHostConstPtr = PointCloudHost::ConstPtr;
+        using PointCloudHost = pcl::PointCloud<PointT>;
+        using PointCloudHostPtr = typename PointCloudHost::Ptr;
+        using PointCloudHostConstPtr = typename PointCloudHost::ConstPtr;
 
         using PointIndicesPtr = PointIndices::Ptr;
         using PointIndicesConstPtr = PointIndices::ConstPtr;
@@ -80,8 +80,7 @@ namespace pcl
 
         //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
         /** \brief Empty constructor. */
-        EuclideanClusterExtraction () : min_pts_per_cluster_ (1), max_pts_per_cluster_ (std::numeric_limits<int>::max ())
-        {};
+        EuclideanClusterExtraction () = default;
 
         /** \brief the destructor */
 /*        ~EuclideanClusterExtraction ()
@@ -143,13 +142,13 @@ namespace pcl
         GPUTreePtr tree_;
 
         /** \brief The spatial cluster tolerance as a measure in the L2 Euclidean space. */
-        double cluster_tolerance_;
+        double cluster_tolerance_ {0};
 
         /** \brief The minimum number of points that a cluster needs to contain in order to be considered valid (default = 1). */
-        int min_pts_per_cluster_;
+        int min_pts_per_cluster_ {1};
 
         /** \brief The maximum number of points that a cluster needs to contain in order to be considered valid (default = MAXINT). */
-        int max_pts_per_cluster_;
+        int max_pts_per_cluster_ {std::numeric_limits<int>::max()};
 
         /** \brief Class getName method. */
         virtual std::string getClassName () const { return ("gpu::EuclideanClusterExtraction"); }
diff --git a/gpu/segmentation/include/pcl/gpu/segmentation/gpu_extract_labeled_clusters.h b/gpu/segmentation/include/pcl/gpu/segmentation/gpu_extract_labeled_clusters.h
index bf5b5e2b..65fc71cc 100644
--- a/gpu/segmentation/include/pcl/gpu/segmentation/gpu_extract_labeled_clusters.h
+++ b/gpu/segmentation/include/pcl/gpu/segmentation/gpu_extract_labeled_clusters.h
@@ -80,8 +80,7 @@ namespace pcl
 
         //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
         /** \brief Empty constructor. */
-        EuclideanLabeledClusterExtraction () : min_pts_per_cluster_ (1), max_pts_per_cluster_ (std::numeric_limits<int>::max ())
-        {};
+        EuclideanLabeledClusterExtraction () = default;
 
         /** \brief Provide a pointer to the search object.
           * \param tree a pointer to the spatial search object.
@@ -137,13 +136,13 @@ namespace pcl
         GPUTreePtr tree_;
 
         /** \brief The spatial cluster tolerance as a measure in the L2 Euclidean space. */
-        double cluster_tolerance_;
+        double cluster_tolerance_ {0};
 
         /** \brief The minimum number of points that a cluster needs to contain in order to be considered valid (default = 1). */
-        int min_pts_per_cluster_;
+        int min_pts_per_cluster_ {1};
 
         /** \brief The maximum number of points that a cluster needs to contain in order to be considered valid (default = MAXINT). */
-        int max_pts_per_cluster_;
+        int max_pts_per_cluster_ {std::numeric_limits<int>::max()};
 
         /** \brief Class getName method. */
         virtual std::string getClassName () const { return ("gpu::EuclideanLabeledClusterExtraction"); }
diff --git a/gpu/segmentation/include/pcl/gpu/segmentation/gpu_seeded_hue_segmentation.h b/gpu/segmentation/include/pcl/gpu/segmentation/gpu_seeded_hue_segmentation.h
index 373f904c..b6c974df 100644
--- a/gpu/segmentation/include/pcl/gpu/segmentation/gpu_seeded_hue_segmentation.h
+++ b/gpu/segmentation/include/pcl/gpu/segmentation/gpu_seeded_hue_segmentation.h
@@ -74,8 +74,7 @@ namespace pcl
 
         //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
         /** \brief Empty constructor. */
-        SeededHueSegmentation () : delta_hue_ (0.0) 
-        {};
+        SeededHueSegmentation () = default;
 
         /** \brief Provide a pointer to the search object.
           * \param tree a pointer to the spatial search object.
@@ -126,10 +125,10 @@ namespace pcl
         GPUTreePtr tree_;
 
         /** \brief The spatial cluster tolerance as a measure in the L2 Euclidean space. */
-        double cluster_tolerance_;
+        double cluster_tolerance_ {0};
 
         /** \brief The allowed difference on the hue*/
-        float delta_hue_;
+        float delta_hue_ {0.0};
 
         /** \brief Class getName method. */
         virtual std::string getClassName () const { return ("gpu::SeededHueSegmentation"); }
diff --git a/gpu/segmentation/include/pcl/gpu/segmentation/impl/gpu_extract_clusters.hpp b/gpu/segmentation/include/pcl/gpu/segmentation/impl/gpu_extract_clusters.hpp
index b5055e02..c654ddc5 100644
--- a/gpu/segmentation/include/pcl/gpu/segmentation/impl/gpu_extract_clusters.hpp
+++ b/gpu/segmentation/include/pcl/gpu/segmentation/impl/gpu_extract_clusters.hpp
@@ -36,13 +36,24 @@
  * @author: Koen Buys
  */
 
-#ifndef PCL_GPU_SEGMENTATION_IMPL_EXTRACT_CLUSTERS_H_
-#define PCL_GPU_SEGMENTATION_IMPL_EXTRACT_CLUSTERS_H_
-
+#pragma once
+#include <pcl/common/copy_point.h>
 #include <pcl/gpu/segmentation/gpu_extract_clusters.h>
 
+namespace pcl {
+namespace detail {
+
+//// Downloads only the neccssary cluster indices from the device to the host.
 void
-pcl::gpu::extractEuclideanClusters (const pcl::PointCloud<pcl::PointXYZ>::Ptr  &host_cloud_,
+economical_download(const pcl::gpu::NeighborIndices& source_indices,
+                    const pcl::Indices& buffer_indices,
+                    std::size_t buffer_size,
+                    pcl::Indices& downloaded_indices);
+} // namespace detail
+} // namespace pcl
+
+template <typename PointT> void
+pcl::gpu::extractEuclideanClusters (const typename pcl::PointCloud<PointT>::Ptr  &host_cloud_,
                                     const pcl::gpu::Octree::Ptr                &tree,
                                     float                                      tolerance,
                                     std::vector<PointIndices>                  &clusters,
@@ -52,6 +63,7 @@ pcl::gpu::extractEuclideanClusters (const pcl::PointCloud<pcl::PointXYZ>::Ptr  &
 
   // Create a bool vector of processed point indices, and initialize it to false
   // cloud is a DeviceArray<PointType>
+  PCL_DEBUG("[pcl::gpu::extractEuclideanClusters]\n");
   std::vector<bool> processed (host_cloud_->size (), false);
 
   int max_answers;
@@ -60,7 +72,7 @@ pcl::gpu::extractEuclideanClusters (const pcl::PointCloud<pcl::PointXYZ>::Ptr  &
     max_answers = host_cloud_->size();
   else
     max_answers = max_pts_per_cluster;
-  std::cout << "Max_answers: " << max_answers << std::endl;
+  PCL_DEBUG("Max_answers: %i\n", max_answers);
 
   // to store the current cluster
   pcl::PointIndices r;
@@ -68,6 +80,8 @@ pcl::gpu::extractEuclideanClusters (const pcl::PointCloud<pcl::PointXYZ>::Ptr  &
   DeviceArray<PointXYZ> queries_device_buffer;
   queries_device_buffer.create(max_answers);
 
+  // Host buffer for results
+  pcl::Indices sizes, data, cpu_tmp;
   // Process all points in the cloud
   for (std::size_t i = 0; i < host_cloud_->size (); ++i)
   {
@@ -80,9 +94,14 @@ pcl::gpu::extractEuclideanClusters (const pcl::PointCloud<pcl::PointXYZ>::Ptr  &
     // Create the query queue on the device, point based not indices
     pcl::gpu::Octree::Queries queries_device;
     // Create the query queue on the host
-	pcl::PointCloud<pcl::PointXYZ>::VectorType queries_host;
+    pcl::PointCloud<pcl::PointXYZ>::VectorType queries_host;
+
+    // Buffer in a new PointXYZ type
+    PointXYZ p;
+    pcl::copyPoint((*host_cloud_)[i], p);
+
     // Push the starting point in the vector
-    queries_host.push_back ((*host_cloud_)[i]);
+    queries_host.push_back (p);
     // Clear vector
     r.indices.clear();
     // Push the starting point in
@@ -96,43 +115,24 @@ pcl::gpu::extractEuclideanClusters (const pcl::PointCloud<pcl::PointXYZ>::Ptr  &
     // once the area stop growing, stop also iterating.
     do
     {
-      // Host buffer for results
-      std::vector<int> sizes, data;
-
+      data.clear();
       // if the number of queries is not high enough implement search on Host here
       if(queries_host.size () <= 10) ///@todo: adjust this to a variable number settable with method
       {
-        std::cout << " CPU: ";
+        PCL_DEBUG(" CPU: ");
         for(std::size_t p = 0; p < queries_host.size (); p++)
         {
           // Execute the radiusSearch on the host
-          tree->radiusSearchHost(queries_host[p], tolerance, data, max_answers);
-        }
-        // Store the previously found number of points
-        previous_found_points = found_points;
-        // Clear queries list
-        queries_host.clear();
-
-        //std::unique(data.begin(), data.end());
-        if(data.size () == 1)
-          continue;
-
-        // Process the results
-        for(std::size_t i = 0; i < data.size (); i++)
-        {
-          if(processed[data[i]])
-            continue;
-          processed[data[i]] = true;
-          queries_host.push_back ((*host_cloud_)[data[i]]);
-          found_points++;
-          r.indices.push_back(data[i]);
+          cpu_tmp.clear();
+          tree->radiusSearchHost(queries_host[p], tolerance, cpu_tmp, max_answers);
+          std::copy(cpu_tmp.begin(), cpu_tmp.end(), std::back_inserter(data));
         }
       }
-
       // If number of queries is high enough do it here
       else
       {
-        std::cout << " GPU: ";
+        PCL_DEBUG(" GPU: ");
+        sizes.clear();
         // Copy buffer
         queries_device = DeviceArray<PointXYZ>(queries_device_buffer.ptr(),queries_host.size());
         // Move queries to GPU
@@ -140,27 +140,33 @@ pcl::gpu::extractEuclideanClusters (const pcl::PointCloud<pcl::PointXYZ>::Ptr  &
         // Execute search
         tree->radiusSearch(queries_device, tolerance, max_answers, result_device);
         // Copy results from GPU to Host
-        result_device.sizes.download (sizes);
-        result_device.data.download (data);
-        // Store the previously found number of points
-        previous_found_points = found_points;
-        // Clear queries list
-        queries_host.clear();
-        for(std::size_t qp = 0; qp < sizes.size (); qp++)
-        {
-          for(int qp_r = 0; qp_r < sizes[qp]; qp_r++)
-          {
-            if(processed[data[qp_r + qp * max_answers]])
-              continue;
-            processed[data[qp_r + qp * max_answers]] = true;
-            queries_host.push_back ((*host_cloud_)[data[qp_r + qp * max_answers]]);
-            found_points++;
-            r.indices.push_back(data[qp_r + qp * max_answers]);
-          }
-        }
+        result_device.sizes.download(sizes);
+        pcl::detail::economical_download(result_device, sizes, max_answers, data);
+      }
+      // Store the previously found number of points
+      previous_found_points = found_points;
+      // Clear queries list
+      queries_host.clear();
+
+      if(data.size () == 1)
+        continue;
+
+      // Process the results
+      for(auto idx : data)
+      {  
+        if(processed[idx])
+          continue;
+        processed[idx] = true;
+        PointXYZ p;
+        pcl::copyPoint((*host_cloud_)[idx], p);
+        queries_host.push_back (p);
+        found_points++;
+        r.indices.push_back(idx);
       }
-      std::cout << " data.size: " << data.size() << " foundpoints: " << found_points << " previous: " << previous_found_points;
-      std::cout << " new points: " << found_points - previous_found_points << " next queries size: " << queries_host.size() << std::endl;
+      PCL_DEBUG(" data.size: %i, foundpoints: %i, previous: %i", data.size() ,
+              found_points, previous_found_points);
+      PCL_DEBUG(" new points: %i, next queries size: %i\n", found_points - previous_found_points,
+             queries_host.size());
     }
     while (previous_found_points < found_points);
     // If this queue is satisfactory, add to the clusters
@@ -176,8 +182,8 @@ pcl::gpu::extractEuclideanClusters (const pcl::PointCloud<pcl::PointXYZ>::Ptr  &
   }
 }
 
-void 
-pcl::gpu::EuclideanClusterExtraction::extract (std::vector<pcl::PointIndices> &clusters)
+template <typename PointT> void
+pcl::gpu::EuclideanClusterExtraction<PointT>::extract (std::vector<pcl::PointIndices> &clusters)
 {
 /*
   // Initialize the GPU search tree
@@ -200,10 +206,11 @@ pcl::gpu::EuclideanClusterExtraction::extract (std::vector<pcl::PointIndices> &c
   }
 */
   // Extract the actual clusters
-  extractEuclideanClusters (host_cloud_, tree_, cluster_tolerance_, clusters, min_pts_per_cluster_, max_pts_per_cluster_);
-  std::cout << "INFO: end of extractEuclideanClusters " << std::endl;
+  extractEuclideanClusters<PointT> (host_cloud_, tree_, cluster_tolerance_, clusters, min_pts_per_cluster_, max_pts_per_cluster_);
+  PCL_DEBUG("INFO: end of extractEuclideanClusters\n");
   // Sort the clusters based on their size (largest one first)
   //std::sort (clusters.rbegin (), clusters.rend (), comparePointClusters);
 }
 
-#endif //PCL_GPU_SEGMENTATION_IMPL_EXTRACT_CLUSTERS_H_
+#define PCL_INSTANTIATE_extractEuclideanClusters(T) template void PCL_EXPORTS pcl::gpu::extractEuclideanClusters<T> (const typename pcl::PointCloud<T>::Ptr  &, const pcl::gpu::Octree::Ptr &,float, std::vector<PointIndices> &, unsigned int, unsigned int);
+#define PCL_INSTANTIATE_EuclideanClusterExtraction(T) template class PCL_EXPORTS pcl::gpu::EuclideanClusterExtraction<T>;
diff --git a/gpu/segmentation/include/pcl/gpu/segmentation/impl/gpu_extract_labeled_clusters.hpp b/gpu/segmentation/include/pcl/gpu/segmentation/impl/gpu_extract_labeled_clusters.hpp
index cbee62f9..af518d99 100644
--- a/gpu/segmentation/include/pcl/gpu/segmentation/impl/gpu_extract_labeled_clusters.hpp
+++ b/gpu/segmentation/include/pcl/gpu/segmentation/impl/gpu_extract_labeled_clusters.hpp
@@ -36,8 +36,7 @@
  *
  */
 
-#ifndef PCL_GPU_SEGMENTATION_IMPL_EXTRACT_LABELED_CLUSTERS_H_
-#define PCL_GPU_SEGMENTATION_IMPL_EXTRACT_LABELED_CLUSTERS_H_
+#pragma once
 
 #include <pcl/gpu/segmentation/gpu_extract_labeled_clusters.h>
 
@@ -176,4 +175,3 @@ pcl::gpu::EuclideanLabeledClusterExtraction<PointT>::extract (std::vector<PointI
 
 #define PCL_INSTANTIATE_extractLabeledEuclideanClusters(T) template void PCL_EXPORTS pcl::gpu::extractLabeledEuclideanClusters<T> (const typename pcl::PointCloud<T>::Ptr  &, const pcl::gpu::Octree::Ptr &,float, std::vector<PointIndices> &, unsigned int, unsigned int);
 #define PCL_INSTANTIATE_EuclideanLabeledClusterExtraction(T) template class PCL_EXPORTS pcl::gpu::EuclideanLabeledClusterExtraction<T>;
-#endif //PCL_GPU_SEGMENTATION_IMPL_EXTRACT_LABELED_CLUSTERS_H_
diff --git a/gpu/segmentation/src/extract_clusters.cpp b/gpu/segmentation/src/extract_clusters.cpp
index fd41feb6..0e5eac3d 100644
--- a/gpu/segmentation/src/extract_clusters.cpp
+++ b/gpu/segmentation/src/extract_clusters.cpp
@@ -35,11 +35,28 @@
  *
  */
 
+#include <pcl/gpu/segmentation/impl/gpu_extract_clusters.hpp>
+#include <pcl/gpu/segmentation/impl/gpu_extract_labeled_clusters.hpp>
 #include <pcl/impl/instantiate.hpp>
 #include <pcl/point_types.h>
-//#include <pcl/gpu/segmentation/gpu_extract_labeled_clusters.h>
-#include <pcl/gpu/segmentation/impl/gpu_extract_labeled_clusters.hpp>
 
 // Instantiations of specific point types
+PCL_INSTANTIATE(extractEuclideanClusters, PCL_XYZ_POINT_TYPES);
+PCL_INSTANTIATE(EuclideanClusterExtraction, PCL_XYZ_POINT_TYPES);
 PCL_INSTANTIATE(extractLabeledEuclideanClusters, PCL_XYZL_POINT_TYPES);
 PCL_INSTANTIATE(EuclideanLabeledClusterExtraction, PCL_XYZL_POINT_TYPES);
+
+void
+pcl::detail::economical_download(const pcl::gpu::NeighborIndices& source_indices,
+                                 const pcl::Indices& buffer_indices,
+                                 std::size_t buffer_size,
+                                 pcl::Indices& downloaded_indices)
+{
+  std::vector<int> tmp;
+  for (std::size_t qp = 0; qp < buffer_indices.size(); qp++) {
+    std::size_t begin = qp * buffer_size;
+    tmp.resize(buffer_indices[qp]);
+    source_indices.data.download(&tmp[0], begin, buffer_indices[qp]);
+    downloaded_indices.insert(downloaded_indices.end(), tmp.begin(), tmp.end());
+  }
+}
diff --git a/gpu/tracking/include/pcl/gpu/tracking/particle_filter.h b/gpu/tracking/include/pcl/gpu/tracking/particle_filter.h
index 82e3e19e..cc0d971d 100644
--- a/gpu/tracking/include/pcl/gpu/tracking/particle_filter.h
+++ b/gpu/tracking/include/pcl/gpu/tracking/particle_filter.h
@@ -11,8 +11,6 @@
 #include <pcl/gpu/kinfu/pixel_rgb.h>
 #include <pcl/tracking/particle_filter.h>
 
-#include <Eigen/Dense>
-
 #include "internal.h"
 
 namespace pcl
diff --git a/io/CMakeLists.txt b/io/CMakeLists.txt
index f2349710..ab918424 100644
--- a/io/CMakeLists.txt
+++ b/io/CMakeLists.txt
@@ -7,7 +7,7 @@ PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON)
 if(WIN32)
   PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS openni openni2 ensenso davidSDK dssdk rssdk rssdk2 pcap png vtk)
 else()
-  PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS openni openni2 ensenso davidSDK dssdk pcap png vtk libusb-1.0)
+  PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS openni openni2 ensenso davidSDK dssdk pcap png vtk libusb)
 endif()
 
 PCL_ADD_DOC("${SUBSYS_NAME}")
@@ -160,8 +160,7 @@ if(WITH_RSSDK)
   )
 endif()
 
-# RSSDK2 is restricted to Windows until it is functional under other OSs
-if(WIN32 AND WITH_RSSDK2)
+if(WITH_RSSDK2)
   set(RSSDK2_GRABBER_INCLUDES
       include/pcl/io/real_sense_2_grabber.h
   )
@@ -170,7 +169,7 @@ if(WIN32 AND WITH_RSSDK2)
   )
 endif()
 
-if(LIBUSB_1_FOUND)
+if(LIBUSB_FOUND)
   set(DINAST_GRABBER_INCLUDES
     include/pcl/io/dinast_grabber.h
   )
@@ -205,6 +204,10 @@ set(PLY_INCLUDES
 )
 
 PCL_ADD_LIBRARY(pcl_io_ply COMPONENT ${SUBSYS_NAME} SOURCES ${PLY_SOURCES} ${PLY_INCLUDES})
+if(MINGW)
+  # libws2_32 isn't added by default for MinGW
+  target_link_libraries(pcl_io_ply ws2_32)
+endif()
 PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/ply" ${PLY_INCLUDES})
 target_include_directories(pcl_io_ply PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include")
 
@@ -224,7 +227,6 @@ set(srcs
   src/hdl_grabber.cpp
   src/vlp_grabber.cpp
   src/robot_eye_grabber.cpp
-  src/file_io.cpp
   src/auto_io.cpp
   src/io_exception.cpp
   ${VTK_IO_SOURCE}
@@ -331,16 +333,30 @@ endif()
 set(LIB_NAME "pcl_${SUBSYS_NAME}")
 
 add_definitions(${VTK_DEFINES})
+
 PCL_ADD_LIBRARY(${LIB_NAME} COMPONENT ${SUBSYS_NAME} SOURCES ${srcs} ${incs} ${compression_incs} ${impl_incs} ${OPENNI_INCLUDES} ${OPENNI2_INCLUDES})
+
 target_include_directories(${LIB_NAME} PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include")
-link_directories(${VTK_LINK_DIRECTORIES})
-target_link_libraries("${LIB_NAME}" pcl_common pcl_io_ply ${VTK_LIBRARIES})
+
+target_link_libraries("${LIB_NAME}" pcl_common pcl_io_ply)
+if(VTK_FOUND)
+  if(${VTK_VERSION} VERSION_LESS 9.0)
+    link_directories(${VTK_LINK_DIRECTORIES})
+    target_link_libraries("${LIB_NAME}" ${VTK_LIBRARIES})
+  else()
+    target_link_libraries("${LIB_NAME}" 
+                          VTK::IOImage
+                          VTK::IOGeometry
+                          VTK::IOPLY)
+  endif()
+endif()
+
 if(PNG_FOUND)
   target_link_libraries("${LIB_NAME}" ${PNG_LIBRARIES})
 endif()
 
-if(LIBUSB_1_FOUND)
-  target_link_libraries("${LIB_NAME}" ${LIBUSB_1_LIBRARIES})
+if(LIBUSB_FOUND)
+  target_link_libraries("${LIB_NAME}" libusb::libusb)
 endif()
 
 if(WITH_OPENNI2)
@@ -349,6 +365,9 @@ endif()
 
 if(WITH_OPENNI)
   target_link_libraries("${LIB_NAME}" ${OPENNI_LIBRARIES})
+  if(VTK_FOUND AND (NOT ${VTK_VERSION} VERSION_LESS 9.0))
+    target_link_libraries("${LIB_NAME}" VTK::FiltersCore VTK::FiltersGeneral)
+  endif()
 endif()
 
 if(WITH_FZAPI)
@@ -374,8 +393,7 @@ if(WITH_RSSDK)
   target_link_libraries(${LIB_NAME} ${RSSDK_LIBRARIES})
 endif()
 
-# RSSDK2 is restricted to Windows until it is functional under other OSs
-if(WIN32 AND WITH_RSSDK2)
+if(WITH_RSSDK2)
   target_link_libraries(${LIB_NAME} ${RSSDK2_LIBRARIES})
 endif()
 
diff --git a/io/include/pcl/compression/color_coding.h b/io/include/pcl/compression/color_coding.h
index aad7cce4..92bb9d88 100644
--- a/io/include/pcl/compression/color_coding.h
+++ b/io/include/pcl/compression/color_coding.h
@@ -37,10 +37,7 @@
 
 #pragma once
 
-#include <cstdio>
-#include <cstring>
 #include <iostream>
-#include <iterator>
 #include <vector>
 
 namespace pcl
@@ -158,18 +155,16 @@ public:
    * \param inputCloud_arg input point cloud
    * */
   void
-  encodeAverageOfPoints (const typename std::vector<int>& indexVector_arg, unsigned char rgba_offset_arg, PointCloudConstPtr inputCloud_arg)
+  encodeAverageOfPoints (const Indices& indexVector_arg, unsigned char rgba_offset_arg, PointCloudConstPtr inputCloud_arg)
   {
-    unsigned int avgRed = 0;
-    unsigned int avgGreen = 0;
-    unsigned int avgBlue = 0;
+    uindex_t avgRed = 0;
+    uindex_t avgGreen = 0;
+    uindex_t avgBlue = 0;
 
     // iterate over points
-    std::size_t len = indexVector_arg.size ();
-    for (std::size_t i = 0; i < len; i++)
+    for (const auto& idx: indexVector_arg)
     {
       // get color information from points
-      const int& idx = indexVector_arg[i];
       const char* idxPointPtr = reinterpret_cast<const char*> (&(*inputCloud_arg)[idx]);
       const int& colorInt = *reinterpret_cast<const int*> (idxPointPtr+rgba_offset_arg);
 
@@ -180,12 +175,13 @@ public:
 
     }
 
+    const uindex_t len = static_cast<uindex_t> (indexVector_arg.size());
     // calculated average color information
     if (len > 1)
     {
-      avgRed   /= static_cast<unsigned int> (len);
-      avgGreen /= static_cast<unsigned int> (len);
-      avgBlue  /= static_cast<unsigned int> (len);
+      avgRed   /= len;
+      avgGreen /= len;
+      avgBlue  /= len;
     }
 
     // remove least significant bits
@@ -205,21 +201,19 @@ public:
    * \param inputCloud_arg input point cloud
    * */
   void
-  encodePoints (const typename std::vector<int>& indexVector_arg, unsigned char rgba_offset_arg, PointCloudConstPtr inputCloud_arg)
+  encodePoints (const Indices& indexVector_arg, unsigned char rgba_offset_arg, PointCloudConstPtr inputCloud_arg)
   {
-    unsigned int avgRed;
-    unsigned int avgGreen;
-    unsigned int avgBlue;
+    uindex_t avgRed;
+    uindex_t avgGreen;
+    uindex_t 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++)
+    for (const auto& idx: indexVector_arg)
     {
       // get color information from point
-      const int& idx = indexVector_arg[i];
       const char* idxPointPtr = reinterpret_cast<const char*> (&(*inputCloud_arg)[idx]);
       const int& colorInt = *reinterpret_cast<const int*> (idxPointPtr+rgba_offset_arg);
 
@@ -230,6 +224,7 @@ public:
 
     }
 
+    const uindex_t len = static_cast<uindex_t> (indexVector_arg.size());
     if (len > 1)
     {
       unsigned char diffRed;
@@ -237,14 +232,13 @@ public:
       unsigned char diffBlue;
 
       // calculated average color information
-      avgRed   /= static_cast<unsigned int> (len);
-      avgGreen /= static_cast<unsigned int> (len);
-      avgBlue  /= static_cast<unsigned int> (len);
+      avgRed   /= len;
+      avgGreen /= len;
+      avgBlue  /= len;
 
       // iterate over points for differential encoding
-      for (std::size_t i = 0; i < len; i++)
+      for (const auto& idx: indexVector_arg)
       {
-        const int& idx = indexVector_arg[i];
         const char* idxPointPtr = reinterpret_cast<const char*> (&(*inputCloud_arg)[idx]);
         const int& colorInt = *reinterpret_cast<const int*> (idxPointPtr+rgba_offset_arg);
 
@@ -284,12 +278,12 @@ public:
     * \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)
+  decodePoints (PointCloudPtr outputCloud_arg, uindex_t beginIdx_arg, uindex_t endIdx_arg, unsigned char rgba_offset_arg)
   {
     assert (beginIdx_arg <= endIdx_arg);
 
     // amount of points to be decoded
-    unsigned int pointCount = static_cast<unsigned int> (endIdx_arg - beginIdx_arg);
+    const index_t pointCount = endIdx_arg - beginIdx_arg;
 
     // get averaged color information for current voxel
     unsigned char avgRed = *(pointAvgColorDataVector_Iterator_++);
@@ -302,7 +296,7 @@ public:
     avgBlue = static_cast<unsigned char> (avgBlue << colorBitReduction_);
 
     // iterate over points
-    for (std::size_t i = 0; i < pointCount; i++)
+    for (index_t i = 0; i < pointCount; i++)
     {
       unsigned int colorInt;
       if (pointCount > 1)
diff --git a/io/include/pcl/compression/entropy_range_coder.h b/io/include/pcl/compression/entropy_range_coder.h
index 74bfcfe5..c449afa0 100644
--- a/io/include/pcl/compression/entropy_range_coder.h
+++ b/io/include/pcl/compression/entropy_range_coder.h
@@ -40,13 +40,9 @@
 
 #pragma once
 
-#include <map>
 #include <iostream>
 #include <vector>
-#include <string>
 #include <cmath>
-#include <algorithm>
-#include <cstdio>
 #include <cstdint>
 
 #include <pcl/pcl_macros.h>
@@ -166,17 +162,6 @@ namespace pcl
     protected:
       using DWord = std::uint32_t; // 4 bytes
 
-      /** \brief Helper function to calculate the binary logarithm
-       * \param n_arg: some value
-       * \return binary logarithm (log2) of argument n_arg
-       */
-      PCL_DEPRECATED(1, 12, "use std::log2 instead")
-      inline double
-      Log2 (double n_arg)
-      {
-        return std::log2 (n_arg);
-      }
-
     private:
       /** \brief Vector containing cumulative symbol frequency table. */
       std::vector<std::uint64_t> cFreqTable_;
diff --git a/io/include/pcl/compression/impl/entropy_range_coder.hpp b/io/include/pcl/compression/impl/entropy_range_coder.hpp
index d348b4d5..31610608 100644
--- a/io/include/pcl/compression/impl/entropy_range_coder.hpp
+++ b/io/include/pcl/compression/impl/entropy_range_coder.hpp
@@ -43,12 +43,10 @@
 #define __PCL_IO_RANGECODING__HPP
 
 #include <pcl/compression/entropy_range_coder.h>
-#include <map>
 #include <iostream>
 #include <vector>
 #include <cstring>
 #include <algorithm>
-#include <cstdio>
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 unsigned long
diff --git a/io/include/pcl/compression/impl/octree_pointcloud_compression.hpp b/io/include/pcl/compression/impl/octree_pointcloud_compression.hpp
index 2fb99d24..098f579f 100644
--- a/io/include/pcl/compression/impl/octree_pointcloud_compression.hpp
+++ b/io/include/pcl/compression/impl/octree_pointcloud_compression.hpp
@@ -38,14 +38,12 @@
 #ifndef OCTREE_COMPRESSION_HPP
 #define OCTREE_COMPRESSION_HPP
 
+#include <pcl/common/io.h> // for getFieldIndex
 #include <pcl/compression/entropy_range_coder.h>
 
-#include <iterator>
 #include <iostream>
 #include <vector>
 #include <cstring>
-#include <iostream>
-#include <cstdio>
 
 namespace pcl
 {
@@ -475,7 +473,7 @@ namespace pcl
         LeafT &leaf_arg, const OctreeKey & key_arg)
     {
       // reference to point indices vector stored within octree leaf
-      const std::vector<int>& leafIdx = leaf_arg.getPointIndicesVector();
+      const auto& leafIdx = leaf_arg.getPointIndicesVector();
 
       if (!do_voxel_grid_enDecoding_)
       {
@@ -526,7 +524,7 @@ namespace pcl
         for (std::size_t i = 0; i < pointCount; i++)
           output_->points.push_back (newPoint);
 
-        // calculcate position of lower voxel corner
+        // calculate position of lower voxel corner
         double lowerVoxelCorner[3];
         lowerVoxelCorner[0] = static_cast<double> (key_arg.x) * this->resolution_ + this->min_x_;
         lowerVoxelCorner[1] = static_cast<double> (key_arg.y) * this->resolution_ + this->min_y_;
diff --git a/io/include/pcl/compression/impl/organized_pointcloud_compression.hpp b/io/include/pcl/compression/impl/organized_pointcloud_compression.hpp
index 4c72a4a5..c2160796 100644
--- a/io/include/pcl/compression/impl/organized_pointcloud_compression.hpp
+++ b/io/include/pcl/compression/impl/organized_pointcloud_compression.hpp
@@ -43,17 +43,10 @@
 #include <pcl/pcl_macros.h>
 #include <pcl/point_cloud.h>
 
-#include <pcl/common/boost.h>
-#include <pcl/common/eigen.h>
-#include <pcl/common/common.h>
-#include <pcl/common/io.h>
-
 #include <pcl/compression/libpng_wrapper.h>
 #include <pcl/compression/organized_pointcloud_conversion.h>
 
-#include <string>
 #include <vector>
-#include <limits>
 #include <cassert>
 
 namespace pcl
diff --git a/io/include/pcl/compression/octree_pointcloud_compression.h b/io/include/pcl/compression/octree_pointcloud_compression.h
index fda475a3..db57e966 100644
--- a/io/include/pcl/compression/octree_pointcloud_compression.h
+++ b/io/include/pcl/compression/octree_pointcloud_compression.h
@@ -37,8 +37,6 @@
 
 #pragma once
 
-#include <pcl/common/common.h>
-#include <pcl/common/io.h>
 #include <pcl/octree/octree2buf_base.h>
 #include <pcl/octree/octree_pointcloud.h>
 #include "entropy_range_coder.h"
@@ -47,10 +45,7 @@
 
 #include "compression_profiles.h"
 
-#include <cstdio>
-#include <cstring>
 #include <iostream>
-#include <iterator>
 #include <vector>
 
 using namespace pcl::octree;
@@ -163,7 +158,7 @@ namespace pcl
          * \param[in] pointIdx_arg the index representing the point in the dataset given by \a setInputCloud to be added
          */
         void
-        addPointIdx (const int pointIdx_arg) override
+        addPointIdx (const uindex_t pointIdx_arg) override
         {
           ++object_count_;
           OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::addPointIdx(pointIdx_arg);
@@ -257,9 +252,6 @@ namespace pcl
         /** \brief Vector for storing binary tree structure */
         std::vector<char> binary_tree_data_vector_;
 
-        /** \brief Iterator on binary tree structure vector */
-        std::vector<char> binary_color_tree_vector_;
-
         /** \brief Vector for storing points per voxel information  */
         std::vector<unsigned int> point_count_data_vector_;
 
diff --git a/io/include/pcl/compression/organized_pointcloud_compression.h b/io/include/pcl/compression/organized_pointcloud_compression.h
index dac8be25..48e6f25a 100644
--- a/io/include/pcl/compression/organized_pointcloud_compression.h
+++ b/io/include/pcl/compression/organized_pointcloud_compression.h
@@ -41,11 +41,6 @@
 #include <pcl/pcl_macros.h>
 #include <pcl/point_cloud.h>
 
-#include <pcl/common/boost.h>
-#include <pcl/common/eigen.h>
-#include <pcl/common/common.h>
-#include <pcl/common/io.h>
-
 #include <pcl/io/openni_camera/openni_shift_to_depth_conversion.h>
 
 #include <vector>
diff --git a/io/include/pcl/compression/organized_pointcloud_conversion.h b/io/include/pcl/compression/organized_pointcloud_conversion.h
index be7792f9..98f64739 100644
--- a/io/include/pcl/compression/organized_pointcloud_conversion.h
+++ b/io/include/pcl/compression/organized_pointcloud_conversion.h
@@ -149,8 +149,8 @@ struct OrganizedConversion<PointT, false>
     assert(disparityData_arg.size()==cloud_size);
 
     // Reset point cloud
-    cloud_arg.points.clear ();
-    cloud_arg.points.reserve (cloud_size);
+    cloud_arg.clear ();
+    cloud_arg.reserve (cloud_size);
 
     // Define point cloud parameters
     cloud_arg.width = static_cast<std::uint32_t> (width_arg);
@@ -189,7 +189,7 @@ struct OrganizedConversion<PointT, false>
           newPoint.x = newPoint.y = newPoint.z = bad_point;
         }
 
-        cloud_arg.points.push_back (newPoint);
+        cloud_arg.push_back (newPoint);
       }
   }
 
@@ -214,8 +214,8 @@ struct OrganizedConversion<PointT, false>
     assert(depthData_arg.size()==cloud_size);
 
     // Reset point cloud
-    cloud_arg.points.clear ();
-    cloud_arg.points.reserve (cloud_size);
+    cloud_arg.clear ();
+    cloud_arg.reserve (cloud_size);
 
     // Define point cloud parameters
     cloud_arg.width = static_cast<std::uint32_t> (width_arg);
@@ -251,7 +251,7 @@ struct OrganizedConversion<PointT, false>
           newPoint.x = newPoint.y = newPoint.z = bad_point;
         }
 
-        cloud_arg.points.push_back (newPoint);
+        cloud_arg.push_back (newPoint);
       }
   }
 };
@@ -380,8 +380,8 @@ struct OrganizedConversion<PointT, true>
     }
 
     // Reset point cloud
-    cloud_arg.points.clear();
-    cloud_arg.points.reserve(cloud_size);
+    cloud_arg.clear();
+    cloud_arg.reserve(cloud_size);
 
     // Define point cloud parameters
     cloud_arg.width = static_cast<std::uint32_t>(width_arg);
@@ -441,7 +441,7 @@ struct OrganizedConversion<PointT, true>
         }
 
         // Add point to cloud
-        cloud_arg.points.push_back(newPoint);
+        cloud_arg.push_back(newPoint);
         // Increment point iterator
         ++i;
     }
@@ -482,8 +482,8 @@ struct OrganizedConversion<PointT, true>
     }
 
     // Reset point cloud
-    cloud_arg.points.clear();
-    cloud_arg.points.reserve(cloud_size);
+    cloud_arg.clear();
+    cloud_arg.reserve(cloud_size);
 
     // Define point cloud parameters
     cloud_arg.width = static_cast<std::uint32_t>(width_arg);
@@ -541,7 +541,7 @@ struct OrganizedConversion<PointT, true>
         }
 
         // Add point to cloud
-        cloud_arg.points.push_back(newPoint);
+        cloud_arg.push_back(newPoint);
         // Increment point iterator
         ++i;
     }
diff --git a/io/include/pcl/compression/point_coding.h b/io/include/pcl/compression/point_coding.h
index 19c45f8a..79db3a0d 100644
--- a/io/include/pcl/compression/point_coding.h
+++ b/io/include/pcl/compression/point_coding.h
@@ -38,10 +38,7 @@
 #pragma once
 
 #include <algorithm>
-#include <cstdio>
-#include <cstring>
 #include <iostream>
-#include <iterator>
 #include <vector>
 
 namespace pcl
@@ -129,18 +126,15 @@ class PointCoding
       * \param inputCloud_arg input point cloud
       */
     void
-    encodePoints (const typename std::vector<int>& indexVector_arg, const double* referencePoint_arg,
+    encodePoints (const Indices& 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++)
+      for (const auto& idx: indexVector_arg)
       {
         unsigned char diffX, diffY, diffZ;
 
         // retrieve point from cloud
-        const int& idx = indexVector_arg[i];
         const PointT& idxPoint = (*inputCloud_arg)[idx];
 
         // differentially encode point coordinates and truncate overflow
@@ -162,15 +156,15 @@ class PointCoding
       * \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)
+    decodePoints (PointCloudPtr outputCloud_arg, const double* referencePoint_arg, uindex_t beginIdx_arg,
+                  uindex_t endIdx_arg)
     {
       assert (beginIdx_arg <= endIdx_arg);
 
-      unsigned int pointCount = static_cast<unsigned int> (endIdx_arg - beginIdx_arg);
+      const uindex_t pointCount = endIdx_arg - beginIdx_arg;
 
       // iterate over points within current voxel
-      for (std::size_t i = 0; i < pointCount; i++)
+      for (uindex_t i = 0; i < pointCount; i++)
       {
         // retrieve differential point information
         const unsigned char& diffX = static_cast<unsigned char> (*(pointDiffDataVectorIterator_++));
diff --git a/io/include/pcl/io/ascii_io.h b/io/include/pcl/io/ascii_io.h
index 9cc02f70..d576d9a4 100644
--- a/io/include/pcl/io/ascii_io.h
+++ b/io/include/pcl/io/ascii_io.h
@@ -113,19 +113,6 @@ namespace pcl
       void 
       setInputFields (const std::vector<pcl::PCLPointField>& fields);
 
-
-      /** \brief Set the ascii file point fields using a point type.
-        * \param[in] p  a point type
-        */
-      template<typename PointT>
-      PCL_DEPRECATED(1, 12, "use parameterless setInputFields<PointT>() instead")
-      inline void setInputFields (const PointT p)
-      {
-        pcl::utils::ignore(p);
-        setInputFields<PointT> ();
-      }
-
-
       /** \brief Set the Separating characters for the ascii point fields 2.
         * \param[in] chars string of separating characters
         *  Sets the separating characters for the point fields.  The
diff --git a/io/include/pcl/io/boost.h b/io/include/pcl/io/boost.h
index 685b36ae..be19192f 100644
--- a/io/include/pcl/io/boost.h
+++ b/io/include/pcl/io/boost.h
@@ -36,7 +36,7 @@
  */
 
 #pragma once
-
+PCL_DEPRECATED_HEADER(1, 15, "Please include the needed boost headers directly.")
 #if defined __GNUC__
 #  pragma GCC system_header
 #endif
diff --git a/io/include/pcl/io/buffers.h b/io/include/pcl/io/buffers.h
index bd8336cc..e8c7ff7d 100644
--- a/io/include/pcl/io/buffers.h
+++ b/io/include/pcl/io/buffers.h
@@ -37,13 +37,9 @@
 
 #pragma once
 
-#include <cassert>
-#include <limits>
 #include <mutex>
 #include <vector>
 
-#include <cstdint>
-
 namespace pcl
 {
 
diff --git a/io/include/pcl/io/debayer.h b/io/include/pcl/io/debayer.h
index f118c5c4..d2797717 100644
--- a/io/include/pcl/io/debayer.h
+++ b/io/include/pcl/io/debayer.h
@@ -38,7 +38,6 @@
 #pragma once
 
 #include <pcl/pcl_exports.h>
-#include <cstdlib>
 
 namespace pcl
 {
diff --git a/io/include/pcl/io/eigen.h b/io/include/pcl/io/eigen.h
index 12d234aa..0979ece9 100644
--- a/io/include/pcl/io/eigen.h
+++ b/io/include/pcl/io/eigen.h
@@ -40,5 +40,6 @@
 #if defined __GNUC__
 #  pragma GCC system_header 
 #endif
+PCL_DEPRECATED_HEADER(1, 15, "Please include the needed eigen headers directly.")
 
 #include <Eigen/Core>
diff --git a/io/include/pcl/io/ensenso_grabber.h b/io/include/pcl/io/ensenso_grabber.h
index e95dd448..c2568485 100644
--- a/io/include/pcl/io/ensenso_grabber.h
+++ b/io/include/pcl/io/ensenso_grabber.h
@@ -42,10 +42,8 @@
 
 #include <pcl/common/time.h>
 #include <pcl/common/io.h>
-#include <pcl/io/eigen.h>
 #include <Eigen/Geometry>
 #include <Eigen/StdVector>
-#include <pcl/io/boost.h>
 
 #include <pcl/io/grabber.h>
 #include <pcl/common/synchronizer.h>
diff --git a/io/include/pcl/io/file_io.h b/io/include/pcl/io/file_io.h
index 635f1f22..ddb6b7aa 100644
--- a/io/include/pcl/io/file_io.h
+++ b/io/include/pcl/io/file_io.h
@@ -40,10 +40,11 @@
 #include <pcl/conversions.h> // for fromPCLPointCloud2, toPCLPointCloud2
 #include <pcl/point_cloud.h> // for PointCloud
 #include <pcl/PCLPointCloud2.h> // for PCLPointCloud2
-#include <pcl/io/boost.h>
 #include <cmath>
 #include <sstream>
 #include <Eigen/Geometry> // for Quaternionf
+#include <boost/numeric/conversion/cast.hpp> // for numeric_cast
+#include <boost/algorithm/string/predicate.hpp> // for iequals
 
 namespace pcl
 {
@@ -234,7 +235,7 @@ namespace pcl
   template <typename Type> inline
   std::enable_if_t<std::is_floating_point<Type>::value>
   copyValueString (const pcl::PCLPointCloud2 &cloud,
-                   const unsigned int point_index, 
+                   const pcl::index_t point_index, 
                    const int point_size, 
                    const unsigned int field_idx, 
                    const unsigned int fields_count, 
@@ -251,7 +252,7 @@ namespace pcl
   template <typename Type> inline
   std::enable_if_t<std::is_integral<Type>::value>
   copyValueString (const pcl::PCLPointCloud2 &cloud,
-                   const unsigned int point_index, 
+                   const pcl::index_t point_index, 
                    const int point_size, 
                    const unsigned int field_idx, 
                    const unsigned int fields_count, 
@@ -264,7 +265,7 @@ namespace pcl
 
   template <> inline void
   copyValueString<std::int8_t> (const pcl::PCLPointCloud2 &cloud,
-                           const unsigned int point_index, 
+                           const pcl::index_t point_index, 
                            const int point_size, 
                            const unsigned int field_idx, 
                            const unsigned int fields_count, 
@@ -278,7 +279,7 @@ namespace pcl
 
   template <> inline void
   copyValueString<std::uint8_t> (const pcl::PCLPointCloud2 &cloud,
-                            const unsigned int point_index, 
+                            const pcl::index_t point_index, 
                             const int point_size, 
                             const unsigned int field_idx, 
                             const unsigned int fields_count, 
@@ -303,7 +304,7 @@ namespace pcl
   template <typename Type> inline
   std::enable_if_t<std::is_floating_point<Type>::value, bool>
   isValueFinite (const pcl::PCLPointCloud2 &cloud,
-                 const unsigned int point_index, 
+                 const pcl::index_t point_index, 
                  const int point_size, 
                  const unsigned int field_idx, 
                  const unsigned int fields_count)
@@ -316,7 +317,7 @@ namespace pcl
   template <typename Type> inline
   std::enable_if_t<std::is_integral<Type>::value, bool>
   isValueFinite (const pcl::PCLPointCloud2& /* cloud */,
-                 const unsigned int /* point_index */,
+                 const pcl::index_t /* point_index */,
                  const int /* point_size */,
                  const unsigned int /* field_idx */,
                  const unsigned int /* fields_count */)
@@ -324,90 +325,119 @@ namespace pcl
     return true;
   }
 
-  /** \brief Copy one single value of type T (uchar, char, uint, int, float, double, ...) from a string
-    * 
-    * Uses aoti/atof to do the conversion.
-    * Checks if the st is "nan" and converts it accordingly.
-    *
-    * \param[in] st the string containing the value to convert and copy
-    * \param[out] cloud the cloud to copy it to
-    * \param[in] point_index the index of the point
-    * \param[in] field_idx the index of the dimension/field
-    * \param[in] fields_count the current fields count
-    */
-  template <typename Type> inline void
-  copyStringValue (const std::string &st, pcl::PCLPointCloud2 &cloud,
-                   unsigned int point_index, unsigned int field_idx, unsigned int fields_count)
+  namespace detail {
+  template <typename Type>
+  inline void
+  copyStringValue(const std::string& st,
+                  pcl::PCLPointCloud2& cloud,
+                  pcl::index_t point_index,
+                  unsigned int field_idx,
+                  unsigned int fields_count,
+                  std::istringstream& is)
   {
     Type value;
-    if (boost::iequals(st, "nan"))
-    {
-      value = std::numeric_limits<Type>::quiet_NaN ();
+    if (boost::iequals(st, "nan")) {
+      value = std::numeric_limits<Type>::quiet_NaN();
       cloud.is_dense = false;
     }
-    else
-    {
-      std::istringstream is (st);
-      is.imbue (std::locale::classic ());
+    else {
+      is.str(st);
       if (!(is >> value))
-        value = static_cast<Type> (atof (st.c_str ()));
+        value = static_cast<Type>(atof(st.c_str()));
     }
 
-    memcpy (&cloud.data[point_index * cloud.point_step + 
-                        cloud.fields[field_idx].offset + 
-                        fields_count * sizeof (Type)], reinterpret_cast<char*> (&value), sizeof (Type));
+    memcpy(&cloud.data[point_index * cloud.point_step + cloud.fields[field_idx].offset +
+                       fields_count * sizeof(Type)],
+           reinterpret_cast<char*>(&value),
+           sizeof(Type));
   }
 
-  template <> inline void
-  copyStringValue<std::int8_t> (const std::string &st, pcl::PCLPointCloud2 &cloud,
-                           unsigned int point_index, unsigned int field_idx, unsigned int fields_count)
+  template <>
+  inline void
+  copyStringValue<std::int8_t>(const std::string& st,
+                               pcl::PCLPointCloud2& cloud,
+                               pcl::index_t point_index,
+                               unsigned int field_idx,
+                               unsigned int fields_count,
+                               std::istringstream& is)
   {
     std::int8_t value;
-    if (boost::iequals(st, "nan"))
-    {
-      value = static_cast<std::int8_t> (std::numeric_limits<int>::quiet_NaN ());
-      cloud.is_dense = false;
-    }
-    else
-    {
-      int val;
-      std::istringstream is (st);
-      is.imbue (std::locale::classic ());
-      //is >> val;  -- unfortunately this fails on older GCC versions and CLANG on MacOS
-      if (!(is >> val))
-        val = static_cast<int> (atof (st.c_str ()));
-      value = static_cast<std::int8_t> (val);
+    int val;
+    is.str(st);
+    // is >> val;  -- unfortunately this fails on older GCC versions and CLANG on MacOS
+    if (!(is >> val)) {
+      val = static_cast<int>(atof(st.c_str()));
     }
+    value = static_cast<std::int8_t>(val);
 
-    memcpy (&cloud.data[point_index * cloud.point_step + 
-                        cloud.fields[field_idx].offset + 
-                        fields_count * sizeof (std::int8_t)], reinterpret_cast<char*> (&value), sizeof (std::int8_t));
+    memcpy(&cloud.data[point_index * cloud.point_step + cloud.fields[field_idx].offset +
+                       fields_count * sizeof(std::int8_t)],
+           reinterpret_cast<char*>(&value),
+           sizeof(std::int8_t));
   }
 
-  template <> inline void
-  copyStringValue<std::uint8_t> (const std::string &st, pcl::PCLPointCloud2 &cloud,
-                           unsigned int point_index, unsigned int field_idx, unsigned int fields_count)
+  template <>
+  inline void
+  copyStringValue<std::uint8_t>(const std::string& st,
+                                pcl::PCLPointCloud2& cloud,
+                                pcl::index_t point_index,
+                                unsigned int field_idx,
+                                unsigned int fields_count,
+                                std::istringstream& is)
   {
     std::uint8_t value;
-    if (boost::iequals(st, "nan"))
-    {
-      value = static_cast<std::uint8_t> (std::numeric_limits<int>::quiet_NaN ());
-      cloud.is_dense = false;
-    }
-    else
-    {
-      int val;
-      std::istringstream is (st);
-      is.imbue (std::locale::classic ());
-      //is >> val;  -- unfortunately this fails on older GCC versions and CLANG on MacOS
-      if (!(is >> val))
-        val = static_cast<int> (atof (st.c_str ()));
-      value = static_cast<std::uint8_t> (val);
+    int val;
+    is.str(st);
+    // is >> val;  -- unfortunately this fails on older GCC versions and CLANG on
+    // MacOS
+    if (!(is >> val)) {
+      val = static_cast<int>(atof(st.c_str()));
     }
+    value = static_cast<std::uint8_t>(val);
 
-    memcpy (&cloud.data[point_index * cloud.point_step + 
-                        cloud.fields[field_idx].offset + 
-                        fields_count * sizeof (std::uint8_t)], reinterpret_cast<char*> (&value), sizeof (std::uint8_t));
+    memcpy(&cloud.data[point_index * cloud.point_step + cloud.fields[field_idx].offset +
+                       fields_count * sizeof(std::uint8_t)],
+           reinterpret_cast<char*>(&value),
+           sizeof(std::uint8_t));
   }
+  } // namespace detail
 
+  /**
+   * \brief Copy one single value of type T (uchar, char, uint, int, float, double, ...) from a string
+   * \details Uses `istringstream` to do the conversion in classic locale
+   * Checks if the st is "nan" and converts it accordingly.
+   *
+   * \param[in] st the string containing the value to convert and copy
+   * \param[out] cloud the cloud to copy it to
+   * \param[in] point_index the index of the point
+   * \param[in] field_idx the index of the dimension/field
+   * \param[in] fields_count the current fields count
+   */
+  template <typename Type> inline void
+  copyStringValue (const std::string &st, pcl::PCLPointCloud2 &cloud,
+                   pcl::index_t point_index, unsigned int field_idx, unsigned int fields_count)
+  {
+    std::istringstream is;
+    is.imbue (std::locale::classic ());
+    detail::copyStringValue<Type> (st, cloud,point_index, field_idx, fields_count, is);
+  }
+/**
+ * \brief Copy one single value of type T (uchar, char, uint, int, float, double, ...) from a string
+ * \details Uses the provided `istringstream` to do the conversion, respecting its locale settings
+ * Checks if the st is "nan" and converts it accordingly.
+ *
+ * \param[in] st the string containing the value to convert and copy
+ * \param[out] cloud the cloud to copy it to
+ * \param[in] point_index the index of the point
+ * \param[in] field_idx the index of the dimension/field
+ * \param[in] fields_count the current fields count
+ * \param[in,out] is input string stream for helping to convert st into cloud
+ */
+  template <typename Type> inline void
+  copyStringValue (const std::string &st, pcl::PCLPointCloud2 &cloud,
+                   pcl::index_t point_index, unsigned int field_idx, unsigned int fields_count,
+                   std::istringstream& is)
+  {
+    detail::copyStringValue<Type> (st, cloud,point_index, field_idx, fields_count, is);
+  }
 }
diff --git a/io/include/pcl/io/grabber.h b/io/include/pcl/io/grabber.h
index 378b81a8..fdbd739c 100644
--- a/io/include/pcl/io/grabber.h
+++ b/io/include/pcl/io/grabber.h
@@ -46,8 +46,8 @@
 #include <vector>
 #include <sstream>
 #include <pcl/pcl_macros.h>
-#include <pcl/io/boost.h>
 #include <pcl/exceptions.h>
+#include <boost/signals2.hpp> // for connection, signal, ...
 
 namespace pcl
 {
@@ -98,18 +98,6 @@ namespace pcl
       template<typename T> boost::signals2::connection
       registerCallback (const std::function<T>& callback);
 
-      /** \brief registers a callback function/method to a signal with the corresponding signature
-        * \param[in] callback: the callback function/method
-        * \return Connection object, that can be used to disconnect the callback method from the signal again.
-        */
-      template<typename T, template<typename> class FunctionT>
-      PCL_DEPRECATED (1, 12, "please assign the callback to a std::function.")
-      boost::signals2::connection
-      registerCallback (const FunctionT<T>& callback)
-      {
-        return registerCallback (std::function<T> (callback));
-      }
-
       /** \brief indicates whether a signal with given parameter-type exists or not
         * \return true if signal exists, false otherwise
         */
diff --git a/io/include/pcl/io/hdl_grabber.h b/io/include/pcl/io/hdl_grabber.h
index c95410c6..3688c2d8 100644
--- a/io/include/pcl/io/hdl_grabber.h
+++ b/io/include/pcl/io/hdl_grabber.h
@@ -71,9 +71,6 @@ namespace pcl
        */
       using sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba = void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &, float, float);
 
-      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
        *         Represents 1 corrected packet from the HDL Velodyne with the returned intensity.
        */
@@ -97,9 +94,6 @@ namespace pcl
        */
       using sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba = void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &);
 
-      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]
        * \param[in] correctionsFile Path to a file which contains the correction parameters for the HDL.  This parameter is mandatory for the HDL-64, optional for the HDL-32
        * \param[in] pcapFile Path to a file which contains previously captured data packets.  This parameter is optional
diff --git a/io/include/pcl/io/ifs_io.h b/io/include/pcl/io/ifs_io.h
index 40694744..6b21808f 100644
--- a/io/include/pcl/io/ifs_io.h
+++ b/io/include/pcl/io/ifs_io.h
@@ -40,7 +40,6 @@
 #include <pcl/point_cloud.h>
 #include <pcl/PCLPointCloud2.h>
 #include <pcl/conversions.h>
-#include <pcl/io/boost.h>
 #include <pcl/PolygonMesh.h>
 
 namespace pcl
diff --git a/io/include/pcl/io/image.h b/io/include/pcl/io/image.h
index 187b388e..090474c6 100644
--- a/io/include/pcl/io/image.h
+++ b/io/include/pcl/io/image.h
@@ -36,7 +36,6 @@
 
 #pragma once
 
-#include <pcl/io/boost.h>
 #include <pcl/io/image_metadata_wrapper.h>
 #include <pcl/memory.h>
 #include <pcl/pcl_config.h>
diff --git a/io/include/pcl/io/image_depth.h b/io/include/pcl/io/image_depth.h
index 2e16ebd9..6a6336cf 100644
--- a/io/include/pcl/io/image_depth.h
+++ b/io/include/pcl/io/image_depth.h
@@ -42,7 +42,6 @@
 
 #include <pcl/pcl_config.h>
 #include <pcl/pcl_exports.h>
-#include <pcl/io/boost.h>
 
 #include<pcl/io/image_metadata_wrapper.h>
 
diff --git a/io/include/pcl/io/image_ir.h b/io/include/pcl/io/image_ir.h
index c0b59311..e2e6fa1e 100644
--- a/io/include/pcl/io/image_ir.h
+++ b/io/include/pcl/io/image_ir.h
@@ -40,7 +40,6 @@
 
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
-#include <pcl/io/boost.h>
 
 #include <pcl/io/image_metadata_wrapper.h>
 
diff --git a/io/include/pcl/io/impl/auto_io.hpp b/io/include/pcl/io/impl/auto_io.hpp
index ab82de93..6a1b50cf 100644
--- a/io/include/pcl/io/impl/auto_io.hpp
+++ b/io/include/pcl/io/impl/auto_io.hpp
@@ -40,12 +40,10 @@
 #ifndef PCL_IO_AUTO_IO_IMPL_H_
 #define PCL_IO_AUTO_IO_IMPL_H_
 
-// #include <pcl/io/file_io.h>
-// #include <pcl/io/boost.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/io/ply_io.h>
 #include <pcl/io/ifs_io.h>
-// #include <pcl/io/vtk_io.h>
+#include <boost/filesystem.hpp> // for path
 
 namespace pcl
 {
@@ -65,7 +63,7 @@ namespace pcl
         result = pcl::io::loadIFSFile (file_name, cloud);
       else
       {
-        PCL_ERROR ("[pcl::io::load] Don't know how to handle file with extension %s", extension.c_str ());
+        PCL_ERROR ("[pcl::io::load] Don't know how to handle file with extension %s\n", extension.c_str ());
         result = -1;
       }
       return (result);
@@ -85,7 +83,7 @@ namespace pcl
         result = pcl::io::saveIFSFile (file_name, cloud);
       else
       {
-        PCL_ERROR ("[pcl::io::save] Don't know how to handle file with extension %s", extension.c_str ());
+        PCL_ERROR ("[pcl::io::save] Don't know how to handle file with extension %s\n", extension.c_str ());
         result = -1;
       }
       return (result);
diff --git a/io/include/pcl/io/impl/buffers.hpp b/io/include/pcl/io/impl/buffers.hpp
index 4a90249f..140e1457 100644
--- a/io/include/pcl/io/impl/buffers.hpp
+++ b/io/include/pcl/io/impl/buffers.hpp
@@ -39,7 +39,6 @@
 #define PCL_IO_IMPL_BUFFERS_HPP
 
 #include <iostream>
-#include <cstring>
 
 #include <pcl/pcl_macros.h>
 
diff --git a/io/include/pcl/io/impl/pcd_io.hpp b/io/include/pcl/io/impl/pcd_io.hpp
index 01a9b72a..bc4535b2 100644
--- a/io/include/pcl/io/impl/pcd_io.hpp
+++ b/io/include/pcl/io/impl/pcd_io.hpp
@@ -40,13 +40,13 @@
 #ifndef PCL_IO_PCD_IO_IMPL_H_
 #define PCL_IO_PCD_IO_IMPL_H_
 
+#include <boost/algorithm/string/trim.hpp> // for trim
 #include <fstream>
 #include <fcntl.h>
 #include <string>
 #include <cstdlib>
 #include <pcl/common/io.h> // for getFields, ...
 #include <pcl/console/print.h>
-#include <pcl/io/boost.h>
 #include <pcl/io/low_level_io.h>
 #include <pcl/io/pcd_io.h>
 
@@ -242,7 +242,7 @@ template <typename PointT> int
 pcl::PCDWriter::writeBinaryCompressed (const std::string &file_name, 
                                        const pcl::PointCloud<PointT> &cloud)
 {
-  if (cloud.points.empty ())
+  if (cloud.empty ())
   {
     throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Input point cloud has no data!");
     return (-1);
@@ -447,7 +447,7 @@ pcl::PCDWriter::writeASCII (const std::string &file_name, const pcl::PointCloud<
   }
 
   std::ofstream fs;
-  fs.open (file_name.c_str ());      // Open file
+  fs.open (file_name.c_str (), std::ios::binary);      // Open file
   
   if (!fs.is_open () || fs.fail ())
   {
@@ -586,9 +586,9 @@ pcl::PCDWriter::writeASCII (const std::string &file_name, const pcl::PointCloud<
 template <typename PointT> int
 pcl::PCDWriter::writeBinary (const std::string &file_name, 
                              const pcl::PointCloud<PointT> &cloud, 
-                             const std::vector<int> &indices)
+                             const pcl::Indices &indices)
 {
-  if (cloud.points.empty () || indices.empty ())
+  if (cloud.empty () || indices.empty ())
   {
     throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Input point cloud has no data or empty indices given!");
     return (-1);
@@ -671,7 +671,7 @@ pcl::PCDWriter::writeBinary (const std::string &file_name,
 
   char *out = &map[0] + data_idx;
   // Copy the data
-  for (const int &index : indices)
+  for (const auto &index : indices)
   {
     int nrj = 0;
     for (const auto &field : fields)
@@ -714,10 +714,10 @@ pcl::PCDWriter::writeBinary (const std::string &file_name,
 template <typename PointT> int
 pcl::PCDWriter::writeASCII (const std::string &file_name, 
                             const pcl::PointCloud<PointT> &cloud, 
-                            const std::vector<int> &indices,
+                            const pcl::Indices &indices,
                             const int precision)
 {
-  if (cloud.points.empty () || indices.empty ())
+  if (cloud.empty () || indices.empty ())
   {
     throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Input point cloud has no data or empty indices given!");
     return (-1);
@@ -730,7 +730,7 @@ pcl::PCDWriter::writeASCII (const std::string &file_name,
   }
 
   std::ofstream fs;
-  fs.open (file_name.c_str ());      // Open file
+  fs.open (file_name.c_str (), std::ios::binary);      // Open file
   if (!fs.is_open () || fs.fail ())
   {
     throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Could not open file for writing!");
@@ -754,7 +754,7 @@ pcl::PCDWriter::writeASCII (const std::string &file_name,
   stream.imbue (std::locale::classic ());
 
   // Iterate through the points
-  for (const int &index : indices)
+  for (const auto &index : indices)
   {
     for (std::size_t d = 0; d < fields.size (); ++d)
     {
diff --git a/io/include/pcl/io/impl/vtk_lib_io.hpp b/io/include/pcl/io/impl/vtk_lib_io.hpp
index 5c2e4b0a..035cae38 100644
--- a/io/include/pcl/io/impl/vtk_lib_io.hpp
+++ b/io/include/pcl/io/impl/vtk_lib_io.hpp
@@ -40,9 +40,7 @@
 #pragma once
 
 // PCL
-#include <pcl/common/io.h>
 #include <pcl/common/point_tests.h> // for pcl::isFinite
-#include <pcl/io/pcd_io.h>
 #include <pcl/point_types.h>
 #include <pcl/type_traits.h>
 
@@ -77,7 +75,7 @@ pcl::io::vtkPolyDataToPointCloud (vtkPolyData* const polydata, pcl::PointCloud<P
   cloud.width = polydata->GetNumberOfPoints ();
   cloud.height = 1; // This indicates that the point cloud is unorganized
   cloud.is_dense = false;
-  cloud.points.resize (cloud.width * cloud.height);
+  cloud.resize (cloud.width * cloud.height);
 
   // Get a list of all the fields available
   std::vector<pcl::PCLPointField> fields;
@@ -166,7 +164,7 @@ pcl::io::vtkStructuredGridToPointCloud (vtkStructuredGrid* const structured_grid
   cloud.width = dimensions[0];
   cloud.height = dimensions[1]; // This indicates that the point cloud is organized
   cloud.is_dense = true;
-  cloud.points.resize (cloud.width * cloud.height);
+  cloud.resize (cloud.width * cloud.height);
 
   // Get a list of all the fields available
   std::vector<pcl::PCLPointField> fields;
diff --git a/io/include/pcl/io/io.h b/io/include/pcl/io/io.h
index e579aab7..ab579c77 100644
--- a/io/include/pcl/io/io.h
+++ b/io/include/pcl/io/io.h
@@ -38,5 +38,5 @@
  */
 
 #pragma once
-
+PCL_DEPRECATED_HEADER(1, 15, "Please include pcl/common/io.h directly.")
 #include <pcl/common/io.h>
diff --git a/io/include/pcl/io/low_level_io.h b/io/include/pcl/io/low_level_io.h
index 39ad6d24..e59d1a63 100644
--- a/io/include/pcl/io/low_level_io.h
+++ b/io/include/pcl/io/low_level_io.h
@@ -51,8 +51,12 @@
 # endif
 # include <io.h>
 # include <windows.h>
-# include <BaseTsd.h>
+# ifdef _MSC_VER
+// ssize_t is already defined in MinGW and its definition conflicts with that of
+// SSIZE_T on a 32-bit target, so do this only for MSVC.
+#  include <basetsd.h>
 using ssize_t = SSIZE_T;
+# endif /* _MSC_VER */
 #else
 # include <unistd.h>
 # include <sys/mman.h>
diff --git a/io/include/pcl/io/oni_grabber.h b/io/include/pcl/io/oni_grabber.h
index 8ab4d59b..fa4e86a3 100644
--- a/io/include/pcl/io/oni_grabber.h
+++ b/io/include/pcl/io/oni_grabber.h
@@ -44,8 +44,6 @@
 #ifdef HAVE_OPENNI
 
 #include <pcl/point_cloud.h>
-#include <pcl/io/eigen.h>
-#include <pcl/io/boost.h>
 #include <pcl/io/grabber.h>
 #include <pcl/io/openni_camera/openni_driver.h>
 #include <pcl/io/openni_camera/openni_device_oni.h>
@@ -53,7 +51,6 @@
 #include <pcl/io/openni_camera/openni_depth_image.h>
 #include <pcl/io/openni_camera/openni_ir_image.h>
 #include <string>
-#include <deque>
 #include <pcl/common/synchronizer.h>
 
 namespace pcl
diff --git a/io/include/pcl/io/openni2/openni2_video_mode.h b/io/include/pcl/io/openni2/openni2_video_mode.h
index dbd87e49..4e6bffa9 100644
--- a/io/include/pcl/io/openni2/openni2_video_mode.h
+++ b/io/include/pcl/io/openni2/openni2_video_mode.h
@@ -31,7 +31,6 @@
 
 #pragma once
 
-#include <cstddef>
 #include <ostream>
 
 #include <OpenNI.h>
diff --git a/io/include/pcl/io/openni2_grabber.h b/io/include/pcl/io/openni2_grabber.h
index edb614e7..e0da3539 100644
--- a/io/include/pcl/io/openni2_grabber.h
+++ b/io/include/pcl/io/openni2_grabber.h
@@ -46,12 +46,9 @@
 #ifdef HAVE_OPENNI2
 
 #include <pcl/point_cloud.h>
-#include <pcl/io/eigen.h>
-#include <pcl/io/boost.h>
 #include <pcl/io/grabber.h>
 #include <pcl/io/openni2/openni2_device.h>
 #include <string>
-#include <deque>
 #include <pcl/common/synchronizer.h>
 
 #include <pcl/io/image.h>
diff --git a/io/include/pcl/io/openni_camera/openni.h b/io/include/pcl/io/openni_camera/openni.h
index c60a0cf1..b49ab988 100644
--- a/io/include/pcl/io/openni_camera/openni.h
+++ b/io/include/pcl/io/openni_camera/openni.h
@@ -45,10 +45,7 @@
 #endif
 
 #include <XnOS.h>
-//work around for qt 5 bug: https://bugreports.qt-project.org/browse/QTBUG-29331
-#ifndef Q_MOC_RUN
 #include <XnCppWrapper.h>
-#endif // Q_MOC_RUN
 #include <XnVersion.h>
 
 #endif
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 46a702df..582948ab 100644
--- a/io/include/pcl/io/openni_camera/openni_depth_image.h
+++ b/io/include/pcl/io/openni_camera/openni_depth_image.h
@@ -47,7 +47,6 @@
 //#include <pcl/pcl_macros.h> // <-- because current header is included in NVCC-compiled code and contains <Eigen/Core>. Consider <pcl/pcl_exports.h>
 #include <pcl/pcl_exports.h>
 #include "openni_exception.h"
-#include <pcl/io/boost.h>
 
 namespace openni_wrapper
 {
diff --git a/io/include/pcl/io/openni_camera/openni_device.h b/io/include/pcl/io/openni_camera/openni_device.h
index b83017f5..fcc8d733 100644
--- a/io/include/pcl/io/openni_camera/openni_device.h
+++ b/io/include/pcl/io/openni_camera/openni_device.h
@@ -44,7 +44,6 @@
 #include "openni_exception.h"
 #include "openni.h"
 
-#include <pcl/io/boost.h>
 #include <pcl/io/openni_camera/openni_image.h>
 #include <pcl/io/openni_camera/openni_depth_image.h>
 #include <pcl/io/openni_camera/openni_ir_image.h>
@@ -55,7 +54,6 @@
 #include <map>
 #include <mutex>
 #include <thread>
-#include <utility>
 #include <vector>
 
 /// @todo Get rid of all exception-specifications, these are useless and soon to be deprecated
diff --git a/io/include/pcl/io/openni_camera/openni_image.h b/io/include/pcl/io/openni_camera/openni_image.h
index 17917229..8b01ad7a 100644
--- a/io/include/pcl/io/openni_camera/openni_image.h
+++ b/io/include/pcl/io/openni_camera/openni_image.h
@@ -43,7 +43,6 @@
 #include <pcl/pcl_exports.h>
 #include "openni.h"
 #include "openni_exception.h"
-#include <pcl/io/boost.h>
 
 namespace openni_wrapper
 {
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 4f32f0e9..c98509ff 100644
--- a/io/include/pcl/io/openni_camera/openni_ir_image.h
+++ b/io/include/pcl/io/openni_camera/openni_ir_image.h
@@ -40,7 +40,6 @@
 #include <pcl/memory.h>
 #include "openni.h"
 #include "openni_exception.h"
-#include <pcl/io/boost.h>
 
 namespace openni_wrapper
 {
diff --git a/io/include/pcl/io/openni_grabber.h b/io/include/pcl/io/openni_grabber.h
index 4f8a8ca7..54e22510 100644
--- a/io/include/pcl/io/openni_grabber.h
+++ b/io/include/pcl/io/openni_grabber.h
@@ -45,8 +45,6 @@
 #ifdef HAVE_OPENNI
 
 #include <pcl/point_cloud.h>
-#include <pcl/io/eigen.h>
-#include <pcl/io/boost.h>
 #include <pcl/io/grabber.h>
 #include <pcl/io/openni_camera/openni_driver.h>
 #include <pcl/io/openni_camera/openni_device_kinect.h>
@@ -54,8 +52,8 @@
 #include <pcl/io/openni_camera/openni_depth_image.h>
 #include <pcl/io/openni_camera/openni_ir_image.h>
 #include <string>
-#include <deque>
 #include <pcl/common/synchronizer.h>
+#include <boost/shared_array.hpp> // for shared_array
 
 namespace pcl
 {
diff --git a/io/include/pcl/io/pcd_io.h b/io/include/pcl/io/pcd_io.h
index e3b58fef..a3e22fb9 100644
--- a/io/include/pcl/io/pcd_io.h
+++ b/io/include/pcl/io/pcd_io.h
@@ -43,6 +43,7 @@
 #include <pcl/pcl_macros.h>
 #include <pcl/point_cloud.h>
 #include <pcl/io/file_io.h>
+#include <boost/interprocess/sync/file_lock.hpp> // for file_lock
 
 namespace pcl
 {
@@ -511,7 +512,7 @@ namespace pcl
       template <typename PointT> int
       writeBinary (const std::string &file_name,
                    const pcl::PointCloud<PointT> &cloud,
-                   const std::vector<int> &indices);
+                   const pcl::Indices &indices);
 
       /** \brief Save point cloud data to a PCD file containing n-D points, in ASCII format
         * \param[in] file_name the output file name
@@ -532,7 +533,7 @@ namespace pcl
       template <typename PointT> int
       writeASCII (const std::string &file_name,
                   const pcl::PointCloud<PointT> &cloud,
-                  const std::vector<int> &indices,
+                  const pcl::Indices &indices,
                   const int precision = 8);
 
       /** \brief Save point cloud data to a PCD file containing n-D points
@@ -575,7 +576,7 @@ namespace pcl
       template<typename PointT> inline int
       write (const std::string &file_name,
              const pcl::PointCloud<PointT> &cloud,
-             const std::vector<int> &indices,
+             const pcl::Indices &indices,
              bool binary = false)
       {
         if (binary)
@@ -757,7 +758,7 @@ namespace pcl
     template<typename PointT> int
     savePCDFile (const std::string &file_name,
                  const pcl::PointCloud<PointT> &cloud,
-                 const std::vector<int> &indices,
+                 const pcl::Indices &indices,
                  const bool binary_mode = false)
     {
       // Save the data
diff --git a/io/include/pcl/io/ply/byte_order.h b/io/include/pcl/io/ply/byte_order.h
index 46325479..8fb57817 100644
--- a/io/include/pcl/io/ply/byte_order.h
+++ b/io/include/pcl/io/ply/byte_order.h
@@ -41,8 +41,8 @@
 
 #include <boost/predef/other/endian.h>
 
-#include <algorithm>
 #include <cstddef>
+#include <utility> // for swap
 
 namespace pcl
 {
diff --git a/io/include/pcl/io/ply/ply.h b/io/include/pcl/io/ply/ply.h
index 3016031a..f658ed69 100644
--- a/io/include/pcl/io/ply/ply.h
+++ b/io/include/pcl/io/ply/ply.h
@@ -40,8 +40,8 @@
 
 #pragma once
 
-#include <pcl/io/boost.h>
 #include <pcl/io/ply/byte_order.h>
+#include <cstdint> // for int8_t, int16_t, ...
 
 /** \file ply.h contains standard typedefs and generic type traits
   * \author Ares Lagae as part of libply, Nizar Sallem
diff --git a/io/include/pcl/io/ply/ply_parser.h b/io/include/pcl/io/ply/ply_parser.h
index 55aee22f..86b6a24f 100644
--- a/io/include/pcl/io/ply/ply_parser.h
+++ b/io/include/pcl/io/ply/ply_parser.h
@@ -40,7 +40,6 @@
 
 #pragma once
 
-#include <pcl/io/boost.h>
 #include <pcl/io/ply/ply.h>
 #include <pcl/io/ply/io_operators.h>
 #include <pcl/pcl_macros.h>
@@ -50,6 +49,13 @@
 #include <string>
 #include <tuple>
 #include <vector>
+#include <boost/lexical_cast.hpp> // for lexical_cast
+#include <boost/mpl/fold.hpp> // for fold
+#include <boost/mpl/inherit.hpp> // for inherit
+#include <boost/mpl/inherit_linearly.hpp> // for inherit_linearly
+#include <boost/mpl/joint_view.hpp> // for joint_view
+#include <boost/mpl/transform.hpp> // for transform
+#include <boost/mpl/vector.hpp> // for vector
 
 namespace pcl
 {
diff --git a/io/include/pcl/io/ply_io.h b/io/include/pcl/io/ply_io.h
index ca0a618f..58c3e844 100644
--- a/io/include/pcl/io/ply_io.h
+++ b/io/include/pcl/io/ply_io.h
@@ -41,7 +41,6 @@
 
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
-#include <pcl/io/boost.h>
 #include <pcl/io/file_io.h>
 #include <pcl/io/ply/ply_parser.h>
 #include <pcl/PolygonMesh.h>
@@ -865,7 +864,7 @@ namespace pcl
       */
     template<typename PointT> int
     savePLYFile (const std::string &file_name, const pcl::PointCloud<PointT> &cloud,
-                 const std::vector<int> &indices, bool binary_mode = false)
+                 const pcl::Indices &indices, bool binary_mode = false)
     {
       // Copy indices to a new point cloud
       pcl::PointCloud<PointT> cloud_out;
diff --git a/io/include/pcl/io/real_sense_2_grabber.h b/io/include/pcl/io/real_sense_2_grabber.h
index 8fe9b1fe..6a1a5e6e 100644
--- a/io/include/pcl/io/real_sense_2_grabber.h
+++ b/io/include/pcl/io/real_sense_2_grabber.h
@@ -40,7 +40,6 @@
 #include <thread>
 #include <mutex>
 
-#include <pcl/io/boost.h>
 #include <pcl/io/grabber.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -66,7 +65,7 @@ namespace pcl
     RealSense2Grabber ( const std::string& file_name_or_serial_number = "", const bool repeat_playback = true );
 
     /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */
-    virtual ~RealSense2Grabber () noexcept;
+    ~RealSense2Grabber ();
 
     /** \brief Set the device options
     * \param[in] width resolution
diff --git a/io/include/pcl/io/robot_eye_grabber.h b/io/include/pcl/io/robot_eye_grabber.h
index 546b0d7d..6f7c7ec1 100644
--- a/io/include/pcl/io/robot_eye_grabber.h
+++ b/io/include/pcl/io/robot_eye_grabber.h
@@ -45,6 +45,7 @@
 #include <pcl/point_cloud.h>
 #include <pcl/memory.h>
 #include <boost/asio.hpp>
+#include <boost/shared_array.hpp> // for shared_array
 
 #include <memory>
 #include <thread>
diff --git a/io/include/pcl/io/vtk_lib_io.h b/io/include/pcl/io/vtk_lib_io.h
index 19cc492d..4ba7bfd9 100644
--- a/io/include/pcl/io/vtk_lib_io.h
+++ b/io/include/pcl/io/vtk_lib_io.h
@@ -46,14 +46,12 @@
 #include <pcl/TextureMesh.h>
 #include <pcl/pcl_macros.h>
 #include <pcl/conversions.h>
-#include <pcl/io/pcd_io.h>
 #include <pcl/range_image/range_image_planar.h>
 
 // Ignore warnings in the above headers
 #ifdef __GNUC__
 #pragma GCC system_header 
 #endif
-#include <vtkVersion.h>
 #include <vtkSmartPointer.h>
 #include <vtkStructuredGrid.h>
 #include <vtkPoints.h>
diff --git a/io/src/ascii_io.cpp b/io/src/ascii_io.cpp
index 4a1971f1..ad5cd7fc 100644
--- a/io/src/ascii_io.cpp
+++ b/io/src/ascii_io.cpp
@@ -40,6 +40,8 @@
 #include <istream>
 #include <fstream>
 #include <boost/filesystem.hpp>
+#include <boost/lexical_cast.hpp> // for lexical_cast
+#include <boost/algorithm/string.hpp> // for split
 #include <cstdint>
 
 //////////////////////////////////////////////////////////////////////////////
diff --git a/io/src/auto_io.cpp b/io/src/auto_io.cpp
index c1bad159..86a206be 100644
--- a/io/src/auto_io.cpp
+++ b/io/src/auto_io.cpp
@@ -59,7 +59,7 @@ pcl::io::load (const std::string& file_name, pcl::PCLPointCloud2& blob)
     result = pcl::io::loadOBJFile (file_name, blob);
   else
   {
-    PCL_ERROR ("[pcl::io::load] Don't know how to handle file with extension %s", extension.c_str ());
+    PCL_ERROR ("[pcl::io::load] Don't know how to handle file with extension %s\n", extension.c_str ());
     result = -1;
   }
   return (result);
@@ -79,7 +79,7 @@ pcl::io::load (const std::string& file_name, pcl::PolygonMesh& mesh)
     result = pcl::io::loadOBJFile (file_name, mesh);
   else
   {
-    PCL_ERROR ("[pcl::io::load] Don't know how to handle file with extension %s", extension.c_str ());
+    PCL_ERROR ("[pcl::io::load] Don't know how to handle file with extension %s\n", extension.c_str ());
     result = -1;
   }
   return (result);
@@ -95,7 +95,7 @@ pcl::io::load (const std::string& file_name, pcl::TextureMesh& mesh)
     result = pcl::io::loadOBJFile (file_name, mesh);
   else
   {
-    PCL_ERROR ("[pcl::io::load] Don't know how to handle file with extension %s", extension.c_str ());
+    PCL_ERROR ("[pcl::io::load] Don't know how to handle file with extension %s\n", extension.c_str ());
     result = -1;
   }
   return (result);
@@ -125,7 +125,7 @@ pcl::io::save (const std::string& file_name, const pcl::PCLPointCloud2& blob, un
     result = pcl::io::saveVTKFile (file_name, blob, precision);
   else
   {
-    PCL_ERROR ("[pcl::io::save] Don't know how to handle file with extension %s", extension.c_str ());
+    PCL_ERROR ("[pcl::io::save] Don't know how to handle file with extension %s\n", extension.c_str ());
     result = -1;
   }
   return (result);
@@ -141,7 +141,7 @@ pcl::io::save (const std::string &file_name, const pcl::TextureMesh &tex_mesh, u
     result = pcl::io::saveOBJFile (file_name, tex_mesh, precision);
   else
   {
-    PCL_ERROR ("[pcl::io::save] Don't know how to handle file with extension %s", extension.c_str ());
+    PCL_ERROR ("[pcl::io::save] Don't know how to handle file with extension %s\n", extension.c_str ());
     result = -1;
   }
   return (result);
@@ -161,7 +161,7 @@ pcl::io::save (const std::string &file_name, const pcl::PolygonMesh &poly_mesh,
     result = pcl::io::saveVTKFile (file_name, poly_mesh, precision);
   else
   {
-    PCL_ERROR ("[pcl::io::save] Don't know how to handle file with extension %s", extension.c_str ());
+    PCL_ERROR ("[pcl::io::save] Don't know how to handle file with extension %s\n", extension.c_str ());
     result = -1;
   }
   return (result);
diff --git a/io/src/debayer.cpp b/io/src/debayer.cpp
index b758f7b8..e22bbe27 100644
--- a/io/src/debayer.cpp
+++ b/io/src/debayer.cpp
@@ -34,6 +34,8 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  *
  */
+#include <cstdlib> // for abs
+
 #include <pcl/io/debayer.h>
 
 #define AVG(a,b) static_cast<unsigned char>((int(a) + int(b)) >> 1)
diff --git a/io/src/dinast_grabber.cpp b/io/src/dinast_grabber.cpp
index 0a65bc01..49b57c1d 100644
--- a/io/src/dinast_grabber.cpp
+++ b/io/src/dinast_grabber.cpp
@@ -47,6 +47,7 @@ pcl::DinastGrabber::DinastGrabber (const int device_position)
   , sync_packet_size_ (512)
   , fov_ (64. * M_PI / 180.)
   , context_ (nullptr)
+  , device_handle_ (nullptr)
   , bulk_ep_ (std::numeric_limits<unsigned char>::max ())
   , second_image_ (false)
   , running_ (false)
diff --git a/io/src/ensenso_grabber.cpp b/io/src/ensenso_grabber.cpp
index 06cdabad..53263e8a 100644
--- a/io/src/ensenso_grabber.cpp
+++ b/io/src/ensenso_grabber.cpp
@@ -123,9 +123,17 @@ pcl::EnsensoGrabber::enumDevices () const
 
     for (int n = 0; n < cams.count (); ++n)
     {
+#if NXLIB_VERSION_MAJOR > 2 
       PCL_INFO ("%s   %s   %s\n", cams[n][itmSerialNumber].asString ().c_str (),
                                   cams[n][itmModelName].asString ().c_str (),
-                                  cams[n][itmStatus].asString ().c_str ());
+                                  cams[n][itmStatus][itmOpen].asBool()
+                                  ? "Open"
+                                  : (cams[n][itmStatus][itmAvailable].asBool() ? "Available" : "In Use"));
+#else
+      PCL_INFO ("%s   %s   %s\n", cams[n][itmSerialNumber].asString().c_str(),
+                                  cams[n][itmModelName].asString().c_str(),
+                                  cams[n][itmStatus].asString().c_str());
+#endif
     }
     PCL_INFO ("\n");
   }
diff --git a/io/src/file_io.cpp b/io/src/file_io.cpp
deleted file mode 100644
index 6db63f2c..00000000
--- a/io/src/file_io.cpp
+++ /dev/null
@@ -1,39 +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 <pcl/io/file_io.h>
-
diff --git a/io/src/hdl_grabber.cpp b/io/src/hdl_grabber.cpp
index 8e116e51..3774f2f8 100644
--- a/io/src/hdl_grabber.cpp
+++ b/io/src/hdl_grabber.cpp
@@ -39,7 +39,6 @@
 #include <thread>
 
 #include <pcl/console/print.h>
-#include <pcl/io/boost.h>
 #include <pcl/io/hdl_grabber.h>
 #include <boost/version.hpp>
 #include <boost/foreach.hpp>
diff --git a/io/src/ifs_io.cpp b/io/src/ifs_io.cpp
index 8b07c4a3..a7a44db3 100644
--- a/io/src/ifs_io.cpp
+++ b/io/src/ifs_io.cpp
@@ -36,13 +36,14 @@
  */
 
 #include <fstream>
-#include <pcl/io/boost.h>
 #include <pcl/common/io.h>
 #include <pcl/io/ifs_io.h>
 #include <pcl/console/time.h>
 
 #include <cstring>
 #include <cerrno>
+#include <boost/filesystem.hpp> // for exists
+#include <boost/iostreams/device/mapped_file.hpp> // for mapped_file_source
 
 ///////////////////////////////////////////////////////////////////////////////////////////
 int
diff --git a/io/src/image_depth.cpp b/io/src/image_depth.cpp
index ea5112e5..8d0a2f82 100644
--- a/io/src/image_depth.cpp
+++ b/io/src/image_depth.cpp
@@ -38,9 +38,7 @@
 #include <pcl/pcl_config.h>
 #include <pcl/io/image_depth.h>
 
-#include <sstream>
 #include <limits>
-#include <iostream>
 
 #include <pcl/io/io_exception.h>
 
diff --git a/io/src/image_grabber.cpp b/io/src/image_grabber.cpp
index 2d3554d6..ce9ac552 100644
--- a/io/src/image_grabber.cpp
+++ b/io/src/image_grabber.cpp
@@ -38,10 +38,12 @@
 #include <pcl/for_each_type.h>
 #include <pcl/io/image_grabber.h>
 #include <pcl/io/lzf_image_io.h>
-#include <pcl/io/pcd_io.h>
 #include <pcl/memory.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
+#include <boost/filesystem.hpp> // for exists, basename, is_directory, ...
+#include <boost/algorithm/string/case_conv.hpp> // for to_upper_copy
+#include <boost/date_time/posix_time/posix_time.hpp> // for posix_time
 
 #ifdef PCL_BUILT_WITH_VTK
   #include <vtkImageReader2.h>
@@ -288,7 +290,7 @@ pcl::ImageGrabberBase::ImageGrabberImpl::loadDepthAndRGBFiles (const std::string
   if (!boost::filesystem::exists (dir) || !boost::filesystem::is_directory (dir))
   {
     PCL_ERROR ("[pcl::ImageGrabber::loadDepthAndRGBFiles] Error: attempted to instantiate a pcl::ImageGrabber from a path which"
-               " is not a directory: %s", dir.c_str ());
+               " is not a directory: %s\n", dir.c_str ());
     return;
   }
   std::string pathname;
@@ -324,13 +326,13 @@ pcl::ImageGrabberBase::ImageGrabberImpl::loadDepthAndRGBFiles (const std::string
   if (!boost::filesystem::exists (depth_dir) || !boost::filesystem::is_directory (depth_dir))
   {
     PCL_ERROR ("[pcl::ImageGrabber::loadDepthAndRGBFiles] Error: attempted to instantiate a pcl::ImageGrabber from a path which"
-               " is not a directory: %s", depth_dir.c_str ());
+               " is not a directory: %s\n", depth_dir.c_str ());
     return;
   }
   if (!boost::filesystem::exists (rgb_dir) || !boost::filesystem::is_directory (rgb_dir))
   {
     PCL_ERROR ("[pcl::ImageGrabber::loadDepthAndRGBFiles] Error: attempted to instantiate a pcl::ImageGrabber from a path which"
-               " is not a directory: %s", rgb_dir.c_str ());
+               " is not a directory: %s\n", rgb_dir.c_str ());
     return;
   }
   std::string pathname;
@@ -368,15 +370,15 @@ pcl::ImageGrabberBase::ImageGrabberImpl::loadDepthAndRGBFiles (const std::string
     }
   }
   if (depth_image_files_.size () != rgb_image_files_.size () )
-    PCL_WARN ("[pcl::ImageGrabberBase::ImageGrabberImpl::loadDepthAndRGBFiles] : Watch out not same amount of depth and rgb images");
+    PCL_WARN ("[pcl::ImageGrabberBase::ImageGrabberImpl::loadDepthAndRGBFiles] : Watch out not same amount of depth and rgb images\n");
   if (!depth_image_files_.empty ())
     sort (depth_image_files_.begin (), depth_image_files_.end ());
   else
-    PCL_ERROR ("[pcl::ImageGrabberBase::ImageGrabberImpl::loadDepthAndRGBFiles] : no depth images added");
+    PCL_ERROR ("[pcl::ImageGrabberBase::ImageGrabberImpl::loadDepthAndRGBFiles] : no depth images added\n");
   if (!rgb_image_files_.empty ())
     sort (rgb_image_files_.begin (), rgb_image_files_.end ());
   else
-    PCL_ERROR ("[pcl::ImageGrabberBase::ImageGrabberImpl::loadDepthAndRGBFiles] : no rgb images added");
+    PCL_ERROR ("[pcl::ImageGrabberBase::ImageGrabberImpl::loadDepthAndRGBFiles] : no rgb images added\n");
 }
 
 
@@ -387,7 +389,7 @@ pcl::ImageGrabberBase::ImageGrabberImpl::loadPCLZFFiles (const std::string &dir)
   if (!boost::filesystem::exists (dir) || !boost::filesystem::is_directory (dir))
   {
     PCL_ERROR ("[pcl::ImageGrabber::loadPCLZFFiles] Error: attempted to instantiate a pcl::ImageGrabber from a path which"
-               " is not a directory: %s", dir.c_str ());
+               " is not a directory: %s\n", dir.c_str ());
     return;
   }
   std::string pathname;
@@ -558,7 +560,7 @@ pcl::ImageGrabberBase::ImageGrabberImpl::getCloudVTK (std::size_t idx,
     cloud_color.width = dims[0];
     cloud_color.height = dims[1];
     cloud_color.is_dense = false;
-    cloud_color.points.resize (depth_image->GetNumberOfPoints ());
+    cloud_color.resize (depth_image->GetNumberOfPoints ());
 
     for (int y = 0; y < dims[1]; ++y)
     {
@@ -596,7 +598,7 @@ pcl::ImageGrabberBase::ImageGrabberImpl::getCloudVTK (std::size_t idx,
     cloud.width = dims[0];
     cloud.height = dims[1];
     cloud.is_dense = false;
-    cloud.points.resize (depth_image->GetNumberOfPoints ());
+    cloud.resize (depth_image->GetNumberOfPoints ());
     for (int y = 0; y < dims[1]; ++y)
     {
       for (int x = 0; x < dims[0]; ++x, ++depth_pixel)
diff --git a/io/src/image_ir.cpp b/io/src/image_ir.cpp
index 0d796182..ba1ff3db 100644
--- a/io/src/image_ir.cpp
+++ b/io/src/image_ir.cpp
@@ -38,10 +38,6 @@
 #include <pcl/pcl_config.h>
 #include <pcl/io/image_ir.h>
 
-#include <sstream>
-#include <limits>
-#include <iostream>
-
 #include <pcl/io/io_exception.h>
 
 using pcl::io::FrameWrapper;
diff --git a/io/src/obj_io.cpp b/io/src/obj_io.cpp
index a16bdc3b..c6cba33c 100644
--- a/io/src/obj_io.cpp
+++ b/io/src/obj_io.cpp
@@ -38,8 +38,10 @@
 #include <pcl/io/obj_io.h>
 #include <fstream>
 #include <pcl/common/io.h>
-#include <pcl/io/boost.h>
 #include <pcl/console/time.h>
+#include <boost/lexical_cast.hpp> // for lexical_cast
+#include <boost/filesystem.hpp> // for exists
+#include <boost/algorithm/string.hpp> // for split
 
 pcl::MTLReader::MTLReader ()
 {
@@ -230,7 +232,7 @@ pcl::MTLReader::read (const std::string& mtl_file_path)
         {
           if (fillRGBfromXYZ (st, *rgb))
           {
-            PCL_ERROR ("[pcl::MTLReader::read] Could not convert %s to RGB values",
+            PCL_ERROR ("[pcl::MTLReader::read] Could not convert %s to RGB values\n",
                        line.c_str ());
             mtl_file.close ();
             materials_.clear ();
@@ -241,7 +243,7 @@ pcl::MTLReader::read (const std::string& mtl_file_path)
         {
           if (fillRGBfromRGB (st, *rgb))
           {
-            PCL_ERROR ("[pcl::MTLReader::read] Could not convert %s to RGB values",
+            PCL_ERROR ("[pcl::MTLReader::read] Could not convert %s to RGB values\n",
                        line.c_str ());
             mtl_file.close ();
             materials_.clear ();
@@ -259,7 +261,7 @@ pcl::MTLReader::read (const std::string& mtl_file_path)
         }
         catch (boost::bad_lexical_cast &)
         {
-          PCL_ERROR ("[pcl::MTLReader::read] Could not convert %s to illumination model",
+          PCL_ERROR ("[pcl::MTLReader::read] Could not convert %s to illumination model\n",
                      line.c_str ());
           mtl_file.close ();
           materials_.clear ();
@@ -279,7 +281,7 @@ pcl::MTLReader::read (const std::string& mtl_file_path)
         }
         catch (boost::bad_lexical_cast &)
         {
-          PCL_ERROR ("[pcl::MTLReader::read] Could not convert %s to transparency value",
+          PCL_ERROR ("[pcl::MTLReader::read] Could not convert %s to transparency value\n",
                      line.c_str ());
           mtl_file.close ();
           materials_.clear ();
@@ -296,7 +298,7 @@ pcl::MTLReader::read (const std::string& mtl_file_path)
         }
         catch (boost::bad_lexical_cast &)
         {
-          PCL_ERROR ("[pcl::MTLReader::read] Could not convert %s to shininess value",
+          PCL_ERROR ("[pcl::MTLReader::read] Could not convert %s to shininess value\n",
                      line.c_str ());
           mtl_file.close ();
           materials_.clear ();
@@ -536,8 +538,8 @@ pcl::OBJReader::read (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
   std::vector<std::string> st;
   try
   {
-    std::size_t point_idx = 0;
-    std::size_t normal_idx = 0;
+    uindex_t point_idx = 0;
+    uindex_t normal_idx = 0;
 
     while (!fs.eof ())
     {
@@ -574,7 +576,7 @@ pcl::OBJReader::read (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
         }
         catch (const boost::bad_lexical_cast&)
         {
-          PCL_ERROR ("Unable to convert %s to vertex coordinates!", line.c_str ());
+          PCL_ERROR ("Unable to convert %s to vertex coordinates!\n", line.c_str ());
           return (-1);
         }
         continue;
@@ -603,7 +605,7 @@ pcl::OBJReader::read (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
         }
         catch (const boost::bad_lexical_cast&)
         {
-          PCL_ERROR ("Unable to convert line %s to vertex normal!", line.c_str ());
+          PCL_ERROR ("Unable to convert line %s to vertex normal!\n", line.c_str ());
           return (-1);
         }
         continue;
@@ -716,7 +718,7 @@ pcl::OBJReader::read (const std::string &file_name, pcl::TextureMesh &mesh,
         }
         catch (const boost::bad_lexical_cast&)
         {
-          PCL_ERROR ("Unable to convert %s to vertex coordinates!", line.c_str ());
+          PCL_ERROR ("Unable to convert %s to vertex coordinates!\n", line.c_str ());
           return (-1);
         }
         continue;
@@ -737,7 +739,7 @@ pcl::OBJReader::read (const std::string &file_name, pcl::TextureMesh &mesh,
         }
         catch (const boost::bad_lexical_cast&)
         {
-          PCL_ERROR ("Unable to convert line %s to vertex normal!", line.c_str ());
+          PCL_ERROR ("Unable to convert line %s to vertex normal!\n", line.c_str ());
           return (-1);
         }
         continue;
@@ -758,7 +760,7 @@ pcl::OBJReader::read (const std::string &file_name, pcl::TextureMesh &mesh,
         }
         catch (const boost::bad_lexical_cast&)
         {
-          PCL_ERROR ("Unable to convert line %s to texture coordinates!", line.c_str ());
+          PCL_ERROR ("Unable to convert line %s to texture coordinates!\n", line.c_str ());
           return (-1);
         }
         continue;
@@ -906,7 +908,7 @@ pcl::OBJReader::read (const std::string &file_name, pcl::PolygonMesh &mesh,
         }
         catch (const boost::bad_lexical_cast&)
         {
-          PCL_ERROR ("Unable to convert %s to vertex coordinates!", line.c_str ());
+          PCL_ERROR ("Unable to convert %s to vertex coordinates!\n", line.c_str ());
           return (-1);
         }
         continue;
@@ -928,7 +930,7 @@ pcl::OBJReader::read (const std::string &file_name, pcl::PolygonMesh &mesh,
         }
         catch (const boost::bad_lexical_cast&)
         {
-          PCL_ERROR ("Unable to convert line %s to vertex normal!", line.c_str ());
+          PCL_ERROR ("Unable to convert line %s to vertex normal!\n", line.c_str ());
           return (-1);
         }
         continue;
diff --git a/io/src/oni_grabber.cpp b/io/src/oni_grabber.cpp
index 24eeb54e..76746241 100644
--- a/io/src/oni_grabber.cpp
+++ b/io/src/oni_grabber.cpp
@@ -42,7 +42,7 @@
 #include <pcl/point_types.h>
 #include <pcl/common/time.h>
 #include <pcl/console/print.h>
-#include <pcl/io/boost.h>  // for boost::shared_array
+#include <boost/shared_array.hpp> // for boost::shared_array
 #include <pcl/memory.h>  // for dynamic_pointer_cast
 #include <pcl/exceptions.h>
 
@@ -313,7 +313,7 @@ ONIGrabber::imageDepthImageCallback(const openni_wrapper::Image::Ptr &image, con
   // check if we have color point cloud slots
   if (point_cloud_rgb_signal_->num_slots () > 0)
   {
-    PCL_WARN ("PointXYZRGB callbacks deprecated. Use PointXYZRGBA instead.");
+    PCL_WARN ("PointXYZRGB callbacks deprecated. Use PointXYZRGBA instead.\n");
     point_cloud_rgb_signal_->operator() (convertToXYZRGBPointCloud (image, depth_image));
   }
 
diff --git a/io/src/openni2/openni2_device.cpp b/io/src/openni2/openni2_device.cpp
index f80eebaa..43f101a3 100644
--- a/io/src/openni2/openni2_device.cpp
+++ b/io/src/openni2/openni2_device.cpp
@@ -501,6 +501,8 @@ pcl::io::openni2::OpenNI2Device::getDefaultIRMode () const
     if ( (mode.x_resolution_ == 640) && (mode.y_resolution_ == 480) && (mode.frame_rate_ == 30.0) )
       return mode;
   }
+  if (modeList.empty())
+      THROW_IO_EXCEPTION("Device claims to have a IR sensor, but doesn't have any IR streaming mode");
   return (modeList.at (0)); // Return first mode if we can't find VGA
 }
 
@@ -514,6 +516,8 @@ pcl::io::openni2::OpenNI2Device::getDefaultColorMode () const
     if ( (mode.x_resolution_ == 640) && (mode.y_resolution_ == 480) && (mode.frame_rate_ == 30.0) )
       return mode;
   }
+  if (modeList.empty())
+      THROW_IO_EXCEPTION("Device claims to have a color sensor, but doesn't have any color streaming mode");
   return (modeList.at (0)); // Return first mode if we can't find VGA
 }
 
@@ -527,6 +531,8 @@ pcl::io::openni2::OpenNI2Device::getDefaultDepthMode () const
     if ( (mode.x_resolution_ == 640) && (mode.y_resolution_ == 480) && (mode.frame_rate_ == 30.0) )
       return mode;
   }
+  if (modeList.empty())
+      THROW_IO_EXCEPTION("Device claims to have a depth sensor, but doesn't have any depth streaming mode");
   return (modeList.at (0)); // Return first mode if we can't find VGA
 }
 
diff --git a/io/src/openni2_grabber.cpp b/io/src/openni2_grabber.cpp
index 0150db14..b81467f4 100644
--- a/io/src/openni2_grabber.cpp
+++ b/io/src/openni2_grabber.cpp
@@ -48,9 +48,9 @@
 #include <pcl/point_types.h>
 #include <pcl/common/time.h>
 #include <pcl/console/print.h>
-#include <pcl/io/boost.h>
 #include <pcl/exceptions.h>
 #include <iostream>
+#include <boost/filesystem.hpp> // for exists
 
 using namespace pcl::io::openni2;
 
diff --git a/io/src/openni_camera/openni_device_primesense.cpp b/io/src/openni_camera/openni_device_primesense.cpp
index aca1a8a0..20072c47 100644
--- a/io/src/openni_camera/openni_device_primesense.cpp
+++ b/io/src/openni_camera/openni_device_primesense.cpp
@@ -45,7 +45,6 @@
 
 #include <pcl/io/openni_camera/openni_device_primesense.h>
 #include <pcl/io/openni_camera/openni_image_yuv_422.h>
-#include <pcl/io/boost.h>
 
 #include <iostream>
 #include <mutex>
diff --git a/io/src/openni_camera/openni_device_xtion.cpp b/io/src/openni_camera/openni_device_xtion.cpp
index cd06198c..56925743 100644
--- a/io/src/openni_camera/openni_device_xtion.cpp
+++ b/io/src/openni_camera/openni_device_xtion.cpp
@@ -44,7 +44,6 @@
 #endif
 
 #include <pcl/io/openni_camera/openni_device_xtion.h>
-#include <pcl/io/boost.h>
 
 #include <mutex>
 #include <sstream>
diff --git a/io/src/openni_camera/openni_driver.cpp b/io/src/openni_camera/openni_driver.cpp
index 070c6b81..65382c39 100644
--- a/io/src/openni_camera/openni_driver.cpp
+++ b/io/src/openni_camera/openni_driver.cpp
@@ -57,8 +57,6 @@
 
 #ifndef _WIN32
 #include <libusb-1.0/libusb.h>
-#else
-#include <pcl/io/boost.h>
 #endif
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/io/src/openni_grabber.cpp b/io/src/openni_grabber.cpp
index cd289890..50844d4a 100644
--- a/io/src/openni_grabber.cpp
+++ b/io/src/openni_grabber.cpp
@@ -44,10 +44,10 @@
 #include <pcl/point_types.h>
 #include <pcl/common/time.h>
 #include <pcl/console/print.h>
-#include <pcl/io/boost.h>
 #include <pcl/exceptions.h>
 #include <iostream>
 #include <thread>
+#include <boost/filesystem.hpp> // for exists
 
 using namespace std::chrono_literals;
 
diff --git a/io/src/pcd_io.cpp b/io/src/pcd_io.cpp
index 1b04bfb1..2f797b24 100644
--- a/io/src/pcd_io.cpp
+++ b/io/src/pcd_io.cpp
@@ -41,7 +41,6 @@
 #include <fcntl.h>
 #include <string>
 #include <cstdlib>
-#include <pcl/io/boost.h>
 #include <pcl/common/utils.h> // pcl::utils::ignore
 #include <pcl/common/io.h>
 #include <pcl/io/low_level_io.h>
@@ -51,6 +50,8 @@
 
 #include <cstring>
 #include <cerrno>
+#include <boost/filesystem.hpp> // for permissions
+#include <boost/algorithm/string.hpp> // for split
 
 ///////////////////////////////////////////////////////////////////////////////////////////
 void
@@ -424,6 +425,10 @@ pcl::PCDReader::readBodyASCII (std::istream &fs, pcl::PCLPointCloud2 &cloud, int
 {
   // Get the number of points the cloud should have
   unsigned int nr_points = cloud.width * cloud.height;
+  // The number of elements each line/point should have
+  const unsigned int elems_per_line = std::accumulate (cloud.fields.cbegin (), cloud.fields.cend (), 0u,
+                                                       [](const auto& i, const auto& field){ return (i + field.count); });
+  PCL_DEBUG ("[pcl::PCDReader::readBodyASCII] Will check that each line in the PCD file has %u elements.\n", elems_per_line);
 
   // Setting the is_dense property to true by default
   cloud.is_dense = true;
@@ -431,6 +436,8 @@ pcl::PCDReader::readBodyASCII (std::istream &fs, pcl::PCLPointCloud2 &cloud, int
   unsigned int idx = 0;
   std::string line;
   std::vector<std::string> st;
+  std::istringstream is;
+  is.imbue (std::locale::classic ());
 
   try
   {
@@ -445,6 +452,14 @@ pcl::PCDReader::readBodyASCII (std::istream &fs, pcl::PCLPointCloud2 &cloud, int
       boost::trim (line);
       boost::split (st, line, boost::is_any_of ("\t\r "), boost::token_compress_on);
 
+      if (st.size () != elems_per_line) // If this is not checked, an exception might occur while accessing st
+      {
+        PCL_WARN ("[pcl::PCDReader::readBodyASCII] Possibly malformed PCD file: point number %u has %zu elements, but should have %u\n",
+                  idx+1, st.size (), elems_per_line);
+        ++idx; // Skip this line/point, but read all others
+        continue;
+      }
+
       if (idx >= nr_points)
       {
         PCL_WARN ("[pcl::PCDReader::read] input has more points (%d) than advertised (%d)!\n", idx, nr_points);
@@ -461,56 +476,56 @@ pcl::PCDReader::readBodyASCII (std::istream &fs, pcl::PCLPointCloud2 &cloud, int
           total += cloud.fields[d].count; // jump over this many elements in the string token
           continue;
         }
-        for (unsigned int c = 0; c < cloud.fields[d].count; ++c)
+        for (uindex_t c = 0; c < cloud.fields[d].count; ++c)
         {
           switch (cloud.fields[d].datatype)
           {
             case pcl::PCLPointField::INT8:
             {
               copyStringValue<pcl::traits::asType<pcl::PCLPointField::INT8>::type> (
-                  st.at (total + c), cloud, idx, d, c);
+                  st.at (total + c), cloud, idx, d, c, is);
               break;
             }
             case pcl::PCLPointField::UINT8:
             {
               copyStringValue<pcl::traits::asType<pcl::PCLPointField::UINT8>::type> (
-                  st.at (total + c), cloud, idx, d, c);
+                  st.at (total + c), cloud, idx, d, c, is);
               break;
             }
             case pcl::PCLPointField::INT16:
             {
               copyStringValue<pcl::traits::asType<pcl::PCLPointField::INT16>::type> (
-                  st.at (total + c), cloud, idx, d, c);
+                  st.at (total + c), cloud, idx, d, c, is);
               break;
             }
             case pcl::PCLPointField::UINT16:
             {
               copyStringValue<pcl::traits::asType<pcl::PCLPointField::UINT16>::type> (
-                  st.at (total + c), cloud, idx, d, c);
+                  st.at (total + c), cloud, idx, d, c, is);
               break;
             }
             case pcl::PCLPointField::INT32:
             {
               copyStringValue<pcl::traits::asType<pcl::PCLPointField::INT32>::type> (
-                  st.at (total + c), cloud, idx, d, c);
+                  st.at (total + c), cloud, idx, d, c, is);
               break;
             }
             case pcl::PCLPointField::UINT32:
             {
               copyStringValue<pcl::traits::asType<pcl::PCLPointField::UINT32>::type> (
-                  st.at (total + c), cloud, idx, d, c);
+                  st.at (total + c), cloud, idx, d, c, is);
               break;
             }
             case pcl::PCLPointField::FLOAT32:
             {
               copyStringValue<pcl::traits::asType<pcl::PCLPointField::FLOAT32>::type> (
-                  st.at (total + c), cloud, idx, d, c);
+                  st.at (total + c), cloud, idx, d, c, is);
               break;
             }
             case pcl::PCLPointField::FLOAT64:
             {
               copyStringValue<pcl::traits::asType<pcl::PCLPointField::FLOAT64>::type> (
-                  st.at (total + c), cloud, idx, d, c);
+                  st.at (total + c), cloud, idx, d, c, is);
               break;
             }
             default:
@@ -597,7 +612,7 @@ pcl::PCDReader::readBodyBinary (const unsigned char *map, pcl::PCLPointCloud2 &c
       toff += fields_sizes[i] * cloud.width * cloud.height;
     }
     // Copy it to the cloud
-    for (std::size_t i = 0; i < cloud.width * cloud.height; ++i)
+    for (uindex_t i = 0; i < cloud.width * cloud.height; ++i)
     {
       for (std::size_t j = 0; j < pters.size (); ++j)
       {
@@ -615,11 +630,11 @@ pcl::PCDReader::readBodyBinary (const unsigned char *map, pcl::PCLPointCloud2 &c
   // Extra checks (not needed for ASCII)
   int point_size = static_cast<int> (cloud.data.size () / (cloud.height * cloud.width));
   // Once copied, we need to go over each field and check if it has NaN/Inf values and assign cloud.is_dense to true or false
-  for (std::uint32_t i = 0; i < cloud.width * cloud.height; ++i)
+  for (uindex_t i = 0; i < cloud.width * cloud.height; ++i)
   {
     for (unsigned int d = 0; d < static_cast<unsigned int> (cloud.fields.size ()); ++d)
     {
-      for (std::uint32_t c = 0; c < cloud.fields[d].count; ++c)
+      for (uindex_t c = 0; c < cloud.fields[d].count; ++c)
       {
         switch (cloud.fields[d].datatype)
         {
@@ -980,7 +995,7 @@ pcl::PCDWriter::generateHeaderBinary (const pcl::PCLPointCloud2 &cloud,
 
   std::stringstream field_names, field_types, field_sizes, field_counts;
   // Check if the size of the fields is smaller than the size of the point step
-  unsigned int toffset = 0;
+  std::size_t toffset = 0;
   for (std::size_t i = 0; i < cloud.fields.size (); ++i)
   {
     // If field offsets do not match, then we need to create fake fields
@@ -1114,7 +1129,7 @@ pcl::PCDWriter::writeASCII (const std::string &file_name, const pcl::PCLPointClo
   std::ofstream fs;
   fs.precision (precision);
   fs.imbue (std::locale::classic ());
-  fs.open (file_name.c_str ());      // Open file
+  fs.open (file_name.c_str (), std::ios::binary);      // Open file
   if (!fs.is_open () || fs.fail ())
   {
     PCL_ERROR("[pcl::PCDWriter::writeASCII] Could not open file '%s' for writing! Error : %s\n", file_name.c_str (), strerror(errno)); 
@@ -1401,7 +1416,7 @@ pcl::PCDWriter::writeBinaryCompressed (std::ostream &os, const pcl::PCLPointClou
   }
 
   // Go over all the points, and copy the data in the appropriate places
-  for (std::size_t i = 0; i < cloud.width * cloud.height; ++i)
+  for (uindex_t i = 0; i < cloud.width * cloud.height; ++i)
   {
     for (std::size_t j = 0; j < pters.size (); ++j)
     {
diff --git a/io/src/ply_io.cpp b/io/src/ply_io.cpp
index 6b5e7b83..5c0dfe1c 100644
--- a/io/src/ply_io.cpp
+++ b/io/src/ply_io.cpp
@@ -38,7 +38,6 @@
 #include <pcl/point_types.h>
 #include <pcl/common/io.h>
 #include <pcl/io/ply_io.h>
-#include <pcl/io/boost.h>
 
 #include <cstdlib>
 #include <fstream>
@@ -49,6 +48,7 @@
 // https://www.boost.org/doc/libs/1_70_0/libs/filesystem/doc/index.htm#Coding-guidelines
 #define BOOST_FILESYSTEM_NO_DEPRECATED
 #include <boost/filesystem.hpp>
+#include <boost/algorithm/string.hpp> // for split
 
 namespace fs = boost::filesystem;
 
@@ -543,6 +543,7 @@ pcl::PLYReader::parse (const std::string& istream_filename)
 
   pcl::io::ply::ply_parser::list_property_definition_callbacks_type list_property_definition_callbacks;
   pcl::io::ply::ply_parser::at<pcl::io::ply::uint8, pcl::io::ply::int32> (list_property_definition_callbacks) = [this] (const std::string& element_name, const std::string& property_name) { return listPropertyDefinitionCallback<pcl::io::ply::uint8, pcl::io::ply::int32> (element_name, property_name); };
+  pcl::io::ply::ply_parser::at<pcl::io::ply::uint8, pcl::io::ply::uint32> (list_property_definition_callbacks) = [this] (const std::string& element_name, const std::string& property_name) { return listPropertyDefinitionCallback<pcl::io::ply::uint8, pcl::io::ply::int32> (element_name, property_name); };
   pcl::io::ply::ply_parser::at<pcl::io::ply::uint32, pcl::io::ply::float64> (list_property_definition_callbacks) = [this] (const std::string& element_name, const std::string& property_name) { return listPropertyDefinitionCallback<pcl::io::ply::uint32, pcl::io::ply::float64> (element_name, property_name); };
   pcl::io::ply::ply_parser::at<pcl::io::ply::uint32, pcl::io::ply::float32> (list_property_definition_callbacks) = [this] (const std::string& element_name, const std::string& property_name) { return listPropertyDefinitionCallback<pcl::io::ply::uint32, pcl::io::ply::float32> (element_name, property_name); };
   pcl::io::ply::ply_parser::at<pcl::io::ply::uint32, pcl::io::ply::uint32> (list_property_definition_callbacks) = [this] (const std::string& element_name, const std::string& property_name) { return listPropertyDefinitionCallback<pcl::io::ply::uint32, pcl::io::ply::uint32> (element_name, property_name); };
@@ -807,7 +808,7 @@ pcl::PLYWriter::generateHeader (const pcl::PCLPointCloud2 &cloud,
         case pcl::PCLPointField::FLOAT64 : oss << " double "; break;
         default :
         {
-          PCL_ERROR ("[pcl::PLYWriter::generateHeader] unknown data field type!");
+          PCL_ERROR ("[pcl::PLYWriter::generateHeader] unknown data field type!\n");
           return ("");
         }
       }
@@ -815,6 +816,9 @@ pcl::PLYWriter::generateHeader (const pcl::PCLPointCloud2 &cloud,
     }
   }
 
+  // vtk requires face entry to load PLY
+  oss << "\nelement face 0";
+
   if (use_camera)
   {
     oss << "\nelement camera 1"
diff --git a/io/src/real_sense_2_grabber.cpp b/io/src/real_sense_2_grabber.cpp
index d13231e5..f5dd3ff9 100644
--- a/io/src/real_sense_2_grabber.cpp
+++ b/io/src/real_sense_2_grabber.cpp
@@ -35,8 +35,6 @@
 *
 */
 
-#include <pcl/io/eigen.h>
-#include <pcl/io/boost.h>
 #include <pcl/io/grabber.h>
 #include <pcl/io/io_exception.h>
 #include <pcl/point_cloud.h>
@@ -228,7 +226,7 @@ namespace pcl
   pcl::PointCloud<pcl::PointXYZ>::Ptr
   RealSense2Grabber::convertDepthToPointXYZ ( const rs2::points& points )
   {
-    return convertRealsensePointsToPointCloud<pcl::PointXYZ> ( points, []( pcl::PointXYZ& p, const rs2::texture_coordinate* uvptr ) {} );
+    return convertRealsensePointsToPointCloud<pcl::PointXYZ> ( points, []( pcl::PointXYZ&, const rs2::texture_coordinate*) {} );
   }
 
   pcl::PointCloud<pcl::PointXYZRGB>::Ptr
@@ -287,15 +285,21 @@ namespace pcl
     cloud->width = sp.width ();
     cloud->height = sp.height ();
     cloud->is_dense = false;
-    cloud->points.resize ( size () );
+    cloud->resize ( points.size () );
 
     const auto cloud_vertices_ptr = points.get_vertices ();
     const auto cloud_texture_ptr = points.get_texture_coordinates ();
 
+#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
 #pragma omp parallel for \
   default(none) \
-  shared(cloud, cloud_vertices_ptr, mapColorFunc)
-    for (int index = 0; index < cloud->size (); ++index)
+  shared(cloud, mapColorFunc)
+#else
+#pragma omp parallel for \
+  default(none) \
+  shared(cloud, cloud_texture_ptr, cloud_vertices_ptr, mapColorFunc)
+#endif
+    for (std::size_t index = 0; index < cloud->size (); ++index)
     {
       const auto ptr = cloud_vertices_ptr + index;
       const auto uvptr = cloud_texture_ptr + index;
diff --git a/io/src/real_sense_grabber.cpp b/io/src/real_sense_grabber.cpp
index e213c883..fbbe8144 100644
--- a/io/src/real_sense_grabber.cpp
+++ b/io/src/real_sense_grabber.cpp
@@ -203,7 +203,7 @@ pcl::RealSenseGrabber::setConfidenceThreshold (unsigned int threshold)
 {
   if (threshold > 15)
   {
-    PCL_WARN ("[pcl::RealSenseGrabber::setConfidenceThreshold] Attempted to set threshold outside valid range (0-15)");
+    PCL_WARN ("[pcl::RealSenseGrabber::setConfidenceThreshold] Attempted to set threshold outside valid range (0-15)\n");
   }
   else
   {
diff --git a/io/src/vtk_lib_io.cpp b/io/src/vtk_lib_io.cpp
index 1916b0b4..f16756d5 100644
--- a/io/src/vtk_lib_io.cpp
+++ b/io/src/vtk_lib_io.cpp
@@ -35,10 +35,10 @@
  *
  */
 
+#include <pcl/io/pcd_io.h> // for loadPCDFile, savePCDFile
 #include <pcl/io/vtk_lib_io.h>
 #include <pcl/io/impl/vtk_lib_io.hpp>
 #include <pcl/PCLPointCloud2.h>
-#include <vtkVersion.h>
 #include <vtkCellArray.h>
 #include <vtkCellData.h>
 #include <vtkDoubleArray.h>
@@ -241,6 +241,11 @@ pcl::io::vtk2mesh (const vtkSmartPointer<vtkPolyData>& poly_data, pcl::PolygonMe
   mesh.cloud.width = mesh.cloud.height = 0;
   mesh.cloud.is_dense = true;
 
+  if (poly_data->GetPoints () == nullptr)
+  {
+    PCL_ERROR ("[pcl::io::vtk2mesh] Given vtkPolyData is misformed (contains nullpointer instead of points).\n");
+    return (0);
+  }
   vtkSmartPointer<vtkPoints> mesh_points = poly_data->GetPoints ();
   vtkIdType nr_points = mesh_points->GetNumberOfPoints ();
   vtkIdType nr_polygons = poly_data->GetNumberOfPolys ();
@@ -343,7 +348,11 @@ pcl::io::vtk2mesh (const vtkSmartPointer<vtkPolyData>& poly_data, pcl::PolygonMe
 
   // Now handle the polygons
   mesh.polygons.resize (nr_polygons);
+#ifdef VTK_CELL_ARRAY_V2
+  vtkIdType const *cell_points;
+#else
   vtkIdType* cell_points;
+#endif
   vtkIdType nr_cell_points;
   vtkCellArray * mesh_polygons = poly_data->GetPolys ();
   mesh_polygons->InitTraversal ();
@@ -404,7 +413,7 @@ pcl::io::vtk2mesh (const vtkSmartPointer<vtkPolyData>& poly_data, pcl::TextureMe
 int
 pcl::io::mesh2vtk (const pcl::PolygonMesh& mesh, vtkSmartPointer<vtkPolyData>& poly_data)
 {
-  unsigned int nr_points = mesh.cloud.width * mesh.cloud.height;
+  auto nr_points = mesh.cloud.width * mesh.cloud.height;
   unsigned int nr_polygons = static_cast<unsigned int> (mesh.polygons.size ());
 
   // reset vtkPolyData object
@@ -465,7 +474,7 @@ pcl::io::mesh2vtk (const pcl::PolygonMesh& mesh, vtkSmartPointer<vtkPolyData>& p
     colors->SetName ("Colors");
     pcl::RGB rgb;
     int offset = (idx_rgb != -1) ? mesh.cloud.fields[idx_rgb].offset : mesh.cloud.fields[idx_rgba].offset;
-    for (vtkIdType cp = 0; cp < nr_points; ++cp)
+    for (pcl::uindex_t cp = 0; cp < nr_points; ++cp)
     {
       memcpy (&rgb, &mesh.cloud.data[cp * mesh.cloud.point_step + offset], sizeof (RGB));
       const unsigned char color[3] = {rgb.r, rgb.g, rgb.b};
@@ -480,7 +489,7 @@ pcl::io::mesh2vtk (const pcl::PolygonMesh& mesh, vtkSmartPointer<vtkPolyData>& p
     vtkSmartPointer<vtkFloatArray> normals = vtkSmartPointer<vtkFloatArray>::New ();
     normals->SetNumberOfComponents (3);
     float nx = 0.0f, ny = 0.0f, nz = 0.0f;
-    for (vtkIdType cp = 0; cp < nr_points; ++cp)
+    for (pcl::uindex_t cp = 0; cp < nr_points; ++cp)
     {
       memcpy (&nx, &mesh.cloud.data[cp*mesh.cloud.point_step+mesh.cloud.fields[idx_normal_x].offset], sizeof (float));
       memcpy (&ny, &mesh.cloud.data[cp*mesh.cloud.point_step+mesh.cloud.fields[idx_normal_y].offset], sizeof (float));
@@ -547,12 +556,12 @@ pcl::io::pointCloudTovtkPolyData(const pcl::PCLPointCloud2Ptr& cloud, vtkSmartPo
   vtkSmartPointer<vtkCellArray> cloud_vertices = vtkSmartPointer<vtkCellArray>::New ();
 
   vtkIdType pid[1];
-  for (std::size_t point_idx = 0; point_idx < cloud->width * cloud->height; point_idx ++)
+  for (uindex_t point_idx = 0; point_idx < cloud->width * cloud->height; point_idx ++)
   {
     float point[3];
 
-    int point_offset = (int (point_idx) * cloud->point_step);
-    int offset = point_offset + cloud->fields[x_idx].offset;
+    auto point_offset = (point_idx * cloud->point_step);
+    auto offset = point_offset + cloud->fields[x_idx].offset;
     memcpy (&point, &cloud->data[offset], sizeof (float)*3);
 
     pid[0] = cloud_points->InsertNextPoint (point);
@@ -573,12 +582,12 @@ pcl::io::pointCloudTovtkPolyData(const pcl::PCLPointCloud2Ptr& cloud, vtkSmartPo
     colors->SetNumberOfComponents (3);
     colors->SetName ("rgb");
 
-    for (std::size_t point_idx = 0; point_idx < cloud->width * cloud->height; point_idx ++)
+    for (uindex_t point_idx = 0; point_idx < cloud->width * cloud->height; point_idx ++)
     {
       unsigned char bgr[3];
 
-      int point_offset = (int (point_idx) * cloud->point_step);
-      int offset = point_offset + cloud->fields[rgb_idx].offset;
+      auto point_offset = (point_idx * cloud->point_step);
+      auto offset = point_offset + cloud->fields[rgb_idx].offset;
       memcpy (&bgr, &cloud->data[offset], sizeof (unsigned char)*3);
 
       colors->InsertNextTuple3(bgr[2], bgr[1], bgr[0]);
@@ -596,12 +605,12 @@ pcl::io::pointCloudTovtkPolyData(const pcl::PCLPointCloud2Ptr& cloud, vtkSmartPo
     cloud_intensity->SetNumberOfComponents (1);
     cloud_intensity->SetName("intensity");
 
-    for (std::size_t point_idx = 0; point_idx < cloud->width * cloud->height; point_idx ++)
+    for (uindex_t point_idx = 0; point_idx < cloud->width * cloud->height; point_idx ++)
     {
       float intensity;
 
-      int point_offset = (int (point_idx) * cloud->point_step);
-      int offset = point_offset + cloud->fields[intensity_idx].offset;
+      auto point_offset = (point_idx * cloud->point_step);
+      auto offset = point_offset + cloud->fields[intensity_idx].offset;
       memcpy (&intensity, &cloud->data[offset], sizeof(float));
 
       cloud_intensity->InsertNextValue(intensity);
@@ -621,12 +630,12 @@ pcl::io::pointCloudTovtkPolyData(const pcl::PCLPointCloud2Ptr& cloud, vtkSmartPo
     normals->SetNumberOfComponents(3); //3d normals (ie x,y,z)
     normals->SetName("normals");
 
-    for (std::size_t point_idx = 0; point_idx < cloud->width * cloud->height; point_idx ++)
+    for (uindex_t point_idx = 0; point_idx < cloud->width * cloud->height; point_idx ++)
     {
       float normal[3];
 
-      int point_offset = (int (point_idx) * cloud->point_step);
-      int offset = point_offset + cloud->fields[normal_x_idx].offset;
+      auto point_offset = (point_idx * cloud->point_step);
+      auto offset = point_offset + cloud->fields[normal_x_idx].offset;
       memcpy (&normal, &cloud->data[offset], sizeof (float)*3);
 
       normals->InsertNextTuple(normal);
diff --git a/io/tools/openni_grabber_example.cpp b/io/tools/openni_grabber_example.cpp
index 28ddb5b5..32448bfb 100644
--- a/io/tools/openni_grabber_example.cpp
+++ b/io/tools/openni_grabber_example.cpp
@@ -40,6 +40,7 @@
 #include <pcl/io/openni_grabber.h>
 #include <pcl/common/time.h>
 #include <pcl/console/parse.h>
+#include <iomanip> // for setprecision
 
 class SimpleOpenNIProcessor
 {
diff --git a/io/tools/openni_pcd_recorder.cpp b/io/tools/openni_pcd_recorder.cpp
index 6d30feec..a8f7e6f8 100644
--- a/io/tools/openni_pcd_recorder.cpp
+++ b/io/tools/openni_pcd_recorder.cpp
@@ -37,6 +37,7 @@
 #include <pcl/point_types.h>
 #include <pcl/io/openni_grabber.h>
 #include <boost/circular_buffer.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp> // for to_iso_string, local_time
 #include <pcl/io/pcd_io.h>
 #include <pcl/console/print.h>
 #include <pcl/console/parse.h>
diff --git a/io/tools/pcd_introduce_nan.cpp b/io/tools/pcd_introduce_nan.cpp
index 74433998..cedebda3 100644
--- a/io/tools/pcd_introduce_nan.cpp
+++ b/io/tools/pcd_introduce_nan.cpp
@@ -36,6 +36,7 @@
  */
 
 #include <pcl/io/pcd_io.h>
+#include <boost/lexical_cast.hpp> // for lexical_cast
 
 /** @brief PCL point object */
 using PointT = pcl::PointXYZRGBA;
diff --git a/kdtree/CMakeLists.txt b/kdtree/CMakeLists.txt
index 826e9026..a5a0fb9b 100644
--- a/kdtree/CMakeLists.txt
+++ b/kdtree/CMakeLists.txt
@@ -19,7 +19,6 @@ set(srcs
 set(incs
   "include/pcl/${SUBSYS_NAME}/kdtree.h"
   "include/pcl/${SUBSYS_NAME}/io.h"
-  "include/pcl/${SUBSYS_NAME}/flann.h"
   "include/pcl/${SUBSYS_NAME}/kdtree_flann.h"
 )
 
diff --git a/kdtree/include/pcl/kdtree/flann.h b/kdtree/include/pcl/kdtree/flann.h
deleted file mode 100644
index a49048d0..00000000
--- a/kdtree/include/pcl/kdtree/flann.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Point Cloud Library (PCL) - www.pointclouds.org
- *  Copyright (c) 2010-2012, Willow Garage, Inc.
- *  Copyright (c) 2012-, Open Perception, Inc.
- *
- *  All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the copyright holder(s) nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- *
- */
-
-#pragma once
-
-#include <pcl/pcl_macros.h>
-
-PCL_DEPRECATED_HEADER(1, 12, "")
-
-#if defined _MSC_VER
-#  pragma warning(disable: 4267 4244)
-#endif
-
-#include <flann/flann.hpp>
-
-#if defined _MSC_VER
-#  pragma warning(default: 4267)
-#endif
diff --git a/kdtree/include/pcl/kdtree/impl/io.hpp b/kdtree/include/pcl/kdtree/impl/io.hpp
index 4e577bee..921b42bb 100644
--- a/kdtree/include/pcl/kdtree/impl/io.hpp
+++ b/kdtree/include/pcl/kdtree/impl/io.hpp
@@ -47,12 +47,12 @@ template <typename Point1T, typename Point2T> void
 pcl::getApproximateIndices (
     const typename pcl::PointCloud<Point1T>::ConstPtr &cloud_in,
     const typename pcl::PointCloud<Point2T>::ConstPtr &cloud_ref,
-    std::vector<int> &indices)
+    Indices &indices)
 {
   pcl::KdTreeFLANN<Point2T> tree;
   tree.setInputCloud (cloud_ref);
 
-  std::vector<int> nn_idx (1);
+  Indices nn_idx (1);
   std::vector<float> nn_dists (1);
   indices.resize (cloud_in->size ());
   for (std::size_t i = 0; i < cloud_in->size (); ++i)
@@ -67,12 +67,12 @@ template <typename PointT> void
 pcl::getApproximateIndices (
     const typename pcl::PointCloud<PointT>::ConstPtr &cloud_in,
     const typename pcl::PointCloud<PointT>::ConstPtr &cloud_ref,
-    std::vector<int> &indices)
+    Indices &indices)
 {
   pcl::KdTreeFLANN<PointT> tree;
   tree.setInputCloud (cloud_ref);
 
-  std::vector<int> nn_idx (1);
+  Indices nn_idx (1);
   std::vector<float> nn_dists (1);
   indices.resize (cloud_in->size ());
   for (std::size_t i = 0; i < cloud_in->size (); ++i)
diff --git a/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp b/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp
index 344fdacb..1b4767b3 100644
--- a/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp
+++ b/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp
@@ -54,11 +54,21 @@ pcl::KdTreeFLANN<PointT, Dist>::KdTreeFLANN (bool sorted)
   , param_k_ (::flann::SearchParams (-1 , epsilon_))
   , param_radius_ (::flann::SearchParams (-1, epsilon_, sorted))
 {
+  if (!std::is_same<std::size_t, pcl::index_t>::value) {
+    const auto message = "FLANN is not optimized for current index type. Will incur "
+                         "extra allocations and copy\n";
+    if (std::is_same<int, pcl::index_t>::value) {
+      PCL_DEBUG(message); // since this has been the default behavior till PCL 1.12
+    }
+    else {
+      PCL_WARN(message);
+    }
+  }
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT, typename Dist>
-pcl::KdTreeFLANN<PointT, Dist>::KdTreeFLANN (const KdTreeFLANN<PointT, Dist> &k) 
+pcl::KdTreeFLANN<PointT, Dist>::KdTreeFLANN (const KdTreeFLANN<PointT, Dist> &k)
   : pcl::KdTree<PointT> (false)
   , flann_index_ ()
   , identity_mapping_ (false)
@@ -70,7 +80,7 @@ pcl::KdTreeFLANN<PointT, Dist>::KdTreeFLANN (const KdTreeFLANN<PointT, Dist> &k)
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT, typename Dist> void 
+template <typename PointT, typename Dist> void
 pcl::KdTreeFLANN<PointT, Dist>::setEpsilon (float eps)
 {
   epsilon_ = eps;
@@ -79,7 +89,7 @@ pcl::KdTreeFLANN<PointT, Dist>::setEpsilon (float eps)
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT, typename Dist> void 
+template <typename PointT, typename Dist> void
 pcl::KdTreeFLANN<PointT, Dist>::setSortedResults (bool sorted)
 {
   sorted_ = sorted;
@@ -88,7 +98,7 @@ pcl::KdTreeFLANN<PointT, Dist>::setSortedResults (bool sorted)
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT, typename Dist> void 
+template <typename PointT, typename Dist> void
 pcl::KdTreeFLANN<PointT, Dist>::setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices)
 {
   cleanup ();   // Perform an automatic cleanup of structures
@@ -98,7 +108,7 @@ pcl::KdTreeFLANN<PointT, Dist>::setInputCloud (const PointCloudConstPtr &cloud,
 
   input_   = cloud;
   indices_ = indices;
-  
+
   // Allocate enough data
   if (!input_)
   {
@@ -113,24 +123,115 @@ pcl::KdTreeFLANN<PointT, Dist>::setInputCloud (const PointCloudConstPtr &cloud,
   {
     convertCloudToArray (*input_);
   }
-  total_nr_points_ = static_cast<int> (index_mapping_.size ());
+  total_nr_points_ = static_cast<uindex_t> (index_mapping_.size ());
   if (total_nr_points_ == 0)
   {
     PCL_ERROR ("[pcl::KdTreeFLANN::setInputCloud] Cannot create a KDTree with an empty input cloud!\n");
     return;
   }
 
-  flann_index_.reset (new FLANNIndex (::flann::Matrix<float> (cloud_.get (), 
-                                                              index_mapping_.size (), 
+  flann_index_.reset (new FLANNIndex (::flann::Matrix<float> (cloud_.get (),
+                                                              index_mapping_.size (),
                                                               dim_),
                                       ::flann::KDTreeSingleIndexParams (15))); // max 15 points/leaf
   flann_index_->buildIndex ();
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////////
+namespace pcl {
+namespace detail {
+// Replace using constexpr in C++17
+template <class IndexT,
+          class A,
+          class B,
+          class C,
+          class D,
+          class F,
+          CompatWithFlann<IndexT> = true>
+int
+knn_search(A& index, B& query, C& k_indices, D& dists, unsigned int k, F& params)
+{
+  // Wrap k_indices vector (no data allocation)
+  ::flann::Matrix<index_t> k_indices_mat(&k_indices[0], 1, k);
+  return index.knnSearch(query, k_indices_mat, dists, k, params);
+}
+
+template <class IndexT,
+          class A,
+          class B,
+          class C,
+          class D,
+          class F,
+          NotCompatWithFlann<IndexT> = true>
+int
+knn_search(A& index, B& query, C& k_indices, D& dists, unsigned int k, F& params)
+{
+  std::vector<std::size_t> indices(k);
+  k_indices.resize(k);
+  // Wrap indices vector (no data allocation)
+  ::flann::Matrix<std::size_t> indices_mat(&indices[0], 1, k);
+  auto ret = index.knnSearch(query, indices_mat, dists, k, params);
+  std::copy(indices.cbegin(), indices.cend(), k_indices.begin());
+  return ret;
+}
+
+template <class IndexT, class A, class B, class F, CompatWithFlann<IndexT> = true>
+int
+knn_search(A& index,
+           B& query,
+           std::vector<Indices>& k_indices,
+           std::vector<std::vector<float>>& dists,
+           unsigned int k,
+           F& params)
+{
+  return index.knnSearch(query, k_indices, dists, k, params);
+}
+
+template <class IndexT, class A, class B, class F, NotCompatWithFlann<IndexT> = true>
+int
+knn_search(A& index,
+           B& query,
+           std::vector<Indices>& k_indices,
+           std::vector<std::vector<float>>& dists,
+           unsigned int k,
+           F& params)
+{
+  std::vector<std::vector<std::size_t>> indices;
+  // flann will resize accordingly
+  auto ret = index.knnSearch(query, indices, dists, k, params);
+
+  k_indices.resize(indices.size());
+  {
+    auto it = indices.cbegin();
+    auto jt = k_indices.begin();
+    for (; it != indices.cend(); ++it, ++jt) {
+      jt->resize(it->size());
+      std::copy(it->cbegin(), it->cend(), jt->begin());
+    }
+  }
+  return ret;
+}
+} // namespace detail
+template <class FlannIndex,
+          class Query,
+          class Indices,
+          class Distances,
+          class SearchParams>
+int
+knn_search(const FlannIndex& index,
+           const Query& query,
+           Indices& indices,
+           Distances& dists,
+           unsigned int k,
+           const SearchParams& params)
+{
+  return detail::knn_search<pcl::index_t>(index, query, indices, dists, k, params);
+}
+} // namespace pcl
+
 template <typename PointT, typename Dist> int 
-pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch (const PointT &point, int k, 
-                                                std::vector<int> &k_indices, 
+pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch (const PointT &point, unsigned int k,
+                                                Indices &k_indices,
                                                 std::vector<float> &k_distances) const
 {
   assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
@@ -141,22 +242,28 @@ pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch (const PointT &point, int k,
   k_indices.resize (k);
   k_distances.resize (k);
 
+  if (k==0)
+    return 0;
+
   std::vector<float> query (dim_);
   point_representation_->vectorize (static_cast<PointT> (point), query);
 
-  ::flann::Matrix<int> k_indices_mat (&k_indices[0], 1, k);
+  // Wrap the k_distances vector (no data copy)
   ::flann::Matrix<float> k_distances_mat (&k_distances[0], 1, k);
-  // Wrap the k_indices and k_distances vectors (no data copy)
-  flann_index_->knnSearch (::flann::Matrix<float> (&query[0], 1, dim_), 
-                           k_indices_mat, k_distances_mat,
-                           k, param_k_);
+
+  knn_search(*flann_index_,
+             ::flann::Matrix<float>(&query[0], 1, dim_),
+             k_indices,
+             k_distances_mat,
+             k,
+             param_k_);
 
   // Do mapping to original point cloud
-  if (!identity_mapping_) 
+  if (!identity_mapping_)
   {
     for (std::size_t i = 0; i < static_cast<std::size_t> (k); ++i)
     {
-      int& neighbor_index = k_indices[i];
+      auto& neighbor_index = k_indices[i];
       neighbor_index = index_mapping_[neighbor_index];
     }
   }
@@ -165,8 +272,99 @@ pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch (const PointT &point, int k,
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT, typename Dist> int 
-pcl::KdTreeFLANN<PointT, Dist>::radiusSearch (const PointT &point, double radius, std::vector<int> &k_indices,
+namespace pcl {
+namespace detail {
+// Replace using constexpr in C++17
+template <class IndexT,
+          class A,
+          class B,
+          class C,
+          class D,
+          class F,
+          CompatWithFlann<IndexT> = true>
+int
+radius_search(A& index, B& query, C& k_indices, D& dists, float radius, F& params)
+{
+  std::vector<pcl::Indices> indices(1);
+  int neighbors_in_radius = index.radiusSearch(query, indices, dists, radius, params);
+  k_indices = std::move(indices[0]);
+  return neighbors_in_radius;
+}
+
+template <class IndexT,
+          class A,
+          class B,
+          class C,
+          class D,
+          class F,
+          NotCompatWithFlann<IndexT> = true>
+int
+radius_search(A& index, B& query, C& k_indices, D& dists, float radius, F& params)
+{
+  std::vector<std::vector<std::size_t>> indices(1);
+  int neighbors_in_radius = index.radiusSearch(query, indices, dists, radius, params);
+  k_indices.resize(indices[0].size());
+  std::copy(indices[0].cbegin(), indices[0].cend(), k_indices.begin());
+  return neighbors_in_radius;
+}
+
+template <class IndexT, class A, class B, class F, CompatWithFlann<IndexT> = true>
+int
+radius_search(A& index,
+              B& query,
+              std::vector<Indices>& k_indices,
+              std::vector<std::vector<float>>& dists,
+              float radius,
+              F& params)
+{
+  return index.radiusSearch(query, k_indices, dists, radius, params);
+}
+
+template <class IndexT, class A, class B, class F, NotCompatWithFlann<IndexT> = true>
+int
+radius_search(A& index,
+              B& query,
+              std::vector<Indices>& k_indices,
+              std::vector<std::vector<float>>& dists,
+              float radius,
+              F& params)
+{
+  std::vector<std::vector<std::size_t>> indices;
+  // flann will resize accordingly
+  auto ret = index.radiusSearch(query, indices, dists, radius, params);
+
+  k_indices.resize(indices.size());
+  {
+    auto it = indices.cbegin();
+    auto jt = k_indices.begin();
+    for (; it != indices.cend(); ++it, ++jt) {
+      jt->resize(it->size());
+      std::copy(it->cbegin(), it->cend(), jt->begin());
+    }
+  }
+  return ret;
+}
+} // namespace detail
+template <class FlannIndex,
+          class Query,
+          class Indices,
+          class Distances,
+          class SearchParams>
+int
+radius_search(const FlannIndex& index,
+              const Query& query,
+              Indices& indices,
+              Distances& dists,
+              float radius,
+              const SearchParams& params)
+{
+  return detail::radius_search<pcl::index_t>(
+      index, query, indices, dists, radius, params);
+}
+} // namespace pcl
+
+template <typename PointT, typename Dist> int
+pcl::KdTreeFLANN<PointT, Dist>::radiusSearch (const PointT &point, double radius, Indices &k_indices,
                                               std::vector<float> &k_sqr_dists, unsigned int max_nn) const
 {
   assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
@@ -175,33 +373,33 @@ pcl::KdTreeFLANN<PointT, Dist>::radiusSearch (const PointT &point, double radius
   point_representation_->vectorize (static_cast<PointT> (point), query);
 
   // Has max_nn been set properly?
-  if (max_nn == 0 || max_nn > static_cast<unsigned int> (total_nr_points_))
+  if (max_nn == 0 || max_nn > total_nr_points_)
     max_nn = total_nr_points_;
 
-  std::vector<std::vector<int> > indices(1);
   std::vector<std::vector<float> > dists(1);
 
   ::flann::SearchParams params (param_radius_);
-  if (max_nn == static_cast<unsigned int>(total_nr_points_))
+  if (max_nn == total_nr_points_)
     params.max_neighbors = -1;  // return all neighbors in radius
   else
     params.max_neighbors = max_nn;
 
-  int neighbors_in_radius = flann_index_->radiusSearch (::flann::Matrix<float> (&query[0], 1, dim_),
-      indices,
-      dists,
-      static_cast<float> (radius * radius), 
-      params);
+  auto query_mat = ::flann::Matrix<float>(&query[0], 1, dim_);
+  int neighbors_in_radius = radius_search(*flann_index_,
+                                          query_mat,
+                                          k_indices,
+                                          dists,
+                                          static_cast<float>(radius * radius),
+                                          params);
 
-  k_indices = indices[0];
   k_sqr_dists = dists[0];
 
   // Do mapping to original point cloud
-  if (!identity_mapping_) 
+  if (!identity_mapping_)
   {
     for (int i = 0; i < neighbors_in_radius; ++i)
     {
-      int& neighbor_index = k_indices[i];
+      auto& neighbor_index = k_indices[i];
       neighbor_index = index_mapping_[neighbor_index];
     }
   }
@@ -210,7 +408,7 @@ pcl::KdTreeFLANN<PointT, Dist>::radiusSearch (const PointT &point, double radius
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT, typename Dist> void 
+template <typename PointT, typename Dist> void
 pcl::KdTreeFLANN<PointT, Dist>::cleanup ()
 {
   // Data array cleanup
@@ -221,11 +419,11 @@ pcl::KdTreeFLANN<PointT, Dist>::cleanup ()
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT, typename Dist> void 
+template <typename PointT, typename Dist> void
 pcl::KdTreeFLANN<PointT, Dist>::convertCloudToArray (const PointCloud &cloud)
 {
   // No point in doing anything if the array is empty
-  if (cloud.points.empty ())
+  if (cloud.empty ())
   {
     cloud_.reset ();
     return;
@@ -255,11 +453,11 @@ pcl::KdTreeFLANN<PointT, Dist>::convertCloudToArray (const PointCloud &cloud)
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT, typename Dist> void 
-pcl::KdTreeFLANN<PointT, Dist>::convertCloudToArray (const PointCloud &cloud, const std::vector<int> &indices)
+template <typename PointT, typename Dist> void
+pcl::KdTreeFLANN<PointT, Dist>::convertCloudToArray (const PointCloud &cloud, const Indices &indices)
 {
   // No point in doing anything if the array is empty
-  if (cloud.points.empty ())
+  if (cloud.empty ())
   {
     cloud_.reset ();
     return;
@@ -271,15 +469,15 @@ pcl::KdTreeFLANN<PointT, Dist>::convertCloudToArray (const PointCloud &cloud, co
   float* cloud_ptr = cloud_.get ();
   index_mapping_.reserve (original_no_of_points);
   // its a subcloud -> false
-  // true only identity: 
+  // true only identity:
   //     - indices size equals cloud size
   //     - indices only contain values between 0 and cloud.size - 1
   //     - no index is multiple times in the list
   //     => index is complete
   // But we can not guarantee that => identity_mapping_ = false
   identity_mapping_ = false;
-  
-  for (const int &index : indices)
+
+  for (const auto &index : indices)
   {
     // Check if the point is invalid
     if (!point_representation_->isValid (cloud[index]))
@@ -287,7 +485,7 @@ pcl::KdTreeFLANN<PointT, Dist>::convertCloudToArray (const PointCloud &cloud, co
 
     // map from 0 - N -> indices [0] - indices [N]
     index_mapping_.push_back (index);  // If the returned index should be for the indices vector
-    
+
     point_representation_->vectorize (cloud[index], cloud_ptr);
     cloud_ptr += dim_;
   }
diff --git a/kdtree/include/pcl/kdtree/io.h b/kdtree/include/pcl/kdtree/io.h
index a3ebb21f..071340dc 100644
--- a/kdtree/include/pcl/kdtree/io.h
+++ b/kdtree/include/pcl/kdtree/io.h
@@ -55,7 +55,7 @@ namespace pcl
   template <typename PointT> void
   getApproximateIndices (const typename pcl::PointCloud<PointT>::ConstPtr &cloud_in,
                          const typename pcl::PointCloud<PointT>::ConstPtr &cloud_ref,
-                         std::vector<int> &indices);
+                         Indices &indices);
 
   /** \brief Get a set of approximate indices for a given point cloud into a reference point cloud. 
     * The coordinates of the two point clouds can differ. The method uses an internal KdTree for 
@@ -69,7 +69,7 @@ namespace pcl
   template <typename Point1T, typename Point2T> void
   getApproximateIndices (const typename pcl::PointCloud<Point1T>::ConstPtr &cloud_in,
                          const typename pcl::PointCloud<Point2T>::ConstPtr &cloud_ref,
-                         std::vector<int> &indices);
+                         Indices &indices);
 }
 
 #include <pcl/kdtree/impl/io.hpp>
diff --git a/kdtree/include/pcl/kdtree/kdtree.h b/kdtree/include/pcl/kdtree/kdtree.h
index 6c77c151..82baf32a 100644
--- a/kdtree/include/pcl/kdtree/kdtree.h
+++ b/kdtree/include/pcl/kdtree/kdtree.h
@@ -38,12 +38,10 @@
 
 #pragma once
 
-#include <climits>
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_representation.h>
-#include <pcl/common/io.h>
 #include <pcl/common/copy_point.h>
 
 namespace pcl
@@ -56,8 +54,8 @@ namespace pcl
   class KdTree
   {
     public:
-      using IndicesPtr = shared_ptr<std::vector<int> >;
-      using IndicesConstPtr = shared_ptr<const std::vector<int> >;
+      using IndicesPtr = shared_ptr<Indices >;
+      using IndicesConstPtr = shared_ptr<const Indices >;
 
       using PointCloud = pcl::PointCloud<PointT>;
       using PointCloudPtr = typename PointCloud::Ptr;
@@ -70,11 +68,11 @@ namespace pcl
       using Ptr = shared_ptr<KdTree<PointT> >;
       using ConstPtr = shared_ptr<const KdTree<PointT> >;
 
-      /** \brief Empty constructor for KdTree. Sets some internal values to their defaults. 
-        * \param[in] sorted set to true if the application that the tree will be used for requires sorted nearest neighbor indices (default). False otherwise. 
+      /** \brief Empty constructor for KdTree. Sets some internal values to their defaults.
+        * \param[in] sorted set to true if the application that the tree will be used for requires sorted nearest neighbor indices (default). False otherwise.
         */
       KdTree (bool sorted = true) : input_(),
-                                    epsilon_(0.0f), min_pts_(1), sorted_(sorted), 
+                                    epsilon_(0.0f), min_pts_(1), sorted_(sorted),
                                     point_representation_ (new DefaultPointRepresentation<PointT>)
       {
       };
@@ -104,7 +102,7 @@ namespace pcl
         return (input_);
       }
 
-      /** \brief Provide a pointer to the point representation to use to convert points into k-D vectors. 
+      /** \brief Provide a pointer to the point representation to use to convert points into k-D vectors.
         * \param[in] point_representation the const boost shared pointer to a PointRepresentation
         */
       inline void
@@ -129,50 +127,50 @@ namespace pcl
         * \param[in] p_q the given query point
         * \param[in] k the number of neighbors to search for
         * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
-        * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k 
+        * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
         * a priori!)
         * \return number of neighbors found
         */
-      virtual int 
-      nearestKSearch (const PointT &p_q, int k, 
-                      std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const = 0;
+      virtual int
+      nearestKSearch (const PointT &p_q, unsigned int k,
+                      Indices &k_indices, std::vector<float> &k_sqr_distances) const = 0;
 
       /** \brief Search for k-nearest neighbors for the given query point.
-        * 
+        *
         * \attention This method does not do any bounds checking for the input index
         * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
-        * 
+        *
         * \param[in] cloud the point cloud data
         * \param[in] index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point
         * \param[in] k the number of neighbors to search for
         * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
-        * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k 
+        * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
         * a priori!)
-        * 
+        *
         * \return number of neighbors found
-        * 
+        *
         * \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<int> &k_indices, std::vector<float> &k_sqr_distances) const
+      virtual int
+      nearestKSearch (const PointCloud &cloud, int index, unsigned int k,
+                      Indices &k_indices, std::vector<float> &k_sqr_distances) const
       {
         assert (index >= 0 && index < static_cast<int> (cloud.size ()) && "Out-of-bounds error in nearestKSearch!");
         return (nearestKSearch (cloud[index], k, k_indices, k_sqr_distances));
       }
 
-      /** \brief Search for k-nearest neighbors for the given query point. 
+      /** \brief Search for k-nearest neighbors for the given query point.
         * This method accepts a different template parameter for the point type.
         * \param[in] point the given query point
         * \param[in] k the number of neighbors to search for
         * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
-        * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k 
+        * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
         * a priori!)
         * \return number of neighbors found
         */
-      template <typename PointTDiff> inline int 
-      nearestKSearchT (const PointTDiff &point, int k, 
-                       std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const
+      template <typename PointTDiff> inline int
+      nearestKSearchT (const PointTDiff &point, unsigned int k,
+                       Indices &k_indices, std::vector<float> &k_sqr_distances) const
       {
         PointT p;
         copyPoint (point, p);
@@ -180,25 +178,25 @@ namespace pcl
       }
 
       /** \brief Search for k-nearest neighbors for the given query point (zero-copy).
-        * 
+        *
         * \attention This method does not do any bounds checking for the input index
         * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
-        * 
-        * \param[in] index a \a valid index representing a \a valid query point in the dataset given 
-        * by \a setInputCloud. If indices were given in setInputCloud, index will be the position in 
+        *
+        * \param[in] index a \a valid index representing a \a valid query point in the dataset given
+        * by \a setInputCloud. If indices were given in setInputCloud, index will be the position in
         * the indices vector.
-        * 
+        *
         * \param[in] k the number of neighbors to search for
         * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
-        * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k 
+        * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
         * a priori!)
         * \return number of neighbors found
-        * 
+        *
         * \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<int> &k_indices, std::vector<float> &k_sqr_distances) const
+      virtual int
+      nearestKSearch (int index, unsigned int k,
+                      Indices &k_indices, std::vector<float> &k_sqr_distances) const
       {
         if (indices_ == nullptr)
         {
@@ -220,15 +218,15 @@ namespace pcl
         * returned.
         * \return number of neighbors found in radius
         */
-      virtual int 
-      radiusSearch (const PointT &p_q, double radius, std::vector<int> &k_indices,
+      virtual int
+      radiusSearch (const PointT &p_q, double radius, Indices &k_indices,
                     std::vector<float> &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.
-        * 
+        *
         * \attention This method does not do any bounds checking for the input index
         * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
-        * 
+        *
         * \param[in] cloud the point cloud data
         * \param[in] index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point
         * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
@@ -238,12 +236,12 @@ namespace pcl
         * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
         * returned.
         * \return number of neighbors found in radius
-        * 
+        *
         * \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<int> &k_indices, std::vector<float> &k_sqr_distances, 
+      virtual int
+      radiusSearch (const PointCloud &cloud, int index, double radius,
+                    Indices &k_indices, std::vector<float> &k_sqr_distances,
                     unsigned int max_nn = 0) const
       {
         assert (index >= 0 && index < static_cast<int> (cloud.size ()) && "Out-of-bounds error in radiusSearch!");
@@ -260,8 +258,8 @@ namespace pcl
         * returned.
         * \return number of neighbors found in radius
         */
-      template <typename PointTDiff> inline int 
-      radiusSearchT (const PointTDiff &point, double radius, std::vector<int> &k_indices,
+      template <typename PointTDiff> inline int
+      radiusSearchT (const PointTDiff &point, double radius, Indices &k_indices,
                      std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const
       {
         PointT p;
@@ -270,14 +268,14 @@ namespace pcl
       }
 
       /** \brief Search for all the nearest neighbors of the query point in a given radius (zero-copy).
-        * 
+        *
         * \attention This method does not do any bounds checking for the input index
         * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
-        * 
-        * \param[in] index a \a valid index representing a \a valid query point in the dataset given 
-        * by \a setInputCloud. If indices were given in setInputCloud, index will be the position in 
+        *
+        * \param[in] index a \a valid index representing a \a valid query point in the dataset given
+        * by \a setInputCloud. If indices were given in setInputCloud, index will be the position in
         * the indices vector.
-        * 
+        *
         * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
         * \param[out] k_indices the resultant indices of the neighboring points
         * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
@@ -285,11 +283,11 @@ namespace pcl
         * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
         * returned.
         * \return number of neighbors found in radius
-        * 
+        *
         * \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<int> &k_indices,
+      virtual int
+      radiusSearch (int index, double radius, Indices &k_indices,
                     std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const
       {
         if (indices_ == nullptr)
@@ -317,8 +315,8 @@ namespace pcl
         return (epsilon_);
       }
 
-      /** \brief Minimum allowed number of k nearest neighbors points that a viable result must contain. 
-        * \param[in] min_pts the minimum number of neighbors in a viable neighborhood 
+      /** \brief Minimum allowed number of k nearest neighbors points that a viable result must contain.
+        * \param[in] min_pts the minimum number of neighbors in a viable neighborhood
         */
       inline void
       setMinPts (int min_pts)
@@ -353,7 +351,7 @@ namespace pcl
       PointRepresentationConstPtr point_representation_;
 
       /** \brief Class getName method. */
-      virtual std::string 
+      virtual std::string
       getName () const = 0;
   };
 }
diff --git a/kdtree/include/pcl/kdtree/kdtree_flann.h b/kdtree/include/pcl/kdtree/kdtree_flann.h
index e50b7cc7..a20ed998 100644
--- a/kdtree/include/pcl/kdtree/kdtree_flann.h
+++ b/kdtree/include/pcl/kdtree/kdtree_flann.h
@@ -54,181 +54,259 @@ namespace flann
 
 namespace pcl
 {
-  // Forward declarations
-  template <typename T> class PointRepresentation;
-
-  /** \brief KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures. The class is making use of
-    * the FLANN (Fast Library for Approximate Nearest Neighbor) project by Marius Muja and David Lowe.
-    *
-    * \author Radu B. Rusu, Marius Muja
-    * \ingroup kdtree 
-    */
-  template <typename PointT, typename Dist = ::flann::L2_Simple<float> >
-  class KdTreeFLANN : public pcl::KdTree<PointT>
+namespace detail {
+// Helper struct to create a compatible Matrix and copy data back when needed
+// Replace using if constexpr in C++17
+template <typename IndexT>
+struct compat_with_flann : std::false_type {};
+
+template <>
+struct compat_with_flann<std::size_t> : std::true_type {};
+
+template <typename IndexT>
+using CompatWithFlann = std::enable_if_t<compat_with_flann<IndexT>::value, bool>;
+template <typename IndexT>
+using NotCompatWithFlann = std::enable_if_t<!compat_with_flann<IndexT>::value, bool>;
+} // namespace detail
+
+/**
+ * @brief Comaptibility template function to allow use of various types of indices with
+ * FLANN
+ * @details Template is used for all params to not constrain any FLANN side capability
+ * @param[in|out] index A index searcher, of type ::flann::Index<Dist> or similar, where
+ * Dist is a template for computing distance between 2 points
+ * @param[in] query A ::flann::Matrix<float> or compatible matrix representation of the
+ * query point
+ * @param[out] indices Indices found in radius
+ * @param[out] dists Computed distance matrix
+ * @param[in] radius Threshold for consideration
+ * @param[in] params Any parameters to pass to the radius_search call
+ */
+template <class FlannIndex,
+          class Query,
+          class Indices,
+          class Distances,
+          class SearchParams>
+int
+radius_search(const FlannIndex& index,
+              const Query& query,
+              Indices& indices,
+              Distances& dists,
+              float radius,
+              const SearchParams& params);
+
+/**
+ * @brief Comaptibility template function to allow use of various types of indices with
+ * FLANN
+ * @details Template is used for all params to not constrain any FLANN side capability
+ * @param[in|out] index A index searcher, of type ::flann::Index<Dist> or similar, where
+ * Dist is a template for computing distance between 2 points
+ * @param[in] query A ::flann::Matrix<float> or compatible matrix representation of the
+ * query point
+ * @param[out] indices Neighboring k indices found
+ * @param[out] dists Computed distance matrix
+ * @param[in] radius Threshold for consideration
+ * @param[in] params Any parameters to pass to the radius_search call
+ */
+template <class FlannIndex,
+          class Query,
+          class Indices,
+          class Distances,
+          class SearchParams>
+int
+knn_search(const FlannIndex& index,
+           const Query& query,
+           Indices& indices,
+           Distances& dists,
+           unsigned int k,
+           const SearchParams& params);
+
+/** \brief KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
+ * The class is making use of the FLANN (Fast Library for Approximate Nearest Neighbor)
+ * project by Marius Muja and David Lowe.
+ *
+ * \author Radu B. Rusu, Marius Muja
+ * \ingroup kdtree
+ */
+template <typename PointT, typename Dist = ::flann::L2_Simple<float>>
+class KdTreeFLANN : public pcl::KdTree<PointT> {
+public:
+  using KdTree<PointT>::input_;
+  using KdTree<PointT>::indices_;
+  using KdTree<PointT>::epsilon_;
+  using KdTree<PointT>::sorted_;
+  using KdTree<PointT>::point_representation_;
+  using KdTree<PointT>::nearestKSearch;
+  using KdTree<PointT>::radiusSearch;
+
+  using PointCloud = typename KdTree<PointT>::PointCloud;
+  using PointCloudConstPtr = typename KdTree<PointT>::PointCloudConstPtr;
+
+  using IndicesPtr = shared_ptr<Indices>;
+  using IndicesConstPtr = shared_ptr<const Indices>;
+
+  using FLANNIndex = ::flann::Index<Dist>;
+
+  // Boost shared pointers
+  using Ptr = shared_ptr<KdTreeFLANN<PointT, Dist>>;
+  using ConstPtr = shared_ptr<const KdTreeFLANN<PointT, Dist>>;
+
+  /** \brief Default Constructor for KdTreeFLANN.
+   * \param[in] sorted set to true if the application that the tree will be used for
+   * requires sorted nearest neighbor indices (default). False otherwise.
+   *
+   * By setting sorted to false, the \ref radiusSearch operations will be faster.
+   */
+  KdTreeFLANN(bool sorted = true);
+
+  /** \brief Copy constructor
+   * \param[in] k the tree to copy into this
+   */
+  KdTreeFLANN(const KdTreeFLANN<PointT, Dist>& k);
+
+  /** \brief Copy operator
+   * \param[in] k the tree to copy into this
+   */
+  inline KdTreeFLANN<PointT, Dist>&
+  operator=(const KdTreeFLANN<PointT, Dist>& k)
   {
-    public:
-      using KdTree<PointT>::input_;
-      using KdTree<PointT>::indices_;
-      using KdTree<PointT>::epsilon_;
-      using KdTree<PointT>::sorted_;
-      using KdTree<PointT>::point_representation_;
-      using KdTree<PointT>::nearestKSearch;
-      using KdTree<PointT>::radiusSearch;
-
-      using PointCloud = typename KdTree<PointT>::PointCloud;
-      using PointCloudConstPtr = typename KdTree<PointT>::PointCloudConstPtr;
-
-      using IndicesPtr = shared_ptr<std::vector<int> >;
-      using IndicesConstPtr = shared_ptr<const std::vector<int> >;
-
-      using FLANNIndex = ::flann::Index<Dist>;
-
-      // Boost shared pointers
-      using Ptr = shared_ptr<KdTreeFLANN<PointT, Dist> >;
-      using ConstPtr = shared_ptr<const KdTreeFLANN<PointT, Dist> >;
-
-      /** \brief Default Constructor for KdTreeFLANN.
-        * \param[in] sorted set to true if the application that the tree will be used for requires sorted nearest neighbor indices (default). False otherwise. 
-        *
-        * By setting sorted to false, the \ref radiusSearch operations will be faster.
-        */
-      KdTreeFLANN (bool sorted = true);
-
-      /** \brief Copy constructor
-        * \param[in] k the tree to copy into this
-        */
-      KdTreeFLANN (const KdTreeFLANN<PointT, Dist> &k);
-
-      /** \brief Copy operator
-        * \param[in] k the tree to copy into this
-        */ 
-      inline KdTreeFLANN<PointT, Dist>&
-      operator = (const KdTreeFLANN<PointT, Dist>& k)
-      {
-        KdTree<PointT>::operator=(k);
-        flann_index_ = k.flann_index_;
-        cloud_ = k.cloud_;
-        index_mapping_ = k.index_mapping_;
-        identity_mapping_ = k.identity_mapping_;
-        dim_ = k.dim_;
-        total_nr_points_ = k.total_nr_points_;
-        param_k_ = k.param_k_;
-        param_radius_ = k.param_radius_;
-        return (*this);
-      }
-
-      /** \brief Set the search epsilon precision (error bound) for nearest neighbors searches.
-        * \param[in] eps precision (error bound) for nearest neighbors searches
-        */
-      void
-      setEpsilon (float eps) override;
-
-      void 
-      setSortedResults (bool sorted);
-      
-      inline Ptr makeShared () { return Ptr (new KdTreeFLANN<PointT, Dist> (*this)); } 
-
-      /** \brief Destructor for KdTreeFLANN. 
-        * Deletes all allocated data arrays and destroys the kd-tree structures. 
-        */
-      ~KdTreeFLANN ()
-      {
-        cleanup ();
-      }
-
-      /** \brief Provide a pointer to the input dataset.
-        * \param[in] cloud the const boost shared pointer to a PointCloud message
-        * \param[in] indices the point indices subset that is to be used from \a cloud - if NULL the whole cloud is used
-        */
-      void 
-      setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr ()) override;
-
-      /** \brief Search for k-nearest neighbors for the given query point.
-        * 
-        * \attention This method does not do any bounds checking for the input index
-        * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
-        * 
-        * \param[in] point a given \a valid (i.e., finite) query point
-        * \param[in] k the number of neighbors to search for
-        * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
-        * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k 
-        * a priori!)
-        * \return number of neighbors found
-        * 
-        * \exception asserts in debug mode if the index is not between 0 and the maximum number of points
-        */
-      int 
-      nearestKSearch (const PointT &point, int k, 
-                      std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const override;
-
-      /** \brief Search for all the nearest neighbors of the query point in a given radius.
-        * 
-        * \attention This method does not do any bounds checking for the input index
-        * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
-        * 
-        * \param[in] point a given \a valid (i.e., finite) query point
-        * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
-        * \param[out] k_indices the resultant indices of the neighboring points
-        * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
-        * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
-        * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
-        * returned.
-        * \return number of neighbors found in radius
-        *
-        * \exception asserts in debug mode if the index is not between 0 and the maximum number of points
-        */
-      int 
-      radiusSearch (const PointT &point, double radius, std::vector<int> &k_indices,
-                    std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const override;
-
-    private:
-      /** \brief Internal cleanup method. */
-      void 
-      cleanup ();
-
-      /** \brief Converts a PointCloud to the internal FLANN point array representation. Returns the number
-        * of points.
-        * \param cloud the PointCloud 
-        */
-      void 
-      convertCloudToArray (const PointCloud &cloud);
-
-      /** \brief Converts a PointCloud with a given set of indices to the internal FLANN point array
-        * representation. Returns the number of points.
-        * \param[in] cloud the PointCloud data
-        * \param[in] indices the point cloud indices
-       */
-      void 
-      convertCloudToArray (const PointCloud &cloud, const std::vector<int> &indices);
-
-    private:
-      /** \brief Class getName method. */
-      std::string 
-      getName () const override { return ("KdTreeFLANN"); }
-
-      /** \brief A FLANN index object. */
-      std::shared_ptr<FLANNIndex> flann_index_;
-
-      /** \brief Internal pointer to data. TODO: replace with std::shared_ptr<float[]> with C++17*/
-      std::shared_ptr<float> cloud_;
-
-      /** \brief mapping between internal and external indices. */
-      std::vector<int> index_mapping_;
-
-      /** \brief whether the mapping between internal and external indices is identity */
-      bool identity_mapping_;
-
-      /** \brief Tree dimensionality (i.e. the number of dimensions per point). */
-      int dim_;
-
-      /** \brief The total size of the data (either equal to the number of points in the input cloud or to the number of indices - if passed). */
-      int total_nr_points_;
-
-      /** \brief The KdTree search parameters for K-nearest neighbors. */
-      ::flann::SearchParams param_k_;
-
-      /** \brief The KdTree search parameters for radius search. */
-      ::flann::SearchParams param_radius_;
+    KdTree<PointT>::operator=(k);
+    flann_index_ = k.flann_index_;
+    cloud_ = k.cloud_;
+    index_mapping_ = k.index_mapping_;
+    identity_mapping_ = k.identity_mapping_;
+    dim_ = k.dim_;
+    total_nr_points_ = k.total_nr_points_;
+    param_k_ = k.param_k_;
+    param_radius_ = k.param_radius_;
+    return (*this);
+  }
+
+  /** \brief Set the search epsilon precision (error bound) for nearest neighbors
+   * searches. \param[in] eps precision (error bound) for nearest neighbors searches
+   */
+  void
+  setEpsilon(float eps) override;
+
+  void
+  setSortedResults(bool sorted);
+
+  inline Ptr
+  makeShared()
+  {
+    return Ptr(new KdTreeFLANN<PointT, Dist>(*this));
+  }
+
+  /** \brief Destructor for KdTreeFLANN.
+   * Deletes all allocated data arrays and destroys the kd-tree structures.
+   */
+  ~KdTreeFLANN() { cleanup(); }
+
+  /** \brief Provide a pointer to the input dataset.
+   * \param[in] cloud the const boost shared pointer to a PointCloud message
+   * \param[in] indices the point indices subset that is to be used from \a cloud - if
+   * NULL the whole cloud is used
+   */
+  void
+  setInputCloud(const PointCloudConstPtr& cloud,
+                const IndicesConstPtr& indices = IndicesConstPtr()) override;
+
+  /** \brief Search for k-nearest neighbors for the given query point.
+   *
+   * \attention This method does not do any bounds checking for the input index
+   * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
+   *
+   * \param[in] point a given \a valid (i.e., finite) query point
+   * \param[in] k the number of neighbors to search for
+   * \param[out] k_indices the resultant indices of the neighboring points (must be
+   * resized to \a k a priori!) \param[out] k_sqr_distances the resultant squared
+   * distances to the neighboring points (must be resized to \a k a priori!) \return
+   * number of neighbors found
+   *
+   * \exception asserts in debug mode if the index is not between 0 and the maximum
+   * number of points
+   */
+  int
+  nearestKSearch(const PointT& point,
+                 unsigned int k,
+                 Indices& k_indices,
+                 std::vector<float>& k_sqr_distances) const override;
+
+  /** \brief Search for all the nearest neighbors of the query point in a given radius.
+   *
+   * \attention This method does not do any bounds checking for the input index
+   * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
+   *
+   * \param[in] point a given \a valid (i.e., finite) query point
+   * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
+   * \param[out] k_indices the resultant indices of the neighboring points
+   * \param[out] k_sqr_distances the resultant squared distances to the neighboring
+   * points \param[in] max_nn if given, bounds the maximum returned neighbors to this
+   * value. If \a max_nn is set to 0 or to a number higher than the number of points in
+   * the input cloud, all neighbors in \a radius will be returned. \return number of
+   * neighbors found in radius
+   *
+   * \exception asserts in debug mode if the index is not between 0 and the maximum
+   * number of points
+   */
+  int
+  radiusSearch(const PointT& point,
+               double radius,
+               Indices& k_indices,
+               std::vector<float>& k_sqr_distances,
+               unsigned int max_nn = 0) const override;
+
+private:
+  /** \brief Internal cleanup method. */
+  void
+  cleanup();
+
+  /** \brief Converts a PointCloud to the internal FLANN point array representation.
+   * Returns the number of points. \param cloud the PointCloud
+   */
+  void
+  convertCloudToArray(const PointCloud& cloud);
+
+  /** \brief Converts a PointCloud with a given set of indices to the internal FLANN
+   * point array representation. Returns the number of points. \param[in] cloud the
+   * PointCloud data \param[in] indices the point cloud indices
+   */
+  void
+  convertCloudToArray(const PointCloud& cloud, const Indices& indices);
+
+private:
+  /** \brief Class getName method. */
+  std::string
+  getName() const override
+  {
+    return ("KdTreeFLANN");
+  }
+
+  /** \brief A FLANN index object. */
+  std::shared_ptr<FLANNIndex> flann_index_;
+
+  /** \brief Internal pointer to data. TODO: replace with std::shared_ptr<float[]> with
+   * C++17*/
+  std::shared_ptr<float> cloud_;
+
+  /** \brief mapping between internal and external indices. */
+  std::vector<int> index_mapping_;
+
+  /** \brief whether the mapping between internal and external indices is identity */
+  bool identity_mapping_;
+
+  /** \brief Tree dimensionality (i.e. the number of dimensions per point). */
+  int dim_;
+
+  /** \brief The total size of the data (either equal to the number of points in the
+   * input cloud or to the number of indices - if passed). */
+  uindex_t total_nr_points_;
+
+  /** \brief The KdTree search parameters for K-nearest neighbors. */
+  ::flann::SearchParams param_k_;
+
+  /** \brief The KdTree search parameters for radius search. */
+  ::flann::SearchParams param_radius_;
   };
 }
 
diff --git a/kdtree/kdtree.doxy b/kdtree/kdtree.doxy
index aae221d3..9bf527c3 100644
--- a/kdtree/kdtree.doxy
+++ b/kdtree/kdtree.doxy
@@ -4,7 +4,7 @@
   \section secKDtreePresentation Overview 
 
   The <b>pcl_kdtree</b> library provides the kd-tree data-structure, using
-  <a href="http://www.cs.ubc.ca/research/flann/</a>,
+  <a href="http://www.cs.ubc.ca/research/flann/">FLANN</a>,
   that allows for fast <a href="http://en.wikipedia.org/wiki/Nearest_neighbor_search">nearest neighbor searches</a>.
 
   A <a href="http://en.wikipedia.org/wiki/Kd-tree">Kd-tree</a> (<i>k</i>-dimensional tree) is a space-partitioning data 
diff --git a/keypoints/include/pcl/keypoints/agast_2d.h b/keypoints/include/pcl/keypoints/agast_2d.h
index 6c3d3e71..76e5c3f0 100644
--- a/keypoints/include/pcl/keypoints/agast_2d.h
+++ b/keypoints/include/pcl/keypoints/agast_2d.h
@@ -44,6 +44,7 @@
 #include <pcl/point_types.h>
 #include <pcl/keypoints/keypoint.h>
 #include <pcl/common/intensity.h>
+#include <pcl/common/io.h> // for copyPointCloud
 
 namespace pcl
 {
diff --git a/keypoints/include/pcl/keypoints/harris_3d.h b/keypoints/include/pcl/keypoints/harris_3d.h
index c406d306..e59c7644 100644
--- a/keypoints/include/pcl/keypoints/harris_3d.h
+++ b/keypoints/include/pcl/keypoints/harris_3d.h
@@ -168,7 +168,7 @@ namespace pcl
       void responseCurvature (PointCloudOut &output) const;
       void refineCorners (PointCloudOut &corners) const;
       /** \brief calculates the upper triangular part of unnormalized covariance matrix over the normals given by the indices.*/
-      void calculateNormalCovar (const std::vector<int>& neighbors, float* coefficients) const;
+      void calculateNormalCovar (const pcl::Indices& neighbors, float* coefficients) const;
     private:
       float threshold_;
       bool refine_;
diff --git a/keypoints/include/pcl/keypoints/harris_6d.h b/keypoints/include/pcl/keypoints/harris_6d.h
index 838fe5fc..6bcb59e0 100644
--- a/keypoints/include/pcl/keypoints/harris_6d.h
+++ b/keypoints/include/pcl/keypoints/harris_6d.h
@@ -126,7 +126,7 @@ namespace pcl
       void detectKeypoints (PointCloudOut &output);
       void responseTomasi (PointCloudOut &output) const;
       void refineCorners (PointCloudOut &corners) const;
-      void calculateCombinedCovar (const std::vector<int>& neighbors, float* coefficients) const;
+      void calculateCombinedCovar (const pcl::Indices& neighbors, float* coefficients) const;
     private:
       float threshold_;
       bool refine_;
diff --git a/keypoints/include/pcl/keypoints/impl/harris_3d.hpp b/keypoints/include/pcl/keypoints/impl/harris_3d.hpp
index f1f17d1b..47e64d13 100644
--- a/keypoints/include/pcl/keypoints/impl/harris_3d.hpp
+++ b/keypoints/include/pcl/keypoints/impl/harris_3d.hpp
@@ -40,11 +40,8 @@
 
 #include <pcl/keypoints/harris_3d.h>
 #include <pcl/common/io.h>
-#include <pcl/filters/passthrough.h>
-#include <pcl/filters/extract_indices.h>
 #include <pcl/features/normal_3d.h>
 #include <pcl/features/integral_image_normal.h>
-#include <pcl/common/time.h>
 #include <pcl/common/centroid.h>
 #ifdef __SSE__
 #include <xmmintrin.h>
@@ -103,7 +100,7 @@ pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::setNormals (const PointClou
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointInT, typename PointOutT, typename NormalT> void
-pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::calculateNormalCovar (const std::vector<int>& neighbors, float* coefficients) const
+pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::calculateNormalCovar (const pcl::Indices& neighbors, float* coefficients) const
 {
   unsigned count = 0;
   // indices        0   1   2   3   4   5   6   7
@@ -120,7 +117,7 @@ pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::calculateNormalCovar (const
 
   float zz = 0;
 
-  for (const int &neighbor : neighbors)
+  for (const auto &neighbor : neighbors)
   {
     if (std::isfinite ((*normals_)[neighbor].normal_x))
     {
@@ -162,17 +159,17 @@ pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::calculateNormalCovar (const
     memset (coefficients, 0, sizeof (float) * 8);
 #else
   memset (coefficients, 0, sizeof (float) * 8);
-  for (std::vector<int>::const_iterator iIt = neighbors.begin(); iIt != neighbors.end(); ++iIt)
+  for (const auto& index : neighbors)
   {
-    if (std::isfinite ((*normals_)[*iIt].normal_x))
+    if (std::isfinite ((*normals_)[index].normal_x))
     {
-      coefficients[0] += (*normals_)[*iIt].normal_x * (*normals_)[*iIt].normal_x;
-      coefficients[1] += (*normals_)[*iIt].normal_x * (*normals_)[*iIt].normal_y;
-      coefficients[2] += (*normals_)[*iIt].normal_x * (*normals_)[*iIt].normal_z;
+      coefficients[0] += (*normals_)[index].normal_x * (*normals_)[index].normal_x;
+      coefficients[1] += (*normals_)[index].normal_x * (*normals_)[index].normal_y;
+      coefficients[2] += (*normals_)[index].normal_x * (*normals_)[index].normal_z;
 
-      coefficients[5] += (*normals_)[*iIt].normal_y * (*normals_)[*iIt].normal_y;
-      coefficients[6] += (*normals_)[*iIt].normal_y * (*normals_)[*iIt].normal_z;
-      coefficients[7] += (*normals_)[*iIt].normal_z * (*normals_)[*iIt].normal_z;
+      coefficients[5] += (*normals_)[index].normal_y * (*normals_)[index].normal_y;
+      coefficients[6] += (*normals_)[index].normal_y * (*normals_)[index].normal_z;
+      coefficients[7] += (*normals_)[index].normal_z * (*normals_)[index].normal_z;
 
       ++count;
     }
@@ -273,8 +270,8 @@ pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloud
   }
   else
   {
-    output.points.clear ();
-    output.points.reserve (response->size());
+    output.clear ();
+    output.reserve (response->size());
 
 #pragma omp parallel for \
   default(none) \
@@ -287,13 +284,13 @@ pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloud
           (*response)[idx].intensity < threshold_)
         continue;
 
-      std::vector<int> nn_indices;
+      pcl::Indices nn_indices;
       std::vector<float> nn_dists;
       tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists);
       bool is_maxima = true;
-      for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
+      for (const auto& index : nn_indices)
       {
-        if ((*response)[idx].intensity < (*response)[*iIt].intensity)
+        if ((*response)[idx].intensity < (*response)[index].intensity)
         {
           is_maxima = false;
           break;
@@ -302,7 +299,7 @@ pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloud
       if (is_maxima)
 #pragma omp critical
       {
-        output.points.push_back ((*response)[idx]);
+        output.push_back ((*response)[idx]);
         keypoints_indices_->indices.push_back (idx);
       }
     }
@@ -333,7 +330,7 @@ pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::responseHarris (PointCloudO
     output [pIdx].intensity = 0.0; //std::numeric_limits<float>::quiet_NaN ();
     if (isFinite (pointIn))
     {
-      std::vector<int> nn_indices;
+      pcl::Indices nn_indices;
       std::vector<float> nn_dists;
       tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
       calculateNormalCovar (nn_indices, covar);
@@ -374,7 +371,7 @@ pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::responseNoble (PointCloudOu
     output [pIdx].intensity = 0.0;
     if (isFinite (pointIn))
     {
-      std::vector<int> nn_indices;
+      pcl::Indices nn_indices;
       std::vector<float> nn_dists;
       tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
       calculateNormalCovar (nn_indices, covar);
@@ -414,7 +411,7 @@ pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::responseLowe (PointCloudOut
     output [pIdx].intensity = 0.0;
     if (isFinite (pointIn))
     {
-      std::vector<int> nn_indices;
+      pcl::Indices nn_indices;
       std::vector<float> nn_dists;
       tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
       calculateNormalCovar (nn_indices, covar);
@@ -448,7 +445,7 @@ pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::responseCurvature (PointClo
     point.y = (*input_)[idx].y;
     point.z = (*input_)[idx].z;
     point.intensity = (*normals_)[idx].curvature;
-    output.points.push_back(point);
+    output.push_back(point);
   }
   // does not change the order
   output.height = input_->height;
@@ -473,7 +470,7 @@ pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::responseTomasi (PointCloudO
     output [pIdx].intensity = 0.0;
     if (isFinite (pointIn))
     {
-      std::vector<int> nn_indices;
+      pcl::Indices nn_indices;
       std::vector<float> nn_dists;
       tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
       calculateNormalCovar (nn_indices, covar);
@@ -508,12 +505,11 @@ pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::refineCorners (PointCloudOu
   Eigen::Matrix3f NNT;
   Eigen::Matrix3f NNTInv;
   Eigen::Vector3f NNTp;
-  float diff;
   const unsigned max_iterations = 10;
 #pragma omp parallel for \
   default(none) \
   shared(corners) \
-  firstprivate(nnT, NNT, NNTInv, NNTp, diff) \
+  firstprivate(nnT, NNT, NNTInv, NNTp) \
   num_threads(threads_)
   for (int cIdx = 0; cIdx < static_cast<int> (corners.size ()); ++cIdx)
   {
@@ -525,26 +521,28 @@ pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::refineCorners (PointCloudOu
       corner.x = corners[cIdx].x;
       corner.y = corners[cIdx].y;
       corner.z = corners[cIdx].z;
-      std::vector<int> nn_indices;
+      pcl::Indices nn_indices;
       std::vector<float> nn_dists;
       tree_->radiusSearch (corner, search_radius_, nn_indices, nn_dists);
-      for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
+      for (const auto& index : nn_indices)
       {
-        if (!std::isfinite ((*normals_)[*iIt].normal_x))
+        if (!std::isfinite ((*normals_)[index].normal_x))
           continue;
 
-        nnT = (*normals_)[*iIt].getNormalVector3fMap () * (*normals_)[*iIt].getNormalVector3fMap ().transpose();
+        nnT = (*normals_)[index].getNormalVector3fMap () * (*normals_)[index].getNormalVector3fMap ().transpose();
         NNT += nnT;
-        NNTp += nnT * (*surface_)[*iIt].getVector3fMap ();
+        NNTp += nnT * (*surface_)[index].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);
+      const auto diff = (corners[cIdx].getVector3fMap () - corner.getVector3fMap()).squaredNorm ();
+      if (diff <= 1e-6) {
+        break;
+      }
+    } while (++iterations < max_iterations);
   }
 }
 
 #define PCL_INSTANTIATE_HarrisKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::HarrisKeypoint3D<T,U,N>;
 #endif // #ifndef PCL_HARRIS_KEYPOINT_3D_IMPL_H_
-
diff --git a/keypoints/include/pcl/keypoints/impl/harris_6d.hpp b/keypoints/include/pcl/keypoints/impl/harris_6d.hpp
index 32d5c17d..72362168 100644
--- a/keypoints/include/pcl/keypoints/impl/harris_6d.hpp
+++ b/keypoints/include/pcl/keypoints/impl/harris_6d.hpp
@@ -38,6 +38,7 @@
 #ifndef PCL_HARRIS_KEYPOINT_6D_IMPL_H_
 #define PCL_HARRIS_KEYPOINT_6D_IMPL_H_
 
+#include <Eigen/Eigenvalues> // for SelfAdjointEigenSolver
 #include <pcl/keypoints/harris_6d.h>
 #include <pcl/common/io.h>
 #include <pcl/features/normal_3d.h>
@@ -71,11 +72,11 @@ pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::setNonMaxSupression (bool n
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointInT, typename PointOutT, typename NormalT> void
-pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::calculateCombinedCovar (const std::vector<int>& neighbors, float* coefficients) const
+pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::calculateCombinedCovar (const pcl::Indices& neighbors, float* coefficients) const
 {
   memset (coefficients, 0, sizeof (float) * 21);
   unsigned count = 0;
-  for (const int &neighbor : neighbors)
+  for (const auto &neighbor : neighbors)
   {
     if (std::isfinite ((*normals_)[neighbor].normal_x) && std::isfinite ((*intensity_gradients_)[neighbor].gradient [0]))
     {
@@ -222,8 +223,8 @@ pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloud
   }
   else
   {
-    output.points.clear ();
-    output.points.reserve (response->size());
+    output.clear ();
+    output.reserve (response->size());
 
 #pragma omp parallel for \
   default(none) \
@@ -233,13 +234,13 @@ pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloud
       if (!isFinite ((*response)[idx]) || (*response)[idx].intensity < threshold_)
         continue;
 
-      std::vector<int> nn_indices;
+      pcl::Indices nn_indices;
       std::vector<float> nn_dists;
       tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists);
       bool is_maxima = true;
-      for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
+      for (const auto& index : nn_indices)
       {
-        if ((*response)[idx].intensity < (*response)[*iIt].intensity)
+        if ((*response)[idx].intensity < (*response)[index].intensity)
         {
           is_maxima = false;
           break;
@@ -248,7 +249,7 @@ pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloud
       if (is_maxima)
         #pragma omp critical
       {
-        output.points.push_back ((*response)[idx]);
+        output.push_back ((*response)[idx]);
         keypoints_indices_->indices.push_back (idx);
       }
     }
@@ -281,7 +282,7 @@ pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::responseTomasi (PointCloudO
     pointOut.intensity = 0.0; //std::numeric_limits<float>::quiet_NaN ();
     if (isFinite (pointIn))
     {
-      std::vector<int> nn_indices;
+      pcl::Indices nn_indices;
       std::vector<float> nn_dists;
       tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
       calculateCombinedCovar (nn_indices, covar);
@@ -346,7 +347,7 @@ pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::responseTomasi (PointCloudO
     pointOut.z = pointIn.z;
 
     #pragma omp critical
-    output.points.push_back(pointOut);
+    output.push_back(pointOut);
   }
   output.height = input_->height;
   output.width = input_->width;
@@ -375,13 +376,13 @@ pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::refineCorners (PointCloudOu
       corner.x = cornerIt->x;
       corner.y = cornerIt->y;
       corner.z = cornerIt->z;
-      std::vector<int> nn_indices;
+      pcl::Indices nn_indices;
       std::vector<float> nn_dists;      
       search.radiusSearch (corner, search_radius_, nn_indices, nn_dists);
-      for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
+      for (const auto& index : nn_indices)
       {
-        normal = reinterpret_cast<const Eigen::Vector3f*> (&((*normals_)[*iIt].normal_x));
-        point = reinterpret_cast<const Eigen::Vector3f*> (&((*surface_)[*iIt].x));
+        normal = reinterpret_cast<const Eigen::Vector3f*> (&((*normals_)[index].normal_x));
+        point = reinterpret_cast<const Eigen::Vector3f*> (&((*surface_)[index].x));
         nnT = (*normal) * (normal->transpose());
         NNT += nnT;
         NNTp += nnT * (*point);
diff --git a/keypoints/include/pcl/keypoints/impl/iss_3d.hpp b/keypoints/include/pcl/keypoints/impl/iss_3d.hpp
index ff618d83..550c6e30 100644
--- a/keypoints/include/pcl/keypoints/impl/iss_3d.hpp
+++ b/keypoints/include/pcl/keypoints/impl/iss_3d.hpp
@@ -38,6 +38,7 @@
 #ifndef PCL_ISS_KEYPOINT3D_IMPL_H_
 #define PCL_ISS_KEYPOINT3D_IMPL_H_
 
+#include <Eigen/Eigenvalues> // for SelfAdjointEigenSolver
 #include <pcl/features/boundary.h>
 #include <pcl/features/normal_3d.h>
 #include <pcl/features/integral_image_normal.h>
@@ -124,11 +125,11 @@ pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::getBoundaryPoints (PointCloudI
 
     if (pcl::isFinite(current_point))
     {
-      std::vector<int> nn_indices;
+      pcl::Indices nn_indices;
       std::vector<float> nn_distances;
       int n_neighbors;
 
-      this->searchForNeighbors (static_cast<int> (index), border_radius, nn_indices, nn_distances);
+      this->searchForNeighbors (index, border_radius, nn_indices, nn_distances);
 
       n_neighbors = static_cast<int> (nn_indices.size ());
 
@@ -160,7 +161,7 @@ pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::getScatterMatrix (const int& c
 
   cov_m = Eigen::Matrix3d::Zero ();
 
-  std::vector<int> nn_indices;
+  pcl::Indices nn_indices;
   std::vector<float> nn_distances;
   int n_neighbors;
 
@@ -174,9 +175,9 @@ pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::getScatterMatrix (const int& c
   double cov[9];
   memset(cov, 0, sizeof(double) * 9);
 
-  for (int n_idx = 0; n_idx < n_neighbors; n_idx++)
+  for (const auto& n_idx : nn_indices)
   {
-    const PointInT& n_point = (*input_)[nn_indices[n_idx]];
+    const PointInT& n_point = (*input_)[n_idx];
 
     double neigh_point[3];
     memset(neigh_point, 0, sizeof(double) * 3);
@@ -292,7 +293,7 @@ template<typename PointInT, typename PointOutT, typename NormalT> void
 pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOut &output)
 {
   // Make sure the output cloud is empty
-  output.points.clear ();
+  output.clear ();
 
   if (border_radius_ > 0.0)
     edge_points_ = getBoundaryPoints (*(input_->makeShared ()), border_radius_, angle_threshold_);
@@ -310,12 +311,12 @@ pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOut
 
     if ((border_radius_ > 0.0) && (pcl::isFinite(current_point)))
     {
-      std::vector<int> nn_indices;
+      pcl::Indices nn_indices;
       std::vector<float> nn_distances;
 
-      this->searchForNeighbors (static_cast<int> (index), border_radius_, nn_indices, nn_distances);
+      this->searchForNeighbors (index, border_radius_, nn_indices, nn_distances);
 
-      for (const int &nn_index : nn_indices)
+      for (const auto &nn_index : nn_indices)
       {
         if (edge_points_[nn_index])
         {
@@ -409,11 +410,11 @@ pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOut
 
     if ((third_eigen_value_[index] > 0.0) && (pcl::isFinite(current_point)))
     {
-      std::vector<int> nn_indices;
+      pcl::Indices nn_indices;
       std::vector<float> nn_distances;
       int n_neighbors;
 
-      this->searchForNeighbors (static_cast<int> (index), non_max_radius_, nn_indices, nn_distances);
+      this->searchForNeighbors (index, non_max_radius_, nn_indices, nn_distances);
 
       n_neighbors = static_cast<int> (nn_indices.size ());
 
@@ -421,8 +422,8 @@ pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOut
       {
         bool is_max = true;
 
-        for (int j = 0 ; j < n_neighbors; j++)
-          if (third_eigen_value_[index] < third_eigen_value_[nn_indices[j]])
+        for (const auto& j : nn_indices)
+          if (third_eigen_value_[index] < third_eigen_value_[j])
             is_max = false;
         if (is_max)
           feat_max[index] = true;
@@ -441,7 +442,7 @@ pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOut
     {
       PointOutT p;
       p.getVector3fMap () = (*input_)[index].getVector3fMap ();
-      output.points.push_back(p);
+      output.push_back(p);
       keypoints_indices_->indices.push_back (index);
     }
   }
diff --git a/keypoints/include/pcl/keypoints/impl/keypoint.hpp b/keypoints/include/pcl/keypoints/impl/keypoint.hpp
index 5a29c444..33985927 100644
--- a/keypoints/include/pcl/keypoints/impl/keypoint.hpp
+++ b/keypoints/include/pcl/keypoints/impl/keypoint.hpp
@@ -39,6 +39,10 @@
 #ifndef PCL_KEYPOINT_IMPL_H_
 #define PCL_KEYPOINT_IMPL_H_
 
+#include <pcl/console/print.h> // for PCL_ERROR
+
+#include <pcl/search/organized.h> // for OrganizedNeighbor
+#include <pcl/search/kdtree.h> // for KdTree
 
 namespace pcl
 {
@@ -79,7 +83,7 @@ Keypoint<PointInT, PointOutT>::initCompute ()
     if (surface_ == input_)       // if the two surfaces are the same
     {
       // Declare the search locator definition
-      search_method_ = [this] (int index, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances)
+      search_method_ = [this] (pcl::index_t index, double radius, pcl::Indices &k_indices, std::vector<float> &k_distances)
       {
         return tree_->radiusSearch (index, radius, k_indices, k_distances, 0);
       };
@@ -87,7 +91,7 @@ Keypoint<PointInT, PointOutT>::initCompute ()
     else
     {
       // Declare the search locator definition
-      search_method_surface_ = [this] (const PointCloudIn &cloud, int index, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances)
+      search_method_surface_ = [this] (const PointCloudIn &cloud, pcl::index_t index, double radius, pcl::Indices &k_indices, std::vector<float> &k_distances)
       {
         return tree_->radiusSearch (cloud, index, radius, k_indices, k_distances, 0);
       };
@@ -101,7 +105,7 @@ Keypoint<PointInT, PointOutT>::initCompute ()
       if (surface_ == input_)       // if the two surfaces are the same
       {
         // Declare the search locator definition
-        search_method_ = [this] (int index, int k, std::vector<int> &k_indices, std::vector<float> &k_distances)
+        search_method_ = [this] (pcl::index_t index, int k, pcl::Indices &k_indices, std::vector<float> &k_distances)
         {
           return tree_->nearestKSearch (index, k, k_indices, k_distances);
         };
@@ -109,7 +113,7 @@ Keypoint<PointInT, PointOutT>::initCompute ()
       else
       {
         // Declare the search locator definition
-        search_method_surface_ = [this] (const PointCloudIn &cloud, int index, int k, std::vector<int> &k_indices, std::vector<float> &k_distances)
+        search_method_surface_ = [this] (const PointCloudIn &cloud, pcl::index_t index, int k, pcl::Indices &k_indices, std::vector<float> &k_distances)
         {
           return tree_->nearestKSearch (cloud, index, k, k_indices, k_distances);
         };
diff --git a/keypoints/include/pcl/keypoints/impl/sift_keypoint.hpp b/keypoints/include/pcl/keypoints/impl/sift_keypoint.hpp
index d47cf889..c9a5e100 100644
--- a/keypoints/include/pcl/keypoints/impl/sift_keypoint.hpp
+++ b/keypoints/include/pcl/keypoints/impl/sift_keypoint.hpp
@@ -109,7 +109,7 @@ pcl::SIFTKeypoint<PointInT, PointOutT>::detectKeypoints (PointCloudOut &output)
   scale_idx_ = pcl::getFieldIndex<PointOutT> ("scale", out_fields_);
 
   // Make sure the output cloud is empty
-  output.points.clear ();
+  output.clear ();
 
   // Create a local copy of the input cloud that will be resized for each octave
   typename pcl::PointCloud<PointInT>::Ptr cloud (new pcl::PointCloud<PointInT> (*input_));
@@ -167,10 +167,11 @@ pcl::SIFTKeypoint<PointInT, PointOutT>::detectKeypointsForOctave (
   computeScaleSpace (input, tree, scales, diff_of_gauss);
 
   // Find extrema in the DoG scale space
-  std::vector<int> extrema_indices, extrema_scales;
+  pcl::Indices extrema_indices;
+  std::vector<int> extrema_scales;
   findScaleSpaceExtrema (input, tree, diff_of_gauss, extrema_indices, extrema_scales);
 
-  output.points.reserve (output.size () + extrema_indices.size ());
+  output.reserve (output.size () + extrema_indices.size ());
   // Save scale?
   if (scale_idx_ != -1)
   {
@@ -178,27 +179,27 @@ pcl::SIFTKeypoint<PointInT, PointOutT>::detectKeypointsForOctave (
     for (std::size_t i_keypoint = 0; i_keypoint < extrema_indices.size (); ++i_keypoint)
     {
       PointOutT keypoint;
-      const int &keypoint_index = extrema_indices[i_keypoint];
+      const auto &keypoint_index = extrema_indices[i_keypoint];
    
       keypoint.x = input[keypoint_index].x;
       keypoint.y = input[keypoint_index].y;
       keypoint.z = input[keypoint_index].z;
       memcpy (reinterpret_cast<char*> (&keypoint) + out_fields_[scale_idx_].offset,
               &scales[extrema_scales[i_keypoint]], sizeof (float));
-      output.points.push_back (keypoint); 
+      output.push_back (keypoint); 
     }
   }
   else
   {
     // Add keypoints to output
-    for (const int &keypoint_index : extrema_indices)
+    for (const auto &keypoint_index : extrema_indices)
     {
       PointOutT keypoint;
       keypoint.x = input[keypoint_index].x;
       keypoint.y = input[keypoint_index].y;
       keypoint.z = input[keypoint_index].z;
 
-      output.points.push_back (keypoint); 
+      output.push_back (keypoint); 
     }
   }
 }
@@ -217,7 +218,7 @@ void pcl::SIFTKeypoint<PointInT, PointOutT>::computeScaleSpace (
 
   for (int i_point = 0; i_point < static_cast<int> (input.size ()); ++i_point)
   {
-    std::vector<int> nn_indices;
+    pcl::Indices nn_indices;
     std::vector<float> nn_dist;
     tree.radiusSearch (i_point, max_radius, nn_indices, nn_dist); // *
     // * note: at this stage of the algorithm, we must find all points within a radius defined by the maximum scale, 
@@ -258,10 +259,10 @@ void pcl::SIFTKeypoint<PointInT, PointOutT>::computeScaleSpace (
 template <typename PointInT, typename PointOutT> void 
 pcl::SIFTKeypoint<PointInT, PointOutT>::findScaleSpaceExtrema (
     const PointCloudIn &input, KdTree &tree, const Eigen::MatrixXf &diff_of_gauss, 
-    std::vector<int> &extrema_indices, std::vector<int> &extrema_scales)
+    pcl::Indices &extrema_indices, std::vector<int> &extrema_scales)
 {
   const int k = 25;
-  std::vector<int> nn_indices (k);
+  pcl::Indices nn_indices (k);
   std::vector<float> nn_dist (k);
 
   const int nr_scales = static_cast<int> (diff_of_gauss.cols ());
diff --git a/keypoints/include/pcl/keypoints/impl/smoothed_surfaces_keypoint.hpp b/keypoints/include/pcl/keypoints/impl/smoothed_surfaces_keypoint.hpp
index 902f0d25..2c1a3dff 100644
--- a/keypoints/include/pcl/keypoints/impl/smoothed_surfaces_keypoint.hpp
+++ b/keypoints/include/pcl/keypoints/impl/smoothed_surfaces_keypoint.hpp
@@ -95,14 +95,14 @@ pcl::SmoothedSurfacesKeypoint<PointT, PointNT>::detectKeypoints (PointCloudT &ou
 
   // Find minima and maxima in differences inside the input cloud
   typename pcl::search::Search<PointT>::Ptr input_tree = cloud_trees_.back ();
-  for (int point_i = 0; point_i < static_cast<int> (input_->size ()); ++point_i)
+  for (pcl::index_t point_i = 0; point_i < static_cast<pcl::index_t> (input_->size ()); ++point_i)
   {
-    std::vector<int> nn_indices;
+    pcl::Indices nn_indices;
     std::vector<float> nn_distances;
     input_tree->radiusSearch (point_i, input_scale_ * neighborhood_constant_, nn_indices, nn_distances);
 
     bool is_min = true, is_max = true;
-    for (const int &nn_index : nn_indices)
+    for (const auto &nn_index : nn_indices)
       if (nn_index != point_i)
       {
         if (diffs[input_index_][point_i] < diffs[input_index_][nn_index])
@@ -126,7 +126,7 @@ pcl::SmoothedSurfacesKeypoint<PointT, PointNT>::detectKeypoints (PointCloudT &ou
         cloud_trees_[cloud_i]->radiusSearch (point_i, scales_[scale_i].first * neighborhood_constant_, nn_indices, nn_distances);
 
         bool is_min_other_scale = true, is_max_other_scale = true;
-        for (const int &nn_index : nn_indices)
+        for (const auto &nn_index : nn_indices)
           if (nn_index != point_i)
           {
             if (diffs[input_index_][point_i] < diffs[cloud_i][nn_index])
@@ -147,7 +147,7 @@ pcl::SmoothedSurfacesKeypoint<PointT, PointNT>::detectKeypoints (PointCloudT &ou
       // check if point was minimum/maximum over all the scales
       if (passed_min || passed_max)
       {
-        output.points.push_back ((*input_)[point_i]);
+        output.push_back ((*input_)[point_i]);
         keypoints_indices_->indices.push_back (point_i);
       }
     }
diff --git a/keypoints/include/pcl/keypoints/impl/susan.hpp b/keypoints/include/pcl/keypoints/impl/susan.hpp
index c267ded0..1f19d08a 100644
--- a/keypoints/include/pcl/keypoints/impl/susan.hpp
+++ b/keypoints/include/pcl/keypoints/impl/susan.hpp
@@ -38,6 +38,7 @@
 #ifndef PCL_SUSAN_IMPL_HPP_
 #define PCL_SUSAN_IMPL_HPP_
 
+#include <pcl/common/io.h> // for getFieldIndex
 #include <pcl/keypoints/susan.h>
 #include <pcl/features/normal_3d.h>
 #include <pcl/features/integral_image_normal.h>
@@ -111,7 +112,7 @@ pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::setNumberOfThreads
 // template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
 // pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::USAN (const PointInT& nucleus,
 //                                                                     const NormalT& nucleus_normal,
-//                                                                     const std::vector<int>& neighbors, 
+//                                                                     const pcl::Indices& neighbors, 
 //                                                                     const float& t,
 //                                                                     float& response,
 //                                                                     Eigen::Vector3f& centroid) const
@@ -124,15 +125,15 @@ pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::setNumberOfThreads
 //   //xx xy xz yy yz zz
 //   std::vector<float> coefficients(6);
 //   memset (&coefficients[0], 0, sizeof (float) * 6);
-//   for (std::vector<int>::const_iterator index = neighbors.begin(); index != neighbors.end(); ++index)
+//   for (const auto& index : neighbors)
 //   {
-//     if (std::isfinite ((*normals_)[*index].normal_x))
+//     if (std::isfinite ((*normals_)[index].normal_x))
 //     {
-//       Eigen::Vector3f diff = (*normals_)[*index].getNormal3fMap () - nucleus_normal.getNormal3fMap ();
+//       Eigen::Vector3f diff = (*normals_)[index].getNormal3fMap () - nucleus_normal.getNormal3fMap ();
 //       float c = diff.norm () / t;
 //       c = -1 * pow (c, 6.0);
 //       c = std::exp (c);
-//       Eigen::Vector3f xyz = (*surface_)[*index].getVector3fMap ();
+//       Eigen::Vector3f xyz = (*surface_)[index].getVector3fMap ();
 //       centroid += c * xyz;
 //       area += c;
 //       coefficients[0] += c * (x0 - xyz.x) * (x0 - xyz.x);
@@ -308,8 +309,8 @@ pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::detectKeypoints (P
   // Check if the output has a "label" field
   label_idx_ = pcl::getFieldIndex<PointOutT> ("label", out_fields_);
 
-  const int input_size = static_cast<int> (input_->size ());
-  for (int point_index = 0; point_index < input_size; ++point_index)
+  const auto input_size = static_cast<pcl::index_t> (input_->size ());
+  for (pcl::index_t point_index = 0; point_index < input_size; ++point_index)
   {
     const PointInT& point_in = input_->points [point_index];
     const NormalT& normal_in = normals_->points [point_index];
@@ -319,24 +320,24 @@ pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::detectKeypoints (P
     Eigen::Vector3f nucleus = point_in.getVector3fMap ();
     Eigen::Vector3f nucleus_normal = normals_->points [point_index].getNormalVector3fMap ();
     float nucleus_intensity = intensity_ (point_in);
-    std::vector<int> nn_indices;
+    pcl::Indices nn_indices;
     std::vector<float> nn_dists;
     tree_->radiusSearch (point_in, search_radius_, nn_indices, nn_dists);
     float area = 0;
     Eigen::Vector3f centroid = Eigen::Vector3f::Zero ();
     // Exclude nucleus from the usan
     std::vector<int> usan; usan.reserve (nn_indices.size () - 1);
-    for (std::vector<int>::const_iterator index = nn_indices.begin(); index != nn_indices.end(); ++index)
+    for (const auto& index : nn_indices)
     {
-      if ((*index != point_index) && std::isfinite ((*normals_)[*index].normal_x))
+      if ((index != point_index) && std::isfinite ((*normals_)[index].normal_x))
       {
         // if the point fulfill condition
-        if ((std::abs (nucleus_intensity - intensity_ ((*input_)[*index])) <= intensity_threshold_) ||
-            (1 - nucleus_normal.dot ((*normals_)[*index].getNormalVector3fMap ()) <= angular_threshold_))
+        if ((std::abs (nucleus_intensity - intensity_ ((*input_)[index])) <= intensity_threshold_) ||
+            (1 - nucleus_normal.dot ((*normals_)[index].getNormalVector3fMap ()) <= angular_threshold_))
         {
           ++area;
-          centroid += (*input_)[*index].getVector3fMap ();
-          usan.push_back (*index);
+          centroid += (*input_)[index].getVector3fMap ();
+          usan.push_back (index);
         }
       }
     }
@@ -412,10 +413,10 @@ pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::detectKeypoints (P
   }
   else
   {
-    output.points.clear ();
-    output.points.reserve (response->size());
+    output.clear ();
+    output.reserve (response->size());
     
-    for (int idx = 0; idx < static_cast<int> (response->size ()); ++idx)
+    for (pcl::index_t idx = 0; idx < static_cast<pcl::index_t> (response->size ()); ++idx)
     {
       const PointOutT& point_in = response->points [idx];
       const NormalT& normal_in = normals_->points [idx];
@@ -423,14 +424,14 @@ pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::detectKeypoints (P
       const float intensity = intensity_out_ ((*response)[idx]);
       if (!isFinite (point_in) || !isFinite (normal_in) || (intensity == 0))
         continue;
-      std::vector<int> nn_indices;
+      pcl::Indices nn_indices;
       std::vector<float> nn_dists;
       tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists);
       bool is_minima = true;
-      for (std::vector<int>::const_iterator nn_it = nn_indices.begin(); nn_it != nn_indices.end(); ++nn_it)
+      for (const auto& nn_index : nn_indices)
       {
-//        if (intensity > (*response)[*nn_it].intensity)
-        if (intensity > intensity_out_ ((*response)[*nn_it]))
+//        if (intensity > (*response)[nn_index].intensity)
+        if (intensity > intensity_out_ ((*response)[nn_index]))
         {
           is_minima = false;
           break;
@@ -438,7 +439,7 @@ pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::detectKeypoints (P
       }
       if (is_minima)
       {
-        output.points.push_back ((*response)[idx]);
+        output.push_back ((*response)[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 8efaa817..f3423acc 100644
--- a/keypoints/include/pcl/keypoints/impl/trajkovic_2d.hpp
+++ b/keypoints/include/pcl/keypoints/impl/trajkovic_2d.hpp
@@ -225,7 +225,7 @@ TrajkovicKeypoint2D<PointInT, PointOutT, IntensityT>::detectKeypoints (PointClou
   }
 
   // Non maximas suppression
-  std::vector<int> indices = *indices_;
+  pcl::Indices indices = *indices_;
   std::sort (indices.begin (), indices.end (), [this] (int p1, int p2) { return greaterCornernessAtIndices (p1, p2); });
 
   output.clear ();
diff --git a/keypoints/include/pcl/keypoints/impl/trajkovic_3d.hpp b/keypoints/include/pcl/keypoints/impl/trajkovic_3d.hpp
index 55f83cf0..0edbcb07 100644
--- a/keypoints/include/pcl/keypoints/impl/trajkovic_3d.hpp
+++ b/keypoints/include/pcl/keypoints/impl/trajkovic_3d.hpp
@@ -240,7 +240,7 @@ TrajkovicKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOu
     }
   }
   // Non maximas suppression
-  std::vector<int> indices = *indices_;
+  pcl::Indices indices = *indices_;
   std::sort (indices.begin (), indices.end (), [this] (int p1, int p2) { return greaterCornernessAtIndices (p1, p2); });
 
   output.clear ();
diff --git a/keypoints/include/pcl/keypoints/iss_3d.h b/keypoints/include/pcl/keypoints/iss_3d.h
index a71a899f..d8ef0c94 100644
--- a/keypoints/include/pcl/keypoints/iss_3d.h
+++ b/keypoints/include/pcl/keypoints/iss_3d.h
@@ -36,6 +36,7 @@
 #pragma once
 
 #include <pcl/keypoints/keypoint.h>
+#include <pcl/octree/octree_search.h> // for OctreePointCloudSearch
 
 namespace pcl
 {
diff --git a/keypoints/include/pcl/keypoints/keypoint.h b/keypoints/include/pcl/keypoints/keypoint.h
index f019d764..02381195 100644
--- a/keypoints/include/pcl/keypoints/keypoint.h
+++ b/keypoints/include/pcl/keypoints/keypoint.h
@@ -39,7 +39,7 @@
 
 // PCL includes
 #include <pcl/pcl_base.h>
-#include <pcl/search/pcl_search.h>
+#include <pcl/search/search.h> // for Search
 #include <pcl/pcl_config.h>
 
 #include <functional>
@@ -67,8 +67,8 @@ namespace pcl
       using PointCloudInPtr = typename PointCloudIn::Ptr;
       using PointCloudInConstPtr = typename PointCloudIn::ConstPtr;
       using PointCloudOut = pcl::PointCloud<PointOutT>;
-      using SearchMethod = std::function<int (int, double, std::vector<int> &, std::vector<float> &)>;
-      using SearchMethodSurface = std::function<int (const PointCloudIn &cloud, int index, double, std::vector<int> &, std::vector<float> &)>;
+      using SearchMethod = std::function<int (pcl::index_t, double, pcl::Indices &, std::vector<float> &)>;
+      using SearchMethodSurface = std::function<int (const PointCloudIn &cloud, pcl::index_t index, double, pcl::Indices &, std::vector<float> &)>;
 
     public:
       /** \brief Empty constructor. */
@@ -152,7 +152,7 @@ namespace pcl
         * k-nearest neighbors
         */
       inline int
-      searchForNeighbors (int index, double parameter, std::vector<int> &indices, std::vector<float> &distances) const
+      searchForNeighbors (pcl::index_t index, double parameter, pcl::Indices &indices, std::vector<float> &distances) const
       {
         if (surface_ == input_)       // if the two surfaces are the same
           return (search_method_ (index, parameter, indices, distances));
diff --git a/keypoints/include/pcl/keypoints/sift_keypoint.h b/keypoints/include/pcl/keypoints/sift_keypoint.h
index 4e33d5fa..688d87d4 100644
--- a/keypoints/include/pcl/keypoints/sift_keypoint.h
+++ b/keypoints/include/pcl/keypoints/sift_keypoint.h
@@ -174,7 +174,7 @@ namespace pcl
       void 
       findScaleSpaceExtrema (const PointCloudIn &input, KdTree &tree, 
                              const Eigen::MatrixXf &diff_of_gauss,
-                             std::vector<int> &extrema_indices, std::vector<int> &extrema_scales);
+                             pcl::Indices &extrema_indices, std::vector<int> &extrema_scales);
 
 
       /** \brief The standard deviation of the smallest scale in the scale space.*/
diff --git a/keypoints/include/pcl/keypoints/uniform_sampling.h b/keypoints/include/pcl/keypoints/uniform_sampling.h
index 4b9da135..66596274 100644
--- a/keypoints/include/pcl/keypoints/uniform_sampling.h
+++ b/keypoints/include/pcl/keypoints/uniform_sampling.h
@@ -39,8 +39,6 @@
 
 #pragma once
 
-#ifdef __DEPRECATED
-PCL_DEPRECATED_HEADER(1, 12, "UniformSampling is not a Keypoint anymore, use <pcl/filters/uniform_sampling.h> instead.")
-#endif
+PCL_DEPRECATED_HEADER(1, 15, "UniformSampling is not a Keypoint anymore, use <pcl/filters/uniform_sampling.h> instead.")
 
 #include <pcl/filters/uniform_sampling.h>
diff --git a/keypoints/src/brisk_2d.cpp b/keypoints/src/brisk_2d.cpp
index 65de55a0..a3c797f0 100644
--- a/keypoints/src/brisk_2d.cpp
+++ b/keypoints/src/brisk_2d.cpp
@@ -1701,7 +1701,7 @@ pcl::keypoints::brisk::Layer::halfsample (
   }
 #else
   pcl::utils::ignore(srcimg, srcwidth, srcheight, dstimg, dstwidth);
-  PCL_ERROR("brisk without SSSE3 support not implemented");
+  PCL_ERROR("brisk without SSSE3 support not implemented\n");
 #endif
 }
 
@@ -1811,7 +1811,7 @@ pcl::keypoints::brisk::Layer::twothirdsample (
   }
 #else
   pcl::utils::ignore(srcimg, srcwidth, srcheight, dstimg, dstwidth);
-  PCL_ERROR("brisk without SSSE3 support not implemented");
+  PCL_ERROR("brisk without SSSE3 support not implemented\n");
 #endif
 }
 
diff --git a/keypoints/src/narf_keypoint.cpp b/keypoints/src/narf_keypoint.cpp
index 62901795..da466453 100644
--- a/keypoints/src/narf_keypoint.cpp
+++ b/keypoints/src/narf_keypoint.cpp
@@ -877,7 +877,7 @@ NarfKeypoint::getRangeImage ()
 void 
 NarfKeypoint::detectKeypoints (NarfKeypoint::PointCloudOut& output)
 {
-  output.points.clear ();
+  output.clear ();
   
   if (indices_)
   {
@@ -911,7 +911,7 @@ NarfKeypoint::detectKeypoints (NarfKeypoint::PointCloudOut& output)
   {
     if (!is_interest_point_image_[index])
       continue;
-    output.points.push_back (index);
+    output.push_back (index);
   }
 }
 
diff --git a/ml/include/pcl/ml/impl/dt/decision_tree_trainer.hpp b/ml/include/pcl/ml/impl/dt/decision_tree_trainer.hpp
index 8a01488d..5c7613c1 100644
--- a/ml/include/pcl/ml/impl/dt/decision_tree_trainer.hpp
+++ b/ml/include/pcl/ml/impl/dt/decision_tree_trainer.hpp
@@ -119,7 +119,7 @@ DecisionTreeTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::
   const std::size_t num_of_examples = examples.size();
   if (num_of_examples == 0) {
     PCL_ERROR(
-        "Reached invalid point in decision tree training: Number of examples is 0!");
+        "Reached invalid point in decision tree training: Number of examples is 0!\n");
     return;
   };
 
diff --git a/ml/src/kmeans.cpp b/ml/src/kmeans.cpp
index 8ce1226c..32c5aeb5 100644
--- a/ml/src/kmeans.cpp
+++ b/ml/src/kmeans.cpp
@@ -97,6 +97,10 @@ pcl::Kmeans::computeCentroids()
   for (Centroids::value_type& centroid : centroids_) {
     PointId num_points_in_cluster = 0;
 
+    // Set zero
+    for (Point::value_type& c : centroid)
+      c = 0.0;
+
     // For each PointId in this set
     for (const auto& pid : clusters_to_points_[cid]) {
       Point p = data_[pid];
diff --git a/octree/include/pcl/octree/boost.h b/octree/include/pcl/octree/boost.h
index 3510b239..b3680aab 100644
--- a/octree/include/pcl/octree/boost.h
+++ b/octree/include/pcl/octree/boost.h
@@ -42,6 +42,7 @@
 #ifdef __GNUC__
 #pragma GCC system_header
 #endif
+PCL_DEPRECATED_HEADER(1, 15, "Please include the needed boost headers directly.")
 
 // Marking all Boost headers as system headers to remove warnings
 #include <boost/graph/adjacency_list.hpp>
diff --git a/octree/include/pcl/octree/impl/octree2buf_base.hpp b/octree/include/pcl/octree/impl/octree2buf_base.hpp
index edaea812..67628df6 100644
--- a/octree/include/pcl/octree/impl/octree2buf_base.hpp
+++ b/octree/include/pcl/octree/impl/octree2buf_base.hpp
@@ -67,17 +67,17 @@ Octree2BufBase<LeafContainerT, BranchContainerT>::~Octree2BufBase()
 template <typename LeafContainerT, typename BranchContainerT>
 void
 Octree2BufBase<LeafContainerT, BranchContainerT>::setMaxVoxelIndex(
-    unsigned int max_voxel_index_arg)
+    uindex_t max_voxel_index_arg)
 {
-  unsigned int treeDepth;
+  uindex_t treeDepth;
 
   assert(max_voxel_index_arg > 0);
 
   // tree depth == amount of bits of maxVoxels
-  treeDepth = std::max(
-      (std::min(static_cast<unsigned int>(OctreeKey::maxDepth),
-                static_cast<unsigned int>(std::ceil(std::log2(max_voxel_index_arg))))),
-      static_cast<unsigned int>(0));
+  treeDepth =
+      std::max<uindex_t>(std::min<uindex_t>(OctreeKey::maxDepth,
+                                            std::ceil(std::log2(max_voxel_index_arg))),
+                         0);
 
   // define depthMask_ by setting a single bit to 1 at bit position == tree depth
   depth_mask_ = (1 << (treeDepth - 1));
@@ -86,7 +86,7 @@ Octree2BufBase<LeafContainerT, BranchContainerT>::setMaxVoxelIndex(
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename LeafContainerT, typename BranchContainerT>
 void
-Octree2BufBase<LeafContainerT, BranchContainerT>::setTreeDepth(unsigned int depth_arg)
+Octree2BufBase<LeafContainerT, BranchContainerT>::setTreeDepth(uindex_t depth_arg)
 {
   assert(depth_arg > 0);
 
@@ -103,9 +103,9 @@ Octree2BufBase<LeafContainerT, BranchContainerT>::setTreeDepth(unsigned int dept
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename LeafContainerT, typename BranchContainerT>
 LeafContainerT*
-Octree2BufBase<LeafContainerT, BranchContainerT>::findLeaf(unsigned int idx_x_arg,
-                                                           unsigned int idx_y_arg,
-                                                           unsigned int idx_z_arg)
+Octree2BufBase<LeafContainerT, BranchContainerT>::findLeaf(uindex_t idx_x_arg,
+                                                           uindex_t idx_y_arg,
+                                                           uindex_t idx_z_arg)
 {
   // generate key
   OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
@@ -117,9 +117,9 @@ Octree2BufBase<LeafContainerT, BranchContainerT>::findLeaf(unsigned int idx_x_ar
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename LeafContainerT, typename BranchContainerT>
 LeafContainerT*
-Octree2BufBase<LeafContainerT, BranchContainerT>::createLeaf(unsigned int idx_x_arg,
-                                                             unsigned int idx_y_arg,
-                                                             unsigned int idx_z_arg)
+Octree2BufBase<LeafContainerT, BranchContainerT>::createLeaf(uindex_t idx_x_arg,
+                                                             uindex_t idx_y_arg,
+                                                             uindex_t idx_z_arg)
 {
   // generate key
   OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
@@ -131,8 +131,9 @@ Octree2BufBase<LeafContainerT, BranchContainerT>::createLeaf(unsigned int idx_x_
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename LeafContainerT, typename BranchContainerT>
 bool
-Octree2BufBase<LeafContainerT, BranchContainerT>::existLeaf(
-    unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) const
+Octree2BufBase<LeafContainerT, BranchContainerT>::existLeaf(uindex_t idx_x_arg,
+                                                            uindex_t idx_y_arg,
+                                                            uindex_t idx_z_arg) const
 {
   // generate key
   OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
@@ -144,9 +145,9 @@ Octree2BufBase<LeafContainerT, BranchContainerT>::existLeaf(
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename LeafContainerT, typename BranchContainerT>
 void
-Octree2BufBase<LeafContainerT, BranchContainerT>::removeLeaf(unsigned int idx_x_arg,
-                                                             unsigned int idx_y_arg,
-                                                             unsigned int idx_z_arg)
+Octree2BufBase<LeafContainerT, BranchContainerT>::removeLeaf(uindex_t idx_x_arg,
+                                                             uindex_t idx_y_arg,
+                                                             uindex_t idx_z_arg)
 {
   // generate key
   OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
@@ -352,10 +353,10 @@ Octree2BufBase<LeafContainerT, BranchContainerT>::serializeNewLeafs(
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename LeafContainerT, typename BranchContainerT>
-unsigned int
+uindex_t
 Octree2BufBase<LeafContainerT, BranchContainerT>::createLeafRecursive(
     const OctreeKey& key_arg,
-    unsigned int depth_mask_arg,
+    uindex_t depth_mask_arg,
     BranchNode* branch_arg,
     LeafNode*& return_leaf_arg,
     BranchNode*& parent_of_leaf_arg,
@@ -431,6 +432,7 @@ Octree2BufBase<LeafContainerT, BranchContainerT>::createLeafRecursive(
       OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_, child_idx);
       if (child_node->getNodeType() == LEAF_NODE) {
         child_leaf = static_cast<LeafNode*>(child_node);
+        child_leaf->getContainer() = LeafContainer(); // Clear contents of leaf
         branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
       }
       else {
@@ -465,7 +467,7 @@ template <typename LeafContainerT, typename BranchContainerT>
 void
 Octree2BufBase<LeafContainerT, BranchContainerT>::findLeafRecursive(
     const OctreeKey& key_arg,
-    unsigned int depth_mask_arg,
+    uindex_t depth_mask_arg,
     BranchNode* branch_arg,
     LeafContainerT*& result_arg) const
 {
@@ -500,7 +502,7 @@ Octree2BufBase<LeafContainerT, BranchContainerT>::findLeafRecursive(
 template <typename LeafContainerT, typename BranchContainerT>
 bool
 Octree2BufBase<LeafContainerT, BranchContainerT>::deleteLeafRecursive(
-    const OctreeKey& key_arg, unsigned int depth_mask_arg, BranchNode* branch_arg)
+    const OctreeKey& key_arg, uindex_t depth_mask_arg, BranchNode* branch_arg)
 {
   // index to branch child
   unsigned char child_idx;
@@ -559,21 +561,17 @@ Octree2BufBase<LeafContainerT, BranchContainerT>::serializeTreeRecursive(
     bool do_XOR_encoding_arg,
     bool new_leafs_filter_arg)
 {
-  // bit pattern
-  char branch_bit_pattern_curr_buffer;
-  char branch_bit_pattern_prev_buffer;
-  char node_XOR_bit_pattern;
-
-  // occupancy bit patterns of branch node  (current and previous octree buffer)
-  branch_bit_pattern_curr_buffer = getBranchBitPattern(*branch_arg, buffer_selector_);
-  branch_bit_pattern_prev_buffer = getBranchBitPattern(*branch_arg, !buffer_selector_);
-
-  // XOR of current and previous occupancy bit patterns
-  node_XOR_bit_pattern =
-      branch_bit_pattern_curr_buffer ^ branch_bit_pattern_prev_buffer;
-
   if (binary_tree_out_arg) {
+    // occupancy bit patterns of branch node  (current octree buffer)
+    const char branch_bit_pattern_curr_buffer =
+        getBranchBitPattern(*branch_arg, buffer_selector_);
     if (do_XOR_encoding_arg) {
+      // occupancy bit patterns of branch node  (previous octree buffer)
+      const char branch_bit_pattern_prev_buffer =
+          getBranchBitPattern(*branch_arg, !buffer_selector_);
+      // XOR of current and previous occupancy bit patterns
+      const char node_XOR_bit_pattern =
+          branch_bit_pattern_curr_buffer ^ branch_bit_pattern_prev_buffer;
       // write XOR bit pattern to output vector
       binary_tree_out_arg->push_back(node_XOR_bit_pattern);
     }
@@ -642,7 +640,7 @@ template <typename LeafContainerT, typename BranchContainerT>
 void
 Octree2BufBase<LeafContainerT, BranchContainerT>::deserializeTreeRecursive(
     BranchNode* branch_arg,
-    unsigned int depth_mask_arg,
+    uindex_t depth_mask_arg,
     OctreeKey& key_arg,
     typename std::vector<char>::const_iterator& binaryTreeIT_arg,
     typename std::vector<char>::const_iterator& binaryTreeIT_End_arg,
diff --git a/octree/include/pcl/octree/impl/octree_base.hpp b/octree/include/pcl/octree/impl/octree_base.hpp
index 5cca0142..afd9d3c8 100644
--- a/octree/include/pcl/octree/impl/octree_base.hpp
+++ b/octree/include/pcl/octree/impl/octree_base.hpp
@@ -39,8 +39,6 @@
 #ifndef PCL_OCTREE_BASE_HPP
 #define PCL_OCTREE_BASE_HPP
 
-#include <pcl/impl/instantiate.hpp>
-
 #include <vector>
 
 namespace pcl {
@@ -69,16 +67,16 @@ OctreeBase<LeafContainerT, BranchContainerT>::~OctreeBase()
 template <typename LeafContainerT, typename BranchContainerT>
 void
 OctreeBase<LeafContainerT, BranchContainerT>::setMaxVoxelIndex(
-    unsigned int max_voxel_index_arg)
+    uindex_t max_voxel_index_arg)
 {
-  unsigned int tree_depth;
+  uindex_t tree_depth;
 
   assert(max_voxel_index_arg > 0);
 
   // tree depth == bitlength of maxVoxels
   tree_depth =
-      std::min(static_cast<unsigned int>(OctreeKey::maxDepth),
-               static_cast<unsigned int>(std::ceil(std::log2(max_voxel_index_arg))));
+      std::min(static_cast<uindex_t>(OctreeKey::maxDepth),
+               static_cast<uindex_t>(std::ceil(std::log2(max_voxel_index_arg))));
 
   // define depthMask_ by setting a single bit to 1 at bit position == tree depth
   depth_mask_ = (1 << (tree_depth - 1));
@@ -87,7 +85,7 @@ OctreeBase<LeafContainerT, BranchContainerT>::setMaxVoxelIndex(
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename LeafContainerT, typename BranchContainerT>
 void
-OctreeBase<LeafContainerT, BranchContainerT>::setTreeDepth(unsigned int depth_arg)
+OctreeBase<LeafContainerT, BranchContainerT>::setTreeDepth(uindex_t depth_arg)
 {
   assert(depth_arg > 0);
 
@@ -104,9 +102,9 @@ OctreeBase<LeafContainerT, BranchContainerT>::setTreeDepth(unsigned int depth_ar
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename LeafContainerT, typename BranchContainerT>
 LeafContainerT*
-OctreeBase<LeafContainerT, BranchContainerT>::findLeaf(unsigned int idx_x_arg,
-                                                       unsigned int idx_y_arg,
-                                                       unsigned int idx_z_arg)
+OctreeBase<LeafContainerT, BranchContainerT>::findLeaf(uindex_t idx_x_arg,
+                                                       uindex_t idx_y_arg,
+                                                       uindex_t idx_z_arg)
 {
   // generate key
   OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
@@ -118,9 +116,9 @@ OctreeBase<LeafContainerT, BranchContainerT>::findLeaf(unsigned int idx_x_arg,
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename LeafContainerT, typename BranchContainerT>
 LeafContainerT*
-OctreeBase<LeafContainerT, BranchContainerT>::createLeaf(unsigned int idx_x_arg,
-                                                         unsigned int idx_y_arg,
-                                                         unsigned int idx_z_arg)
+OctreeBase<LeafContainerT, BranchContainerT>::createLeaf(uindex_t idx_x_arg,
+                                                         uindex_t idx_y_arg,
+                                                         uindex_t idx_z_arg)
 {
   // generate key
   OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
@@ -132,9 +130,9 @@ OctreeBase<LeafContainerT, BranchContainerT>::createLeaf(unsigned int idx_x_arg,
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename LeafContainerT, typename BranchContainerT>
 bool
-OctreeBase<LeafContainerT, BranchContainerT>::existLeaf(unsigned int idx_x_arg,
-                                                        unsigned int idx_y_arg,
-                                                        unsigned int idx_z_arg) const
+OctreeBase<LeafContainerT, BranchContainerT>::existLeaf(uindex_t idx_x_arg,
+                                                        uindex_t idx_y_arg,
+                                                        uindex_t idx_z_arg) const
 {
   // generate key
   OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
@@ -146,9 +144,9 @@ OctreeBase<LeafContainerT, BranchContainerT>::existLeaf(unsigned int idx_x_arg,
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename LeafContainerT, typename BranchContainerT>
 void
-OctreeBase<LeafContainerT, BranchContainerT>::removeLeaf(unsigned int idx_x_arg,
-                                                         unsigned int idx_y_arg,
-                                                         unsigned int idx_z_arg)
+OctreeBase<LeafContainerT, BranchContainerT>::removeLeaf(uindex_t idx_x_arg,
+                                                         uindex_t idx_y_arg,
+                                                         uindex_t idx_z_arg)
 {
   // generate key
   OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
@@ -283,10 +281,10 @@ OctreeBase<LeafContainerT, BranchContainerT>::deserializeTree(
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename LeafContainerT, typename BranchContainerT>
-unsigned int
+uindex_t
 OctreeBase<LeafContainerT, BranchContainerT>::createLeafRecursive(
     const OctreeKey& key_arg,
-    unsigned int depth_mask_arg,
+    uindex_t depth_mask_arg,
     BranchNode* branch_arg,
     LeafNode*& return_leaf_arg,
     BranchNode*& parent_of_leaf_arg)
@@ -346,7 +344,7 @@ template <typename LeafContainerT, typename BranchContainerT>
 void
 OctreeBase<LeafContainerT, BranchContainerT>::findLeafRecursive(
     const OctreeKey& key_arg,
-    unsigned int depth_mask_arg,
+    uindex_t depth_mask_arg,
     BranchNode* branch_arg,
     LeafContainerT*& result_arg) const
 {
@@ -383,7 +381,7 @@ OctreeBase<LeafContainerT, BranchContainerT>::findLeafRecursive(
 template <typename LeafContainerT, typename BranchContainerT>
 bool
 OctreeBase<LeafContainerT, BranchContainerT>::deleteLeafRecursive(
-    const OctreeKey& key_arg, unsigned int depth_mask_arg, BranchNode* branch_arg)
+    const OctreeKey& key_arg, uindex_t depth_mask_arg, BranchNode* branch_arg)
 {
   // index to branch child
   unsigned char child_idx;
@@ -493,7 +491,7 @@ template <typename LeafContainerT, typename BranchContainerT>
 void
 OctreeBase<LeafContainerT, BranchContainerT>::deserializeTreeRecursive(
     BranchNode* branch_arg,
-    unsigned int depth_mask_arg,
+    uindex_t depth_mask_arg,
     OctreeKey& key_arg,
     typename std::vector<char>::const_iterator& binary_tree_input_it_arg,
     typename std::vector<char>::const_iterator& binary_tree_input_it_end_arg,
diff --git a/octree/include/pcl/octree/impl/octree_iterator.hpp b/octree/include/pcl/octree/impl/octree_iterator.hpp
index 76e33fab..1398cf5d 100644
--- a/octree/include/pcl/octree/impl/octree_iterator.hpp
+++ b/octree/include/pcl/octree/impl/octree_iterator.hpp
@@ -45,7 +45,7 @@ namespace pcl {
 namespace octree {
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename OctreeT>
-OctreeDepthFirstIterator<OctreeT>::OctreeDepthFirstIterator(unsigned int max_depth_arg)
+OctreeDepthFirstIterator<OctreeT>::OctreeDepthFirstIterator(uindex_t max_depth_arg)
 : OctreeIteratorBase<OctreeT>(max_depth_arg), stack_()
 {
   // initialize iterator
@@ -55,7 +55,7 @@ OctreeDepthFirstIterator<OctreeT>::OctreeDepthFirstIterator(unsigned int max_dep
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename OctreeT>
 OctreeDepthFirstIterator<OctreeT>::OctreeDepthFirstIterator(OctreeT* octree_arg,
-                                                            unsigned int max_depth_arg)
+                                                            uindex_t max_depth_arg)
 : OctreeIteratorBase<OctreeT>(octree_arg, max_depth_arg), stack_()
 {
   // initialize iterator
@@ -163,8 +163,7 @@ OctreeDepthFirstIterator<OctreeT>::operator++()
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename OctreeT>
-OctreeBreadthFirstIterator<OctreeT>::OctreeBreadthFirstIterator(
-    unsigned int max_depth_arg)
+OctreeBreadthFirstIterator<OctreeT>::OctreeBreadthFirstIterator(uindex_t max_depth_arg)
 : OctreeIteratorBase<OctreeT>(max_depth_arg), FIFO_()
 {
   OctreeIteratorBase<OctreeT>::reset();
@@ -175,8 +174,8 @@ OctreeBreadthFirstIterator<OctreeT>::OctreeBreadthFirstIterator(
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename OctreeT>
-OctreeBreadthFirstIterator<OctreeT>::OctreeBreadthFirstIterator(
-    OctreeT* octree_arg, unsigned int max_depth_arg)
+OctreeBreadthFirstIterator<OctreeT>::OctreeBreadthFirstIterator(OctreeT* octree_arg,
+                                                                uindex_t max_depth_arg)
 : OctreeIteratorBase<OctreeT>(octree_arg, max_depth_arg), FIFO_()
 {
   OctreeIteratorBase<OctreeT>::reset();
@@ -259,13 +258,13 @@ OctreeBreadthFirstIterator<OctreeT>::operator++()
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename OctreeT>
 OctreeFixedDepthIterator<OctreeT>::OctreeFixedDepthIterator()
-: OctreeBreadthFirstIterator<OctreeT>(0u), fixed_depth_(0u)
+: OctreeBreadthFirstIterator<OctreeT>(nullptr, 0), fixed_depth_(0u)
 {}
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename OctreeT>
-OctreeFixedDepthIterator<OctreeT>::OctreeFixedDepthIterator(
-    OctreeT* octree_arg, unsigned int fixed_depth_arg)
+OctreeFixedDepthIterator<OctreeT>::OctreeFixedDepthIterator(OctreeT* octree_arg,
+                                                            uindex_t fixed_depth_arg)
 : OctreeBreadthFirstIterator<OctreeT>(octree_arg, fixed_depth_arg)
 , fixed_depth_(fixed_depth_arg)
 {
@@ -275,7 +274,7 @@ OctreeFixedDepthIterator<OctreeT>::OctreeFixedDepthIterator(
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename OctreeT>
 void
-OctreeFixedDepthIterator<OctreeT>::reset(unsigned int fixed_depth_arg)
+OctreeFixedDepthIterator<OctreeT>::reset(uindex_t fixed_depth_arg)
 {
   // Set the desired depth to walk through
   fixed_depth_ = fixed_depth_arg;
@@ -315,7 +314,7 @@ OctreeFixedDepthIterator<OctreeT>::reset(unsigned int fixed_depth_arg)
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename OctreeT>
 OctreeLeafNodeBreadthFirstIterator<OctreeT>::OctreeLeafNodeBreadthFirstIterator(
-    unsigned int max_depth_arg)
+    uindex_t max_depth_arg)
 : OctreeBreadthFirstIterator<OctreeT>(max_depth_arg)
 {
   reset();
@@ -324,7 +323,7 @@ OctreeLeafNodeBreadthFirstIterator<OctreeT>::OctreeLeafNodeBreadthFirstIterator(
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename OctreeT>
 OctreeLeafNodeBreadthFirstIterator<OctreeT>::OctreeLeafNodeBreadthFirstIterator(
-    OctreeT* octree_arg, unsigned int max_depth_arg)
+    OctreeT* octree_arg, uindex_t max_depth_arg)
 : OctreeBreadthFirstIterator<OctreeT>(octree_arg, max_depth_arg)
 {
   reset();
@@ -334,7 +333,7 @@ OctreeLeafNodeBreadthFirstIterator<OctreeT>::OctreeLeafNodeBreadthFirstIterator(
 template <typename OctreeT>
 OctreeLeafNodeBreadthFirstIterator<OctreeT>::OctreeLeafNodeBreadthFirstIterator(
     OctreeT* octree_arg,
-    unsigned int max_depth_arg,
+    uindex_t max_depth_arg,
     IteratorState* current_state,
     const std::deque<IteratorState>& fifo)
 : OctreeBreadthFirstIterator<OctreeT>(octree_arg, max_depth_arg, current_state, fifo)
diff --git a/octree/include/pcl/octree/impl/octree_pointcloud.hpp b/octree/include/pcl/octree/impl/octree_pointcloud.hpp
index 19bced45..a7507d32 100644
--- a/octree/include/pcl/octree/impl/octree_pointcloud.hpp
+++ b/octree/include/pcl/octree/impl/octree_pointcloud.hpp
@@ -41,6 +41,7 @@
 #include <pcl/common/common.h>
 #include <pcl/common/point_tests.h> // for pcl::isFinite
 #include <pcl/octree/impl/octree_base.hpp>
+#include <pcl/types.h>
 
 #include <cassert>
 
@@ -78,8 +79,8 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
     addPointsFromInputCloud()
 {
   if (indices_) {
-    for (const int& index : *indices_) {
-      assert((index >= 0) && (index < static_cast<int>(input_->size())));
+    for (const auto& index : *indices_) {
+      assert((index >= 0) && (static_cast<std::size_t>(index) < input_->size()));
 
       if (isFinite((*input_)[index])) {
         // add points to octree
@@ -88,10 +89,10 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
     }
   }
   else {
-    for (std::size_t i = 0; i < input_->size(); i++) {
+    for (index_t i = 0; i < static_cast<index_t>(input_->size()); i++) {
       if (isFinite((*input_)[i])) {
         // add points to octree
-        this->addPointIdx(static_cast<unsigned int>(i));
+        this->addPointIdx(i);
       }
     }
   }
@@ -104,7 +105,7 @@ template <typename PointT,
           typename OctreeT>
 void
 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
-    addPointFromCloud(const int point_idx_arg, IndicesPtr indices_arg)
+    addPointFromCloud(const uindex_t point_idx_arg, IndicesPtr indices_arg)
 {
   this->addPointIdx(point_idx_arg);
   if (indices_arg)
@@ -124,7 +125,7 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
 
   cloud_arg->push_back(point_arg);
 
-  this->addPointIdx(static_cast<const int>(cloud_arg->size()) - 1);
+  this->addPointIdx(cloud_arg->size() - 1);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
@@ -143,7 +144,7 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
 
   cloud_arg->push_back(point_arg);
 
-  this->addPointFromCloud(static_cast<const int>(cloud_arg->size()) - 1, indices_arg);
+  this->addPointFromCloud(cloud_arg->size() - 1, indices_arg);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
@@ -175,7 +176,7 @@ template <typename PointT,
           typename OctreeT>
 bool
 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
-    isVoxelOccupiedAtPoint(const int& point_idx_arg) const
+    isVoxelOccupiedAtPoint(const index_t& point_idx_arg) const
 {
   // retrieve point from input cloud
   const PointT& point = (*this->input_)[point_idx_arg];
@@ -233,7 +234,7 @@ template <typename PointT,
           typename OctreeT>
 void
 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
-    deleteVoxelAtPoint(const int& point_idx_arg)
+    deleteVoxelAtPoint(const index_t& point_idx_arg)
 {
   // retrieve point from input cloud
   const PointT& point = (*this->input_)[point_idx_arg];
@@ -247,7 +248,7 @@ template <typename PointT,
           typename LeafContainerT,
           typename BranchContainerT,
           typename OctreeT>
-int
+pcl::uindex_t
 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
     getOccupiedVoxelCenters(AlignedPointTVector& voxel_center_list_arg) const
 {
@@ -264,7 +265,7 @@ template <typename PointT,
           typename LeafContainerT,
           typename BranchContainerT,
           typename OctreeT>
-int
+pcl::uindex_t
 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
     getApproxIntersectedVoxelCentersBySegment(const Eigen::Vector3f& origin,
                                               const Eigen::Vector3f& end,
@@ -277,14 +278,14 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
 
   const float step_size = static_cast<float>(resolution_) * precision;
   // Ensure we get at least one step for the first voxel.
-  const int nsteps = std::max(1, static_cast<int>(norm / step_size));
+  const auto nsteps = std::max<std::size_t>(1, norm / step_size);
 
   OctreeKey prev_key;
 
   bool bkeyDefined = false;
 
   // Walk along the line segment with small steps.
-  for (int i = 0; i < nsteps; ++i) {
+  for (std::size_t i = 0; i < nsteps; ++i) {
     Eigen::Vector3f p = origin + (direction * step_size * static_cast<float>(i));
 
     PointT octree_p;
@@ -319,7 +320,7 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
     voxel_center_list.push_back(center);
   }
 
-  return (static_cast<int>(voxel_center_list.size()));
+  return (static_cast<uindex_t>(voxel_center_list.size()));
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
@@ -604,7 +605,7 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
     expandLeafNode(LeafNode* leaf_node,
                    BranchNode* parent_branch,
                    unsigned char child_idx,
-                   unsigned int depth_mask)
+                   uindex_t depth_mask)
 {
 
   if (depth_mask) {
@@ -612,7 +613,7 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
     std::size_t leaf_obj_count = (*leaf_node)->getSize();
 
     // copy leaf data
-    std::vector<int> leafIndices;
+    Indices leafIndices;
     leafIndices.reserve(leaf_obj_count);
 
     (*leaf_node)->getPointIndices(leafIndices);
@@ -628,7 +629,7 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
     // add data to new branch
     OctreeKey new_index_key;
 
-    for (const int& leafIndex : leafIndices) {
+    for (const auto& leafIndex : leafIndices) {
 
       const PointT& point_from_index = (*input_)[leafIndex];
       // generate key
@@ -651,11 +652,11 @@ template <typename PointT,
           typename OctreeT>
 void
 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
-    addPointIdx(const int point_idx_arg)
+    addPointIdx(const uindex_t point_idx_arg)
 {
   OctreeKey key;
 
-  assert(point_idx_arg < static_cast<int>(input_->size()));
+  assert(point_idx_arg < input_->size());
 
   const PointT& point = (*input_)[point_idx_arg];
 
@@ -667,7 +668,7 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
 
   LeafNode* leaf_node;
   BranchNode* parent_branch_of_leaf_node;
-  unsigned int depth_mask = this->createLeafRecursive(
+  auto depth_mask = this->createLeafRecursive(
       key, this->depth_mask_, this->root_node_, leaf_node, parent_branch_of_leaf_node);
 
   if (this->dynamic_depth_enabled_ && depth_mask) {
@@ -699,10 +700,10 @@ template <typename PointT,
           typename OctreeT>
 const PointT&
 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
-    getPointByIndex(const unsigned int index_arg) const
+    getPointByIndex(const uindex_t index_arg) const
 {
   // retrieve point from input cloud
-  assert(index_arg < static_cast<unsigned int>(input_->size()));
+  assert(index_arg < input_->size());
   return ((*this->input_)[index_arg]);
 }
 
@@ -715,36 +716,28 @@ void
 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
     getKeyBitSize()
 {
-  unsigned int max_voxels;
-
-  unsigned int max_key_x;
-  unsigned int max_key_y;
-  unsigned int max_key_z;
-
-  double octree_side_len;
-
   const float minValue = std::numeric_limits<float>::epsilon();
 
   // find maximum key values for x, y, z
-  max_key_x =
-      static_cast<unsigned int>(std::ceil((max_x_ - min_x_ - minValue) / resolution_));
-  max_key_y =
-      static_cast<unsigned int>(std::ceil((max_y_ - min_y_ - minValue) / resolution_));
-  max_key_z =
-      static_cast<unsigned int>(std::ceil((max_z_ - min_z_ - minValue) / resolution_));
+  const auto max_key_x =
+      static_cast<uindex_t>(std::ceil((max_x_ - min_x_ - minValue) / resolution_));
+  const auto max_key_y =
+      static_cast<uindex_t>(std::ceil((max_y_ - min_y_ - minValue) / resolution_));
+  const auto max_key_z =
+      static_cast<uindex_t>(std::ceil((max_z_ - min_z_ - minValue) / resolution_));
 
   // find maximum amount of keys
-  max_voxels = std::max(std::max(std::max(max_key_x, max_key_y), max_key_z),
-                        static_cast<unsigned int>(2));
+  const auto max_voxels =
+      std::max<uindex_t>(std::max(std::max(max_key_x, max_key_y), max_key_z), 2);
 
   // tree depth == amount of bits of max_voxels
-  this->octree_depth_ =
-      std::max((std::min(static_cast<unsigned int>(OctreeKey::maxDepth),
-                         static_cast<unsigned int>(
-                             std::ceil(std::log2(max_voxels) - minValue)))),
-               static_cast<unsigned int>(0));
+  this->octree_depth_ = std::max<uindex_t>(
+      std::min<uindex_t>(OctreeKey::maxDepth,
+                         std::ceil(std::log2(max_voxels) - minValue)),
+      0);
 
-  octree_side_len = static_cast<double>(1 << this->octree_depth_) * resolution_;
+  const auto octree_side_len =
+      static_cast<double>(1 << this->octree_depth_) * resolution_;
 
   if (this->leaf_count_ == 0) {
     double octree_oversize_x;
@@ -792,12 +785,9 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
     genOctreeKeyforPoint(const PointT& point_arg, OctreeKey& key_arg) const
 {
   // calculate integer key for point coordinates
-  key_arg.x =
-      static_cast<unsigned int>((point_arg.x - this->min_x_) / this->resolution_);
-  key_arg.y =
-      static_cast<unsigned int>((point_arg.y - this->min_y_) / this->resolution_);
-  key_arg.z =
-      static_cast<unsigned int>((point_arg.z - this->min_z_) / this->resolution_);
+  key_arg.x = static_cast<uindex_t>((point_arg.x - this->min_x_) / this->resolution_);
+  key_arg.y = static_cast<uindex_t>((point_arg.y - this->min_y_) / this->resolution_);
+  key_arg.z = static_cast<uindex_t>((point_arg.z - this->min_z_) / this->resolution_);
 
   assert(key_arg.x <= this->max_key_.x);
   assert(key_arg.y <= this->max_key_.y);
@@ -833,7 +823,7 @@ template <typename PointT,
           typename OctreeT>
 bool
 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
-    genOctreeKeyForDataT(const int& data_arg, OctreeKey& key_arg) const
+    genOctreeKeyForDataT(const index_t& data_arg, OctreeKey& key_arg) const
 {
   const PointT temp_point = getPointByIndex(data_arg);
 
@@ -869,7 +859,7 @@ template <typename PointT,
 void
 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
     genVoxelCenterFromOctreeKey(const OctreeKey& key_arg,
-                                unsigned int tree_depth_arg,
+                                uindex_t tree_depth_arg,
                                 PointT& point_arg) const
 {
   // generate point for voxel center defined by treedepth (bitLen) and key
@@ -898,7 +888,7 @@ template <typename PointT,
 void
 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
     genVoxelBoundsFromOctreeKey(const OctreeKey& key_arg,
-                                unsigned int tree_depth_arg,
+                                uindex_t tree_depth_arg,
                                 Eigen::Vector3f& min_pt,
                                 Eigen::Vector3f& max_pt) const
 {
@@ -930,7 +920,7 @@ template <typename PointT,
           typename OctreeT>
 double
 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
-    getVoxelSquaredSideLen(unsigned int tree_depth_arg) const
+    getVoxelSquaredSideLen(uindex_t tree_depth_arg) const
 {
   double side_len;
 
@@ -951,7 +941,7 @@ template <typename PointT,
           typename OctreeT>
 double
 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
-    getVoxelSquaredDiameter(unsigned int tree_depth_arg) const
+    getVoxelSquaredDiameter(uindex_t tree_depth_arg) const
 {
   // return the squared side length of the voxel cube as a function of the octree depth
   return (getVoxelSquaredSideLen(tree_depth_arg) * 3);
@@ -962,13 +952,13 @@ template <typename PointT,
           typename LeafContainerT,
           typename BranchContainerT,
           typename OctreeT>
-int
+pcl::uindex_t
 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
     getOccupiedVoxelCentersRecursive(const BranchNode* node_arg,
                                      const OctreeKey& key_arg,
                                      AlignedPointTVector& voxel_center_list_arg) const
 {
-  int voxel_count = 0;
+  uindex_t voxel_count = 0;
 
   // iterate over all children
   for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
diff --git a/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp b/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp
index 77e83c51..57590745 100644
--- a/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp
+++ b/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp
@@ -37,7 +37,6 @@
 
 #pragma once
 
-#include <pcl/common/geometry.h>
 #include <pcl/common/point_tests.h> // for pcl::isFinite
 #include <pcl/console/print.h>
 
@@ -128,12 +127,9 @@ pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>
     if (pcl::isFinite(temp)) // Make sure transformed point is finite - if it is not, it
                              // gets default key
     {
-      key_arg.x =
-          static_cast<unsigned int>((temp.x - this->min_x_) / this->resolution_);
-      key_arg.y =
-          static_cast<unsigned int>((temp.y - this->min_y_) / this->resolution_);
-      key_arg.z =
-          static_cast<unsigned int>((temp.z - this->min_z_) / this->resolution_);
+      key_arg.x = static_cast<uindex_t>((temp.x - this->min_x_) / this->resolution_);
+      key_arg.y = static_cast<uindex_t>((temp.y - this->min_y_) / this->resolution_);
+      key_arg.z = static_cast<uindex_t>((temp.z - this->min_z_) / this->resolution_);
     }
     else {
       key_arg = OctreeKey();
@@ -141,12 +137,9 @@ pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>
   }
   else {
     // calculate integer key for point coordinates
-    key_arg.x =
-        static_cast<unsigned int>((point_arg.x - this->min_x_) / this->resolution_);
-    key_arg.y =
-        static_cast<unsigned int>((point_arg.y - this->min_y_) / this->resolution_);
-    key_arg.z =
-        static_cast<unsigned int>((point_arg.z - this->min_z_) / this->resolution_);
+    key_arg.x = static_cast<uindex_t>((point_arg.x - this->min_x_) / this->resolution_);
+    key_arg.y = static_cast<uindex_t>((point_arg.y - this->min_y_) / this->resolution_);
+    key_arg.z = static_cast<uindex_t>((point_arg.z - this->min_z_) / this->resolution_);
   }
 }
 
@@ -154,11 +147,11 @@ pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>
 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
 void
 pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::
-    addPointIdx(const int pointIdx_arg)
+    addPointIdx(const uindex_t pointIdx_arg)
 {
   OctreeKey key;
 
-  assert(pointIdx_arg < static_cast<int>(this->input_->size()));
+  assert(pointIdx_arg < this->input_->size());
 
   const PointT& point = (*this->input_)[pointIdx_arg];
   if (!pcl::isFinite(point))
@@ -295,13 +288,13 @@ pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>
   direction.normalize();
   float precision = 1.0f;
   const float step_size = static_cast<const float>(resolution_) * precision;
-  const int nsteps = std::max(1, static_cast<int>(norm / step_size));
+  const auto nsteps = std::max<std::size_t>(1, norm / step_size);
 
   OctreeKey prev_key = key;
   // Walk along the line segment with small steps.
   Eigen::Vector3f p = leaf_centroid;
   PointT octree_p;
-  for (int i = 0; i < nsteps; ++i) {
+  for (std::size_t i = 0; i < nsteps; ++i) {
     // Start at the leaf voxel, and move back towards sensor.
     p += (direction * step_size);
 
diff --git a/octree/include/pcl/octree/impl/octree_pointcloud_voxelcentroid.hpp b/octree/include/pcl/octree/impl/octree_pointcloud_voxelcentroid.hpp
index ed3f132f..f5e6e889 100644
--- a/octree/include/pcl/octree/impl/octree_pointcloud_voxelcentroid.hpp
+++ b/octree/include/pcl/octree/impl/octree_pointcloud_voxelcentroid.hpp
@@ -70,7 +70,7 @@ pcl::octree::OctreePointCloudVoxelCentroid<PointT, LeafContainerT, BranchContain
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
-std::size_t
+pcl::uindex_t
 pcl::octree::OctreePointCloudVoxelCentroid<PointT, LeafContainerT, BranchContainerT>::
     getVoxelCentroids(
         typename OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::
diff --git a/octree/include/pcl/octree/impl/octree_search.hpp b/octree/include/pcl/octree/impl/octree_search.hpp
index 16b98ce0..548b97fc 100644
--- a/octree/include/pcl/octree/impl/octree_search.hpp
+++ b/octree/include/pcl/octree/impl/octree_search.hpp
@@ -48,7 +48,7 @@ namespace octree {
 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
 bool
 OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::voxelSearch(
-    const PointT& point, std::vector<int>& point_idx_data)
+    const PointT& point, Indices& point_idx_data)
 {
   assert(isFinite(point) &&
          "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
@@ -71,18 +71,18 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::voxelSearch(
 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
 bool
 OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::voxelSearch(
-    const int index, std::vector<int>& point_idx_data)
+    const uindex_t index, Indices& point_idx_data)
 {
   const PointT search_point = this->getPointByIndex(index);
   return (this->voxelSearch(search_point, point_idx_data));
 }
 
 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
-int
+uindex_t
 OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::nearestKSearch(
     const PointT& p_q,
-    int k,
-    std::vector<int>& k_indices,
+    uindex_t k,
+    Indices& k_indices,
     std::vector<float>& k_sqr_distances)
 {
   assert(this->leaf_count_ > 0);
@@ -107,23 +107,23 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::nearestKSearch
   getKNearestNeighborRecursive(
       p_q, k, this->root_node_, key, 1, smallest_dist, point_candidates);
 
-  unsigned int result_count = static_cast<unsigned int>(point_candidates.size());
+  const auto result_count = static_cast<uindex_t>(point_candidates.size());
 
   k_indices.resize(result_count);
   k_sqr_distances.resize(result_count);
 
-  for (unsigned int i = 0; i < result_count; ++i) {
+  for (uindex_t i = 0; i < result_count; ++i) {
     k_indices[i] = point_candidates[i].point_idx_;
     k_sqr_distances[i] = point_candidates[i].point_distance_;
   }
 
-  return static_cast<int>(k_indices.size());
+  return k_indices.size();
 }
 
 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
-int
+uindex_t
 OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::nearestKSearch(
-    int index, int k, std::vector<int>& k_indices, std::vector<float>& k_sqr_distances)
+    uindex_t index, uindex_t k, Indices& k_indices, std::vector<float>& k_sqr_distances)
 {
   const PointT search_point = this->getPointByIndex(index);
   return (nearestKSearch(search_point, k, k_indices, k_sqr_distances));
@@ -132,7 +132,7 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::nearestKSearch
 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
 void
 OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::approxNearestSearch(
-    const PointT& p_q, int& result_index, float& sqr_distance)
+    const PointT& p_q, index_t& result_index, float& sqr_distance)
 {
   assert(this->leaf_count_ > 0);
   assert(isFinite(p_q) &&
@@ -150,7 +150,7 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::approxNearestS
 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
 void
 OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::approxNearestSearch(
-    int query_index, int& result_index, float& sqr_distance)
+    uindex_t query_index, index_t& result_index, float& sqr_distance)
 {
   const PointT search_point = this->getPointByIndex(query_index);
 
@@ -158,13 +158,13 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::approxNearestS
 }
 
 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
-int
+uindex_t
 OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::radiusSearch(
     const PointT& p_q,
     const double radius,
-    std::vector<int>& k_indices,
+    Indices& k_indices,
     std::vector<float>& k_sqr_distances,
-    unsigned int max_nn) const
+    uindex_t max_nn) const
 {
   assert(isFinite(p_q) &&
          "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
@@ -183,17 +183,17 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::radiusSearch(
                                     k_sqr_distances,
                                     max_nn);
 
-  return (static_cast<int>(k_indices.size()));
+  return k_indices.size();
 }
 
 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
-int
+uindex_t
 OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::radiusSearch(
-    int index,
+    uindex_t index,
     const double radius,
-    std::vector<int>& k_indices,
+    Indices& k_indices,
     std::vector<float>& k_sqr_distances,
-    unsigned int max_nn) const
+    uindex_t max_nn) const
 {
   const PointT search_point = this->getPointByIndex(index);
 
@@ -201,11 +201,11 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::radiusSearch(
 }
 
 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
-int
+uindex_t
 OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::boxSearch(
     const Eigen::Vector3f& min_pt,
     const Eigen::Vector3f& max_pt,
-    std::vector<int>& k_indices) const
+    Indices& k_indices) const
 {
 
   OctreeKey key;
@@ -215,7 +215,7 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::boxSearch(
 
   boxSearchRecursive(min_pt, max_pt, this->root_node_, key, 1, k_indices);
 
-  return (static_cast<int>(k_indices.size()));
+  return k_indices.size();
 }
 
 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
@@ -223,10 +223,10 @@ double
 OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
     getKNearestNeighborRecursive(
         const PointT& point,
-        unsigned int K,
+        uindex_t K,
         const BranchNode* node,
         const OctreeKey& key,
-        unsigned int tree_depth,
+        uindex_t tree_depth,
         const double squared_search_radius,
         std::vector<prioPointQueueEntry>& point_candidates) const
 {
@@ -277,7 +277,7 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
     child_node = search_heap.back().node;
     new_key = search_heap.back().key;
 
-    if (tree_depth < this->octree_depth_) {
+    if (child_node->getNodeType() == BRANCH_NODE) {
       // we have not reached maximum tree depth
       smallest_squared_dist =
           getKNearestNeighborRecursive(point,
@@ -290,7 +290,7 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
     }
     else {
       // we reached leaf node level
-      std::vector<int> decoded_point_vector;
+      Indices decoded_point_vector;
 
       const LeafNode* child_leaf = static_cast<const LeafNode*>(child_node);
 
@@ -298,7 +298,7 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
       (*child_leaf)->getPointIndices(decoded_point_vector);
 
       // Linearly iterate over all decoded (unsorted) points
-      for (const int& point_index : decoded_point_vector) {
+      for (const auto& point_index : decoded_point_vector) {
 
         const PointT& candidate_point = this->getPointByIndex(point_index);
 
@@ -337,10 +337,10 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
                                       const double radiusSquared,
                                       const BranchNode* node,
                                       const OctreeKey& key,
-                                      unsigned int tree_depth,
-                                      std::vector<int>& k_indices,
+                                      uindex_t tree_depth,
+                                      Indices& k_indices,
                                       std::vector<float>& k_sqr_distances,
-                                      unsigned int max_nn) const
+                                      uindex_t max_nn) const
 {
   // get spatial voxel information
   double voxel_squared_diameter = this->getVoxelSquaredDiameter(tree_depth);
@@ -373,7 +373,7 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
         voxel_squared_diameter / 4.0 + radiusSquared +
             sqrt(voxel_squared_diameter * radiusSquared)) {
 
-      if (tree_depth < this->octree_depth_) {
+      if (child_node->getNodeType() == BRANCH_NODE) {
         // we have not reached maximum tree depth
         getNeighborsWithinRadiusRecursive(point,
                                           radiusSquared,
@@ -383,19 +383,19 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
                                           k_indices,
                                           k_sqr_distances,
                                           max_nn);
-        if (max_nn != 0 && k_indices.size() == static_cast<unsigned int>(max_nn))
+        if (max_nn != 0 && k_indices.size() == max_nn)
           return;
       }
       else {
         // we reached leaf node level
         const LeafNode* child_leaf = static_cast<const LeafNode*>(child_node);
-        std::vector<int> decoded_point_vector;
+        Indices decoded_point_vector;
 
         // decode leaf node into decoded_point_vector
         (*child_leaf)->getPointIndices(decoded_point_vector);
 
         // Linearly iterate over all decoded (unsorted) points
-        for (const int& index : decoded_point_vector) {
+        for (const auto& index : decoded_point_vector) {
           const PointT& candidate_point = this->getPointByIndex(index);
 
           // calculate point distance to search point
@@ -409,7 +409,7 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
           k_indices.push_back(index);
           k_sqr_distances.push_back(squared_dist);
 
-          if (max_nn != 0 && k_indices.size() == static_cast<unsigned int>(max_nn))
+          if (max_nn != 0 && k_indices.size() == max_nn)
             return;
         }
       }
@@ -423,8 +423,8 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
     approxNearestSearchRecursive(const PointT& point,
                                  const BranchNode* node,
                                  const OctreeKey& key,
-                                 unsigned int tree_depth,
-                                 int& result_index,
+                                 uindex_t tree_depth,
+                                 index_t& result_index,
                                  float& sqr_distance)
 {
   OctreeKey minChildKey;
@@ -468,7 +468,7 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
 
   child_node = this->getBranchChildPtr(*node, min_child_idx);
 
-  if (tree_depth < this->octree_depth_) {
+  if (child_node->getNodeType() == BRANCH_NODE) {
     // we have not reached maximum tree depth
     approxNearestSearchRecursive(point,
                                  static_cast<const BranchNode*>(child_node),
@@ -479,21 +479,21 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
   }
   else {
     // we reached leaf node level
-    std::vector<int> decoded_point_vector;
+    Indices decoded_point_vector;
 
     const LeafNode* child_leaf = static_cast<const LeafNode*>(child_node);
 
-    double smallest_squared_dist = std::numeric_limits<double>::max();
+    float smallest_squared_dist = std::numeric_limits<float>::max();
 
     // decode leaf node into decoded_point_vector
     (**child_leaf).getPointIndices(decoded_point_vector);
 
     // Linearly iterate over all decoded (unsorted) points
-    for (const int& index : decoded_point_vector) {
+    for (const auto& index : decoded_point_vector) {
       const PointT& candidate_point = this->getPointByIndex(index);
 
       // calculate point distance to search point
-      double squared_dist = pointSquaredDist(candidate_point, point);
+      float squared_dist = pointSquaredDist(candidate_point, point);
 
       // check if a closer match is found
       if (squared_dist >= smallest_squared_dist)
@@ -501,7 +501,7 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
 
       result_index = index;
       smallest_squared_dist = squared_dist;
-      sqr_distance = static_cast<float>(squared_dist);
+      sqr_distance = squared_dist;
     }
   }
 }
@@ -521,8 +521,8 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::boxSearchRecur
     const Eigen::Vector3f& max_pt,
     const BranchNode* node,
     const OctreeKey& key,
-    unsigned int tree_depth,
-    std::vector<int>& k_indices) const
+    uindex_t tree_depth,
+    Indices& k_indices) const
 {
   // iterate over all children
   for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
@@ -552,7 +552,7 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::boxSearchRecur
           (lower_voxel_corner(1) > max_pt(1)) || (min_pt(1) > upper_voxel_corner(1)) ||
           (lower_voxel_corner(2) > max_pt(2)) || (min_pt(2) > upper_voxel_corner(2)))) {
 
-      if (tree_depth < this->octree_depth_) {
+      if (child_node->getNodeType() == BRANCH_NODE) {
         // we have not reached maximum tree depth
         boxSearchRecursive(min_pt,
                            max_pt,
@@ -563,7 +563,7 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::boxSearchRecur
       }
       else {
         // we reached leaf node level
-        std::vector<int> decoded_point_vector;
+        Indices decoded_point_vector;
 
         const LeafNode* child_leaf = static_cast<const LeafNode*>(child_node);
 
@@ -571,7 +571,7 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::boxSearchRecur
         (**child_leaf).getPointIndices(decoded_point_vector);
 
         // Linearly iterate over all decoded (unsorted) points
-        for (const int& index : decoded_point_vector) {
+        for (const auto& index : decoded_point_vector) {
           const PointT& candidate_point = this->getPointByIndex(index);
 
           // check if point falls within search box
@@ -590,12 +590,12 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::boxSearchRecur
 }
 
 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
-int
+uindex_t
 OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
     getIntersectedVoxelCenters(Eigen::Vector3f origin,
                                Eigen::Vector3f direction,
                                AlignedPointTVector& voxel_center_list,
-                               int max_voxel_count) const
+                               uindex_t max_voxel_count) const
 {
   OctreeKey key;
   key.x = key.y = key.z = 0;
@@ -626,12 +626,12 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
 }
 
 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
-int
+uindex_t
 OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
     getIntersectedVoxelIndices(Eigen::Vector3f origin,
                                Eigen::Vector3f direction,
-                               std::vector<int>& k_indices,
-                               int max_voxel_count) const
+                               Indices& k_indices,
+                               uindex_t max_voxel_count) const
 {
   OctreeKey key;
   key.x = key.y = key.z = 0;
@@ -660,7 +660,7 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
 }
 
 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
-int
+uindex_t
 OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
     getIntersectedVoxelCentersRecursive(double min_x,
                                         double min_y,
@@ -672,7 +672,7 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
                                         const OctreeNode* node,
                                         const OctreeKey& key,
                                         AlignedPointTVector& voxel_center_list,
-                                        int max_voxel_count) const
+                                        uindex_t max_voxel_count) const
 {
   if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
     return (0);
@@ -689,7 +689,7 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
   }
 
   // Voxel intersection count for branches children
-  int voxel_count = 0;
+  uindex_t voxel_count = 0;
 
   // Voxel mid lines
   double mid_x = 0.5 * (min_x + max_x);
@@ -697,7 +697,7 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
   double mid_z = 0.5 * (min_z + max_z);
 
   // First voxel node ray will intersect
-  int curr_node = getFirstIntersectedNode(min_x, min_y, min_z, mid_x, mid_y, mid_z);
+  auto curr_node = getFirstIntersectedNode(min_x, min_y, min_z, mid_x, mid_y, mid_z);
 
   // Child index, node and key
   unsigned char child_idx;
@@ -856,7 +856,7 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
 }
 
 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
-int
+uindex_t
 OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
     getIntersectedVoxelIndicesRecursive(double min_x,
                                         double min_y,
@@ -867,8 +867,8 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
                                         unsigned char a,
                                         const OctreeNode* node,
                                         const OctreeKey& key,
-                                        std::vector<int>& k_indices,
-                                        int max_voxel_count) const
+                                        Indices& k_indices,
+                                        uindex_t max_voxel_count) const
 {
   if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
     return (0);
@@ -884,7 +884,7 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
   }
 
   // Voxel intersection count for branches children
-  int voxel_count = 0;
+  uindex_t voxel_count = 0;
 
   // Voxel mid lines
   double mid_x = 0.5 * (min_x + max_x);
@@ -892,7 +892,7 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
   double mid_z = 0.5 * (min_z + max_z);
 
   // First voxel node ray will intersect
-  int curr_node = getFirstIntersectedNode(min_x, min_y, min_z, mid_x, mid_y, mid_z);
+  auto curr_node = getFirstIntersectedNode(min_x, min_y, min_z, mid_x, mid_y, mid_z);
 
   // Child index, node and key
   unsigned char child_idx;
diff --git a/octree/include/pcl/octree/octree2buf_base.h b/octree/include/pcl/octree/octree2buf_base.h
index 6d9e70ee..6554e705 100644
--- a/octree/include/pcl/octree/octree2buf_base.h
+++ b/octree/include/pcl/octree/octree2buf_base.h
@@ -212,7 +212,7 @@ protected:
  * \ingroup octree
  * \author Julius Kammerl (julius@kammerl.de)
  */
-template <typename LeafContainerT = int,
+template <typename LeafContainerT = index_t,
           typename BranchContainerT = OctreeContainerEmpty>
 class Octree2BufBase {
 
@@ -236,7 +236,7 @@ public:
   using Iterator = OctreeDepthFirstIterator<OctreeT>;
   using ConstIterator = const OctreeDepthFirstIterator<OctreeT>;
   Iterator
-  begin(unsigned int max_depth_arg = 0)
+  begin(uindex_t max_depth_arg = 0)
   {
     return Iterator(this, max_depth_arg);
   };
@@ -253,26 +253,12 @@ public:
   using LeafNodeIterator = OctreeLeafNodeDepthFirstIterator<OctreeT>;
   using ConstLeafNodeIterator = const OctreeLeafNodeDepthFirstIterator<OctreeT>;
 
-  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(1, 12, "use leaf_depth_end() instead")
-  const LeafNodeIterator
-  leaf_end()
-  {
-    return LeafNodeIterator();
-  };
-
   // The currently valide names
   using LeafNodeDepthFirstIterator = OctreeLeafNodeDepthFirstIterator<OctreeT>;
   using ConstLeafNodeDepthFirstIterator =
       const OctreeLeafNodeDepthFirstIterator<OctreeT>;
   LeafNodeDepthFirstIterator
-  leaf_depth_begin(unsigned int max_depth_arg = 0)
+  leaf_depth_begin(uindex_t max_depth_arg = 0)
   {
     return LeafNodeDepthFirstIterator(this, max_depth_arg);
   };
@@ -287,7 +273,7 @@ public:
   using DepthFirstIterator = OctreeDepthFirstIterator<OctreeT>;
   using ConstDepthFirstIterator = const OctreeDepthFirstIterator<OctreeT>;
   DepthFirstIterator
-  depth_begin(unsigned int maxDepth_arg = 0)
+  depth_begin(uindex_t maxDepth_arg = 0)
   {
     return DepthFirstIterator(this, maxDepth_arg);
   };
@@ -301,7 +287,7 @@ public:
   using BreadthFirstIterator = OctreeBreadthFirstIterator<OctreeT>;
   using ConstBreadthFirstIterator = const OctreeBreadthFirstIterator<OctreeT>;
   BreadthFirstIterator
-  breadth_begin(unsigned int max_depth_arg = 0)
+  breadth_begin(uindex_t max_depth_arg = 0)
   {
     return BreadthFirstIterator(this, max_depth_arg);
   };
@@ -317,7 +303,7 @@ public:
       const OctreeLeafNodeBreadthFirstIterator<OctreeT>;
 
   LeafNodeBreadthIterator
-  leaf_breadth_begin(unsigned int max_depth_arg = 0u)
+  leaf_breadth_begin(uindex_t max_depth_arg = 0u)
   {
     return LeafNodeBreadthIterator(this,
                                    max_depth_arg ? max_depth_arg : this->octree_depth_);
@@ -368,18 +354,18 @@ public:
    *  \param max_voxel_index_arg: maximum amount of voxels per dimension
    */
   void
-  setMaxVoxelIndex(unsigned int max_voxel_index_arg);
+  setMaxVoxelIndex(uindex_t max_voxel_index_arg);
 
   /** \brief Set the maximum depth of the octree.
    *  \param depth_arg: maximum depth of octree
    */
   void
-  setTreeDepth(unsigned int depth_arg);
+  setTreeDepth(uindex_t depth_arg);
 
   /** \brief Get the maximum depth of the octree.
    *  \return depth_arg: maximum depth of octree
    */
-  inline unsigned int
+  inline uindex_t
   getTreeDepth() const
   {
     return this->octree_depth_;
@@ -393,7 +379,7 @@ public:
    *  \return pointer to new leaf node container.
    */
   LeafContainerT*
-  createLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);
+  createLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg);
 
   /** \brief Find leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
    *  \note If leaf node already exist, this method returns the existing node
@@ -403,7 +389,7 @@ public:
    *  \return pointer to leaf node container if found, null pointer otherwise.
    */
   LeafContainerT*
-  findLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);
+  findLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg);
 
   /** \brief Check for the existence of leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
    *  \param idx_x_arg: index of leaf node in the X axis.
@@ -412,9 +398,7 @@ public:
    *  \return "true" if leaf node search is successful, otherwise it returns "false".
    */
   bool
-  existLeaf(unsigned int idx_x_arg,
-            unsigned int idx_y_arg,
-            unsigned int idx_z_arg) const;
+  existLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg) const;
 
   /** \brief Remove leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
    *  \param idx_x_arg: index of leaf node in the X axis.
@@ -422,7 +406,7 @@ public:
    *  \param idx_z_arg: index of leaf node in the Z axis.
    */
   void
-  removeLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);
+  removeLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg);
 
   /** \brief Return the amount of existing leafs in the octree.
    *  \return amount of registered leaf nodes.
@@ -839,9 +823,9 @@ protected:
    * \param branch_reset_arg: Reset pointer array of current branch
    * \return depth mask at which leaf node was created/found
    **/
-  unsigned int
+  uindex_t
   createLeafRecursive(const OctreeKey& key_arg,
-                      unsigned int depth_mask_arg,
+                      uindex_t depth_mask_arg,
                       BranchNode* branch_arg,
                       LeafNode*& return_leaf_arg,
                       BranchNode*& parent_of_leaf_arg,
@@ -857,7 +841,7 @@ protected:
    **/
   void
   findLeafRecursive(const OctreeKey& key_arg,
-                    unsigned int depth_mask_arg,
+                    uindex_t depth_mask_arg,
                     BranchNode* branch_arg,
                     LeafContainerT*& result_arg) const;
 
@@ -871,7 +855,7 @@ protected:
    **/
   bool
   deleteLeafRecursive(const OctreeKey& key_arg,
-                      unsigned int depth_mask_arg,
+                      uindex_t depth_mask_arg,
                       BranchNode* branch_arg);
 
   /** \brief Recursively explore the octree and output binary octree description
@@ -915,7 +899,7 @@ protected:
   void
   deserializeTreeRecursive(
       BranchNode* branch_arg,
-      unsigned int depth_mask_arg,
+      uindex_t depth_mask_arg,
       OctreeKey& key_arg,
       typename std::vector<char>::const_iterator& binary_tree_in_it_arg,
       typename std::vector<char>::const_iterator& binary_tree_in_it_end_arg,
@@ -952,15 +936,6 @@ protected:
   void
   treeCleanUpRecursive(BranchNode* branch_arg);
 
-  /** \brief Helper function to calculate the binary logarithm
-   * \param n_arg: some value
-   * \return binary logarithm (log2) of argument n_arg
-   */
-  PCL_DEPRECATED(1, 12, "use std::log2 instead") inline double Log2(double n_arg)
-  {
-    return std::log2(n_arg);
-  }
-
   /** \brief Test if octree is able to dynamically change its depth. This is required
    * for adaptive bounding box adjustment.
    * \return "false" - not resizeable due to XOR serialization
@@ -1001,7 +976,7 @@ protected:
   BranchNode* root_node_;
 
   /** \brief Depth mask based on octree depth   **/
-  unsigned int depth_mask_;
+  uindex_t depth_mask_;
 
   /** \brief key range */
   OctreeKey max_key_;
@@ -1009,11 +984,12 @@ protected:
   /** \brief Currently active octree buffer  **/
   unsigned char buffer_selector_;
 
-  // flags indicating if unused branches and leafs might exist in previous buffer
+  /** \brief flags indicating if unused branches and leafs might exist in previous
+   * buffer  **/
   bool tree_dirty_flag_;
 
   /** \brief Octree depth */
-  unsigned int octree_depth_;
+  uindex_t octree_depth_;
 
   /** \brief Enable dynamic_depth
    *  \note Note that this parameter is ignored in octree2buf! */
diff --git a/octree/include/pcl/octree/octree_base.h b/octree/include/pcl/octree/octree_base.h
index c487b924..5201f64d 100644
--- a/octree/include/pcl/octree/octree_base.h
+++ b/octree/include/pcl/octree/octree_base.h
@@ -57,7 +57,7 @@ namespace octree {
  * \ingroup octree
  * \author Julius Kammerl (julius@kammerl.de)
  */
-template <typename LeafContainerT = int,
+template <typename LeafContainerT = index_t,
           typename BranchContainerT = OctreeContainerEmpty>
 class OctreeBase {
 public:
@@ -84,10 +84,10 @@ protected:
   BranchNode* root_node_;
 
   /** \brief Depth mask based on octree depth   **/
-  unsigned int depth_mask_;
+  uindex_t depth_mask_;
 
   /** \brief Octree depth */
-  unsigned int octree_depth_;
+  uindex_t octree_depth_;
 
   /** \brief Enable dynamic_depth **/
   bool dynamic_depth_enabled_;
@@ -109,7 +109,7 @@ public:
   using ConstIterator = const OctreeDepthFirstIterator<OctreeT>;
 
   Iterator
-  begin(unsigned int max_depth_arg = 0u)
+  begin(uindex_t max_depth_arg = 0u)
   {
     return Iterator(this, max_depth_arg ? max_depth_arg : this->octree_depth_);
   };
@@ -127,27 +127,13 @@ public:
   using LeafNodeIterator = OctreeLeafNodeDepthFirstIterator<OctreeT>;
   using ConstLeafNodeIterator = const OctreeLeafNodeDepthFirstIterator<OctreeT>;
 
-  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(1, 12, "use leaf_depth_end() instead")
-  const LeafNodeIterator
-  leaf_end()
-  {
-    return LeafNodeIterator(this, 0, nullptr);
-  };
-
   // The currently valide names
   using LeafNodeDepthFirstIterator = OctreeLeafNodeDepthFirstIterator<OctreeT>;
   using ConstLeafNodeDepthFirstIterator =
       const OctreeLeafNodeDepthFirstIterator<OctreeT>;
 
   LeafNodeDepthFirstIterator
-  leaf_depth_begin(unsigned int max_depth_arg = 0u)
+  leaf_depth_begin(uindex_t max_depth_arg = 0u)
   {
     return LeafNodeDepthFirstIterator(
         this, max_depth_arg ? max_depth_arg : this->octree_depth_);
@@ -164,7 +150,7 @@ public:
   using ConstDepthFirstIterator = const OctreeDepthFirstIterator<OctreeT>;
 
   DepthFirstIterator
-  depth_begin(unsigned int max_depth_arg = 0u)
+  depth_begin(uindex_t max_depth_arg = 0u)
   {
     return DepthFirstIterator(this,
                               max_depth_arg ? max_depth_arg : this->octree_depth_);
@@ -181,7 +167,7 @@ public:
   using ConstBreadthFirstIterator = const OctreeBreadthFirstIterator<OctreeT>;
 
   BreadthFirstIterator
-  breadth_begin(unsigned int max_depth_arg = 0u)
+  breadth_begin(uindex_t max_depth_arg = 0u)
   {
     return BreadthFirstIterator(this,
                                 max_depth_arg ? max_depth_arg : this->octree_depth_);
@@ -198,7 +184,7 @@ public:
   using ConstFixedDepthIterator = const OctreeFixedDepthIterator<OctreeT>;
 
   FixedDepthIterator
-  fixed_depth_begin(unsigned int fixed_depth_arg = 0u)
+  fixed_depth_begin(uindex_t fixed_depth_arg = 0u)
   {
     return FixedDepthIterator(this, fixed_depth_arg);
   };
@@ -215,7 +201,7 @@ public:
       const OctreeLeafNodeBreadthFirstIterator<OctreeT>;
 
   LeafNodeBreadthFirstIterator
-  leaf_breadth_begin(unsigned int max_depth_arg = 0u)
+  leaf_breadth_begin(uindex_t max_depth_arg = 0u)
   {
     return LeafNodeBreadthFirstIterator(
         this, max_depth_arg ? max_depth_arg : this->octree_depth_);
@@ -263,18 +249,18 @@ public:
    * \param[in] max_voxel_index_arg maximum amount of voxels per dimension
    */
   void
-  setMaxVoxelIndex(unsigned int max_voxel_index_arg);
+  setMaxVoxelIndex(uindex_t max_voxel_index_arg);
 
   /** \brief Set the maximum depth of the octree.
    *  \param max_depth_arg: maximum depth of octree
    */
   void
-  setTreeDepth(unsigned int max_depth_arg);
+  setTreeDepth(uindex_t max_depth_arg);
 
   /** \brief Get the maximum depth of the octree.
    *  \return depth_arg: maximum depth of octree
    */
-  unsigned int
+  uindex_t
   getTreeDepth() const
   {
     return this->octree_depth_;
@@ -288,7 +274,7 @@ public:
    *  \return pointer to new leaf node container.
    */
   LeafContainerT*
-  createLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);
+  createLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg);
 
   /** \brief Find leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
    *  \note If leaf node already exist, this method returns the existing node
@@ -298,7 +284,7 @@ public:
    *  \return pointer to leaf node container if found, null pointer otherwise.
    */
   LeafContainerT*
-  findLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);
+  findLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg);
 
   /** \brief idx_x_arg for the existence of leaf node at (idx_x_arg, idx_y_arg,
    * idx_z_arg).
@@ -308,9 +294,7 @@ public:
    * \return "true" if leaf node search is successful, otherwise it returns "false".
    */
   bool
-  existLeaf(unsigned int idx_x_arg,
-            unsigned int idx_y_arg,
-            unsigned int idx_z_arg) const;
+  existLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg) const;
 
   /** \brief Remove leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
    *  \param idx_x_arg: index of leaf node in the X axis.
@@ -318,7 +302,7 @@ public:
    *  \param idx_z_arg: index of leaf node in the Z axis.
    */
   void
-  removeLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);
+  removeLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg);
 
   /** \brief Return the amount of existing leafs in the octree.
    *  \return amount of registered leaf nodes.
@@ -594,9 +578,9 @@ protected:
    * \param parent_of_leaf_arg: return pointer to parent of leaf node
    * \return depth mask at which leaf node was created
    **/
-  unsigned int
+  uindex_t
   createLeafRecursive(const OctreeKey& key_arg,
-                      unsigned int depth_mask_arg,
+                      uindex_t depth_mask_arg,
                       BranchNode* branch_arg,
                       LeafNode*& return_leaf_arg,
                       BranchNode*& parent_of_leaf_arg);
@@ -611,7 +595,7 @@ protected:
    **/
   void
   findLeafRecursive(const OctreeKey& key_arg,
-                    unsigned int depth_mask_arg,
+                    uindex_t depth_mask_arg,
                     BranchNode* branch_arg,
                     LeafContainerT*& result_arg) const;
 
@@ -625,7 +609,7 @@ protected:
    **/
   bool
   deleteLeafRecursive(const OctreeKey& key_arg,
-                      unsigned int depth_mask_arg,
+                      uindex_t depth_mask_arg,
                       BranchNode* branch_arg);
 
   /** \brief Recursively explore the octree and output binary octree description
@@ -658,7 +642,7 @@ protected:
   void
   deserializeTreeRecursive(
       BranchNode* branch_arg,
-      unsigned int depth_mask_arg,
+      uindex_t depth_mask_arg,
       OctreeKey& key_arg,
       typename std::vector<char>::const_iterator& binary_tree_input_it_arg,
       typename std::vector<char>::const_iterator& binary_tree_input_it_end_arg,
@@ -687,15 +671,6 @@ protected:
   // Helpers
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
-  /** \brief Helper function to calculate the binary logarithm
-   * \param n_arg: some value
-   * \return binary logarithm (log2) of argument n_arg
-   */
-  PCL_DEPRECATED(1, 12, "use std::log2 instead") double Log2(double n_arg)
-  {
-    return std::log2(n_arg);
-  }
-
   /** \brief Test if octree is able to dynamically change its depth. This is required
    *for adaptive bounding box adjustment.
    * \return "true"
diff --git a/octree/include/pcl/octree/octree_container.h b/octree/include/pcl/octree/octree_container.h
index c4655b7c..f520cbcd 100644
--- a/octree/include/pcl/octree/octree_container.h
+++ b/octree/include/pcl/octree/octree_container.h
@@ -38,6 +38,8 @@
 
 #pragma once
 
+#include <pcl/types.h>
+
 #include <cassert>
 #include <cstddef>
 #include <vector>
@@ -74,7 +76,7 @@ public:
   /** \brief Pure abstract method to get size of container (number of indices)
    * \return number of points/indices stored in leaf node container.
    */
-  virtual std::size_t
+  virtual uindex_t
   getSize() const
   {
     return 0u;
@@ -88,21 +90,21 @@ public:
    * indices.
    */
   void
-  addPointIndex(const int&)
+  addPointIndex(const index_t&)
   {}
 
   /** \brief Empty getPointIndex implementation as this leaf node does not store any
    * point indices.
    */
   void
-  getPointIndex(int&) const
+  getPointIndex(index_t&) const
   {}
 
   /** \brief Empty getPointIndices implementation as this leaf node does not store any
    * data. \
    */
   void
-  getPointIndices(std::vector<int>&) const
+  getPointIndices(Indices&) const
   {}
 };
 
@@ -125,7 +127,7 @@ public:
   /** \brief Abstract get size of container (number of DataT objects)
    * \return number of DataT elements in leaf node container.
    */
-  std::size_t
+  uindex_t
   getSize() const override
   {
     return 0;
@@ -139,14 +141,12 @@ public:
   /** \brief Empty addPointIndex implementation. This leaf node does not store any point
    * indices.
    */
-  void
-  addPointIndex(int)
-  {}
+  void addPointIndex(index_t) {}
 
   /** \brief Empty getPointIndex implementation as this leaf node does not store any
    * point indices.
    */
-  int
+  index_t
   getPointIndex() const
   {
     assert("getPointIndex: undefined point index");
@@ -157,7 +157,7 @@ public:
    * data.
    */
   void
-  getPointIndices(std::vector<int>&) const
+  getPointIndices(Indices&) const
   {}
 };
 
@@ -195,7 +195,7 @@ public:
    * \param[in] data_arg index to be stored within leaf node.
    */
   void
-  addPointIndex(int data_arg)
+  addPointIndex(index_t data_arg)
   {
     data_ = data_arg;
   }
@@ -204,7 +204,7 @@ public:
    * point index
    * \return index stored within container.
    */
-  int
+  index_t
   getPointIndex() const
   {
     return data_;
@@ -216,31 +216,31 @@ public:
    * data vector
    */
   void
-  getPointIndices(std::vector<int>& data_vector_arg) const
+  getPointIndices(Indices& data_vector_arg) const
   {
-    if (data_ >= 0)
+    if (data_ != static_cast<index_t>(-1))
       data_vector_arg.push_back(data_);
   }
 
   /** \brief Get size of container (number of DataT objects)
    * \return number of DataT elements in leaf node container.
    */
-  std::size_t
+  uindex_t
   getSize() const override
   {
-    return data_ < 0 ? 0 : 1;
+    return data_ != static_cast<index_t>(-1) ? 0 : 1;
   }
 
   /** \brief Reset leaf node memory to zero. */
   void
   reset() override
   {
-    data_ = -1;
+    data_ = static_cast<index_t>(-1);
   }
 
 protected:
   /** \brief Point index stored in octree. */
-  int data_;
+  index_t data_;
 };
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -274,7 +274,7 @@ public:
    * \param[in] data_arg index to be stored within leaf node.
    */
   void
-  addPointIndex(int data_arg)
+  addPointIndex(index_t data_arg)
   {
     leafDataTVector_.push_back(data_arg);
   }
@@ -283,7 +283,7 @@ public:
    * point indices.
    * \return index stored within container.
    */
-  int
+  index_t
   getPointIndex() const
   {
     return leafDataTVector_.back();
@@ -295,7 +295,7 @@ public:
    * within data vector
    */
   void
-  getPointIndices(std::vector<int>& data_vector_arg) const
+  getPointIndices(Indices& data_vector_arg) const
   {
     data_vector_arg.insert(
         data_vector_arg.end(), leafDataTVector_.begin(), leafDataTVector_.end());
@@ -305,7 +305,7 @@ public:
    * of point indices.
    * \return reference to vector of point indices to be stored within data vector
    */
-  std::vector<int>&
+  Indices&
   getPointIndicesVector()
   {
     return leafDataTVector_;
@@ -314,10 +314,10 @@ public:
   /** \brief Get size of container (number of indices)
    * \return number of point indices in container.
    */
-  std::size_t
+  uindex_t
   getSize() const override
   {
-    return leafDataTVector_.size();
+    return static_cast<uindex_t>(leafDataTVector_.size());
   }
 
   /** \brief Reset leaf node. Clear DataT vector.*/
@@ -329,7 +329,7 @@ public:
 
 protected:
   /** \brief Leaf node DataT vector. */
-  std::vector<int> leafDataTVector_;
+  Indices leafDataTVector_;
 };
 
 } // namespace octree
diff --git a/octree/include/pcl/octree/octree_iterator.h b/octree/include/pcl/octree/octree_iterator.h
index 2156b327..2c1a848d 100644
--- a/octree/include/pcl/octree/octree_iterator.h
+++ b/octree/include/pcl/octree/octree_iterator.h
@@ -59,7 +59,7 @@ namespace octree {
 struct IteratorState {
   OctreeNode* node_;
   OctreeKey key_;
-  unsigned int depth_;
+  uindex_t depth_;
 };
 
 /** \brief @b Abstract octree iterator class
@@ -82,8 +82,13 @@ public:
 
   /** \brief Empty constructor.
    */
-  explicit OctreeIteratorBase(unsigned int max_depth_arg = 0)
-  : octree_(0), current_state_(0), max_octree_depth_(max_depth_arg)
+  OctreeIteratorBase() : OctreeIteratorBase(nullptr, 0u) {}
+
+  /** \brief Constructor.
+   * \param[in] max_depth_arg Depth limitation during traversal
+   */
+  explicit OctreeIteratorBase(uindex_t max_depth_arg)
+  : octree_(nullptr), current_state_(nullptr), max_octree_depth_(max_depth_arg)
   {
     this->reset();
   }
@@ -93,7 +98,14 @@ public:
    * root node.
    * \param[in] max_depth_arg Depth limitation during traversal
    */
-  explicit OctreeIteratorBase(OctreeT* octree_arg, unsigned int max_depth_arg = 0)
+  OctreeIteratorBase(OctreeT* octree_arg) : OctreeIteratorBase(octree_arg, 0u) {}
+
+  /** \brief Constructor.
+   * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its
+   * root node.
+   * \param[in] max_depth_arg Depth limitation during traversal
+   */
+  explicit OctreeIteratorBase(OctreeT* octree_arg, uindex_t max_depth_arg)
   : octree_(octree_arg), current_state_(0), max_octree_depth_(max_depth_arg)
   {
     this->reset();
@@ -108,7 +120,7 @@ public:
    *  \warning For advanced users only.
    */
   explicit OctreeIteratorBase(OctreeT* octree_arg,
-                              unsigned int max_depth_arg,
+                              uindex_t max_depth_arg,
                               IteratorState* current_state)
   : octree_(octree_arg), current_state_(current_state), max_octree_depth_(max_depth_arg)
   {}
@@ -169,7 +181,7 @@ public:
   /** \brief Get the current depth level of octree
    * \return depth level
    */
-  inline unsigned int
+  inline uindex_t
   getCurrentOctreeDepth() const
   {
     assert(octree_ != 0);
@@ -327,7 +339,7 @@ public:
     if (current_state_) {
       const OctreeKey& key = getCurrentOctreeKey();
       // calculate integer id with respect to octree key
-      unsigned int depth = octree_->getTreeDepth();
+      uindex_t depth = octree_->getTreeDepth();
       id = static_cast<unsigned long>(key.x) << (depth * 2) |
            static_cast<unsigned long>(key.y) << (depth * 1) |
            static_cast<unsigned long>(key.z) << (depth * 0);
@@ -344,7 +356,7 @@ protected:
   IteratorState* current_state_;
 
   /** \brief Maximum octree depth */
-  unsigned int max_octree_depth_;
+  uindex_t max_octree_depth_;
 };
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -364,15 +376,14 @@ public:
   /** \brief Empty constructor.
    * \param[in] max_depth_arg Depth limitation during traversal
    */
-  explicit OctreeDepthFirstIterator(unsigned int max_depth_arg = 0);
+  explicit OctreeDepthFirstIterator(uindex_t max_depth_arg = 0);
 
   /** \brief Constructor.
    * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its
    * root node.
    * \param[in] max_depth_arg Depth limitation during traversal
    */
-  explicit OctreeDepthFirstIterator(OctreeT* octree_arg,
-                                    unsigned int max_depth_arg = 0);
+  explicit OctreeDepthFirstIterator(OctreeT* octree_arg, uindex_t max_depth_arg = 0);
 
   /** \brief Constructor.
    * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its
@@ -384,7 +395,7 @@ public:
    */
   explicit OctreeDepthFirstIterator(
       OctreeT* octree_arg,
-      unsigned int max_depth_arg,
+      uindex_t max_depth_arg,
       IteratorState* current_state,
       const std::vector<IteratorState>& stack = std::vector<IteratorState>())
   : OctreeIteratorBase<OctreeT>(octree_arg, max_depth_arg, current_state), stack_(stack)
@@ -469,15 +480,14 @@ public:
   /** \brief Empty constructor.
    * \param[in] max_depth_arg Depth limitation during traversal
    */
-  explicit OctreeBreadthFirstIterator(unsigned int max_depth_arg = 0);
+  explicit OctreeBreadthFirstIterator(uindex_t max_depth_arg = 0);
 
   /** \brief Constructor.
    * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its
    * root node.
    * \param[in] max_depth_arg Depth limitation during traversal
    */
-  explicit OctreeBreadthFirstIterator(OctreeT* octree_arg,
-                                      unsigned int max_depth_arg = 0);
+  explicit OctreeBreadthFirstIterator(OctreeT* octree_arg, uindex_t max_depth_arg = 0);
 
   /** \brief Constructor.
    * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its
@@ -489,7 +499,7 @@ public:
    */
   explicit OctreeBreadthFirstIterator(
       OctreeT* octree_arg,
-      unsigned int max_depth_arg,
+      uindex_t max_depth_arg,
       IteratorState* current_state,
       const std::deque<IteratorState>& fifo = std::deque<IteratorState>())
   : OctreeIteratorBase<OctreeT>(octree_arg, max_depth_arg, current_state), FIFO_(fifo)
@@ -575,8 +585,7 @@ public:
    * root node.
    * \param[in] fixed_depth_arg Depth level during traversal
    */
-  explicit OctreeFixedDepthIterator(OctreeT* octree_arg,
-                                    unsigned int fixed_depth_arg = 0);
+  explicit OctreeFixedDepthIterator(OctreeT* octree_arg, uindex_t fixed_depth_arg = 0);
 
   /** \brief Constructor.
    * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its
@@ -589,7 +598,7 @@ public:
    */
   OctreeFixedDepthIterator(
       OctreeT* octree_arg,
-      unsigned int fixed_depth_arg,
+      uindex_t fixed_depth_arg,
       IteratorState* current_state,
       const std::deque<IteratorState>& fifo = std::deque<IteratorState>())
   : OctreeBreadthFirstIterator<OctreeT>(
@@ -623,7 +632,7 @@ public:
    * \param[in] fixed_depth_arg Depth level during traversal
    */
   void
-  reset(unsigned int fixed_depth_arg);
+  reset(uindex_t fixed_depth_arg);
 
   /** \brief Reset the iterator to the first node at the current depth
    */
@@ -637,7 +646,7 @@ protected:
   using OctreeBreadthFirstIterator<OctreeT>::FIFO_;
 
   /** \brief Given level of the node to be iterated */
-  unsigned int fixed_depth_;
+  uindex_t fixed_depth_;
 };
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -657,7 +666,7 @@ public:
   /** \brief Empty constructor.
    * \param[in] max_depth_arg Depth limitation during traversal
    */
-  explicit OctreeLeafNodeDepthFirstIterator(unsigned int max_depth_arg = 0)
+  explicit OctreeLeafNodeDepthFirstIterator(uindex_t max_depth_arg = 0)
   : OctreeDepthFirstIterator<OctreeT>(max_depth_arg)
   {
     reset();
@@ -669,7 +678,7 @@ public:
    * \param[in] max_depth_arg Depth limitation during traversal
    */
   explicit OctreeLeafNodeDepthFirstIterator(OctreeT* octree_arg,
-                                            unsigned int max_depth_arg = 0)
+                                            uindex_t max_depth_arg = 0)
   : OctreeDepthFirstIterator<OctreeT>(octree_arg, max_depth_arg)
   {
     reset();
@@ -685,7 +694,7 @@ public:
    */
   explicit OctreeLeafNodeDepthFirstIterator(
       OctreeT* octree_arg,
-      unsigned int max_depth_arg,
+      uindex_t max_depth_arg,
       IteratorState* current_state,
       const std::vector<IteratorState>& stack = std::vector<IteratorState>())
   : OctreeDepthFirstIterator<OctreeT>(octree_arg, max_depth_arg, current_state, stack)
@@ -759,7 +768,7 @@ public:
   /** \brief Empty constructor.
    * \param[in] max_depth_arg Depth limitation during traversal
    */
-  explicit OctreeLeafNodeBreadthFirstIterator(unsigned int max_depth_arg = 0);
+  explicit OctreeLeafNodeBreadthFirstIterator(uindex_t max_depth_arg = 0);
 
   /** \brief Constructor.
    * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its
@@ -767,7 +776,7 @@ public:
    * \param[in] max_depth_arg Depth limitation during traversal
    */
   explicit OctreeLeafNodeBreadthFirstIterator(OctreeT* octree_arg,
-                                              unsigned int max_depth_arg = 0);
+                                              uindex_t max_depth_arg = 0);
 
   /** \brief Copy constructor.
    * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its
@@ -780,7 +789,7 @@ public:
    */
   explicit OctreeLeafNodeBreadthFirstIterator(
       OctreeT* octree_arg,
-      unsigned int max_depth_arg,
+      uindex_t max_depth_arg,
       IteratorState* current_state,
       const std::deque<IteratorState>& fifo = std::deque<IteratorState>());
 
diff --git a/octree/include/pcl/octree/octree_key.h b/octree/include/pcl/octree/octree_key.h
index a103d5a3..4e676221 100644
--- a/octree/include/pcl/octree/octree_key.h
+++ b/octree/include/pcl/octree/octree_key.h
@@ -55,9 +55,7 @@ public:
   OctreeKey() : x(0), y(0), z(0) {}
 
   /** \brief Constructor for key initialization. */
-  OctreeKey(unsigned int keyX, unsigned int keyY, unsigned int keyZ)
-  : x(keyX), y(keyY), z(keyZ)
-  {}
+  OctreeKey(uindex_t keyX, uindex_t keyY, uindex_t keyZ) : x(keyX), y(keyY), z(keyZ) {}
 
   /** \brief Copy constructor. */
   OctreeKey(const OctreeKey& source) { std::memcpy(key_, source.key_, sizeof(key_)); }
@@ -131,7 +129,7 @@ public:
    *  \return child node index
    * */
   inline unsigned char
-  getChildIdxWithDepthMask(unsigned int depthMask) const
+  getChildIdxWithDepthMask(uindex_t depthMask) const
   {
     return static_cast<unsigned char>(((!!(this->x & depthMask)) << 2) |
                                       ((!!(this->y & depthMask)) << 1) |
@@ -140,17 +138,17 @@ public:
 
   /* \brief maximum depth that can be addressed */
   static const unsigned char maxDepth =
-      static_cast<unsigned char>(sizeof(std::uint32_t) * 8);
+      static_cast<unsigned char>(sizeof(uindex_t) * 8);
 
   // Indices addressing a voxel at (X, Y, Z)
 
   union {
     struct {
-      std::uint32_t x;
-      std::uint32_t y;
-      std::uint32_t z;
+      uindex_t x;
+      uindex_t y;
+      uindex_t z;
     };
-    std::uint32_t key_[3];
+    uindex_t key_[3];
   };
 };
 } // namespace octree
diff --git a/octree/include/pcl/octree/octree_nodes.h b/octree/include/pcl/octree/octree_nodes.h
index e00182df..b1599a78 100644
--- a/octree/include/pcl/octree/octree_nodes.h
+++ b/octree/include/pcl/octree/octree_nodes.h
@@ -42,10 +42,7 @@
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
 
-#include <Eigen/Core>
-
 #include <cassert>
-#include <cstddef>
 
 namespace pcl {
 namespace octree {
diff --git a/octree/include/pcl/octree/octree_pointcloud.h b/octree/include/pcl/octree/octree_pointcloud.h
index 4f66675c..be4df0fc 100644
--- a/octree/include/pcl/octree/octree_pointcloud.h
+++ b/octree/include/pcl/octree/octree_pointcloud.h
@@ -82,8 +82,8 @@ public:
   OctreePointCloud(const double resolution_arg);
 
   // public typedefs
-  using IndicesPtr = shared_ptr<std::vector<int>>;
-  using IndicesConstPtr = shared_ptr<const std::vector<int>>;
+  using IndicesPtr = shared_ptr<Indices>;
+  using IndicesConstPtr = shared_ptr<const Indices>;
 
   using PointCloud = pcl::PointCloud<PointT>;
   using PointCloudPtr = typename PointCloud::Ptr;
@@ -183,7 +183,7 @@ public:
   /** \brief Get the maximum depth of the octree.
    *  \return depth_arg: maximum depth of octree
    * */
-  inline unsigned int
+  inline uindex_t
   getTreeDepth() const
   {
     return this->octree_depth_;
@@ -200,7 +200,7 @@ public:
    * setInputCloud)
    */
   void
-  addPointFromCloud(const int point_idx_arg, IndicesPtr indices_arg);
+  addPointFromCloud(uindex_t point_idx_arg, IndicesPtr indices_arg);
 
   /** \brief Add point simultaneously to octree and input point cloud.
    *  \param[in] point_arg point to be added
@@ -258,14 +258,14 @@ public:
    * \return "true" if voxel exist; "false" otherwise
    */
   bool
-  isVoxelOccupiedAtPoint(const int& point_idx_arg) const;
+  isVoxelOccupiedAtPoint(const index_t& point_idx_arg) const;
 
   /** \brief Get a PointT vector of centers of all occupied voxels.
    * \param[out] voxel_center_list_arg results are written to this vector of PointT
    * elements
    * \return number of occupied voxels
    */
-  int
+  uindex_t
   getOccupiedVoxelCenters(AlignedPointTVector& voxel_center_list_arg) const;
 
   /** \brief Get a PointT vector of centers of voxels intersected by a line segment.
@@ -279,7 +279,7 @@ public:
    * octree_resolution x precision
    * \return number of intersected voxels
    */
-  int
+  uindex_t
   getApproxIntersectedVoxelCentersBySegment(const Eigen::Vector3f& origin,
                                             const Eigen::Vector3f& end,
                                             AlignedPointTVector& voxel_center_list,
@@ -295,7 +295,7 @@ public:
    *  \param[in] point_idx_arg index of point addressing the voxel to be deleted.
    */
   void
-  deleteVoxelAtPoint(const int& point_idx_arg);
+  deleteVoxelAtPoint(const index_t& point_idx_arg);
 
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   // Bounding box methods
@@ -365,7 +365,7 @@ public:
    * \return squared diameter
    */
   double
-  getVoxelSquaredDiameter(unsigned int tree_depth_arg) const;
+  getVoxelSquaredDiameter(uindex_t tree_depth_arg) const;
 
   /** \brief Calculates the squared diameter of a voxel at leaf depth
    * \return squared diameter
@@ -381,7 +381,7 @@ public:
    * \return squared voxel cube side length
    */
   double
-  getVoxelSquaredSideLen(unsigned int tree_depth_arg) const;
+  getVoxelSquaredSideLen(uindex_t tree_depth_arg) const;
 
   /** \brief Calculates the squared voxel cube side length at leaf level
    * \return squared voxel cube side length
@@ -428,7 +428,7 @@ protected:
    * \a setInputCloud to be added
    */
   virtual void
-  addPointIdx(const int point_idx_arg);
+  addPointIdx(uindex_t point_idx_arg);
 
   /** \brief Add point at index from input pointcloud dataset to octree
    * \param[in] leaf_node to be expanded
@@ -440,7 +440,7 @@ protected:
   expandLeafNode(LeafNode* leaf_node,
                  BranchNode* parent_branch,
                  unsigned char child_idx,
-                 unsigned int depth_mask);
+                 uindex_t depth_mask);
 
   /** \brief Get point at index from input pointcloud dataset
    * \param[in] index_arg index representing the point in the dataset given by \a
@@ -448,7 +448,7 @@ protected:
    * \return PointT from input pointcloud dataset
    */
   const PointT&
-  getPointByIndex(const unsigned int index_arg) const;
+  getPointByIndex(uindex_t index_arg) const;
 
   /** \brief Find octree leaf node at a given point
    * \param[in] point_arg query point
@@ -520,7 +520,7 @@ protected:
    * are assignable
    */
   virtual bool
-  genOctreeKeyForDataT(const int& data_arg, OctreeKey& key_arg) const;
+  genOctreeKeyForDataT(const index_t& data_arg, OctreeKey& key_arg) const;
 
   /** \brief Generate a point at center of leaf node voxel
    * \param[in] key_arg octree key addressing a leaf node.
@@ -536,7 +536,7 @@ protected:
    */
   void
   genVoxelCenterFromOctreeKey(const OctreeKey& key_arg,
-                              unsigned int tree_depth_arg,
+                              uindex_t tree_depth_arg,
                               PointT& point_arg) const;
 
   /** \brief Generate bounds of an octree voxel using octree key and tree depth
@@ -548,7 +548,7 @@ protected:
    */
   void
   genVoxelBoundsFromOctreeKey(const OctreeKey& key_arg,
-                              unsigned int tree_depth_arg,
+                              uindex_t tree_depth_arg,
                               Eigen::Vector3f& min_pt,
                               Eigen::Vector3f& max_pt) const;
 
@@ -560,7 +560,7 @@ protected:
    * elements
    * \return number of voxels found
    */
-  int
+  uindex_t
   getOccupiedVoxelCentersRecursive(const BranchNode* node_arg,
                                    const OctreeKey& key_arg,
                                    AlignedPointTVector& voxel_center_list_arg) const;
diff --git a/octree/include/pcl/octree/octree_pointcloud_adjacency.h b/octree/include/pcl/octree/octree_pointcloud_adjacency.h
index 64db9804..0d4ea118 100644
--- a/octree/include/pcl/octree/octree_pointcloud_adjacency.h
+++ b/octree/include/pcl/octree/octree_pointcloud_adjacency.h
@@ -39,12 +39,10 @@
 
 #pragma once
 
-#include <pcl/octree/boost.h>
 #include <pcl/octree/octree_pointcloud.h>
 #include <pcl/octree/octree_pointcloud_adjacency_container.h>
 
-#include <list>
-#include <set>
+#include <boost/graph/adjacency_list.hpp> // for adjacency_list
 
 namespace pcl {
 
@@ -197,7 +195,7 @@ protected:
    * \note This virtual implementation allows the use of a transform function to compute
    * keys. */
   void
-  addPointIdx(const int point_idx_arg) override;
+  addPointIdx(uindex_t point_idx_arg) override;
 
   /** \brief Fills in the neighbors fields for new voxels.
    *
diff --git a/octree/include/pcl/octree/octree_pointcloud_adjacency_container.h b/octree/include/pcl/octree/octree_pointcloud_adjacency_container.h
index b6e5ad62..6cff9dad 100644
--- a/octree/include/pcl/octree/octree_pointcloud_adjacency_container.h
+++ b/octree/include/pcl/octree/octree_pointcloud_adjacency_container.h
@@ -39,6 +39,8 @@
 
 #pragma once
 
+#include <list> // for std::list
+
 namespace pcl {
 
 namespace octree {
@@ -92,7 +94,7 @@ public:
   }
 
   /** \brief Gets the number of points contributing to this leaf */
-  int
+  uindex_t
   getPointCounter() const
   {
     return num_points_;
@@ -117,7 +119,7 @@ public:
   /** \brief  virtual method to get size of container
    * \return number of points added to leaf node container.
    */
-  std::size_t
+  uindex_t
   getSize() const override
   {
     return num_points_;
@@ -168,7 +170,7 @@ protected:
 
   /** \brief Sets the number of points contributing to this leaf */
   void
-  setPointCounter(int points_arg)
+  setPointCounter(uindex_t points_arg)
   {
     num_points_ = points_arg;
   }
@@ -216,7 +218,7 @@ protected:
   }
 
 private:
-  int num_points_;
+  uindex_t num_points_;
   NeighborListT neighbors_;
   DataT data_;
 };
diff --git a/octree/include/pcl/octree/octree_pointcloud_changedetector.h b/octree/include/pcl/octree/octree_pointcloud_changedetector.h
index 902dfd2c..c8ddadf3 100644
--- a/octree/include/pcl/octree/octree_pointcloud_changedetector.h
+++ b/octree/include/pcl/octree/octree_pointcloud_changedetector.h
@@ -91,15 +91,15 @@ public:
    * \return number of point indices
    */
   std::size_t
-  getPointIndicesFromNewVoxels(std::vector<int>& indicesVector_arg,
-                               const int minPointsPerLeaf_arg = 0)
+  getPointIndicesFromNewVoxels(Indices& indicesVector_arg,
+                               const uindex_t minPointsPerLeaf_arg = 0)
   {
 
     std::vector<OctreeContainerPointIndices*> leaf_containers;
     this->serializeNewLeafs(leaf_containers);
 
     for (const auto& leaf_container : leaf_containers) {
-      if (static_cast<int>(leaf_container->getSize()) >= minPointsPerLeaf_arg)
+      if (static_cast<uindex_t>(leaf_container->getSize()) >= minPointsPerLeaf_arg)
         leaf_container->getPointIndices(indicesVector_arg);
     }
 
diff --git a/octree/include/pcl/octree/octree_pointcloud_density.h b/octree/include/pcl/octree/octree_pointcloud_density.h
index 19e97d04..72be1cb1 100644
--- a/octree/include/pcl/octree/octree_pointcloud_density.h
+++ b/octree/include/pcl/octree/octree_pointcloud_density.h
@@ -76,16 +76,12 @@ public:
 
   /** \brief Read input data. Only an internal counter is increased.
    */
-  void
-  addPointIndex(int)
-  {
-    point_counter_++;
-  }
+  void addPointIndex(uindex_t) { point_counter_++; }
 
   /** \brief Return point counter.
    * \return Amount of points
    */
-  unsigned int
+  uindex_t
   getPointCounter()
   {
     return (point_counter_);
@@ -99,7 +95,7 @@ public:
   }
 
 private:
-  unsigned int point_counter_;
+  uindex_t point_counter_;
 };
 
 /** \brief @b Octree pointcloud density class
@@ -133,10 +129,10 @@ public:
    * \param[in] point_arg: a point addressing a voxel \return amount of points
    * that fall within leaf node voxel
    */
-  unsigned int
+  uindex_t
   getVoxelDensityAtPoint(const PointT& point_arg) const
   {
-    unsigned int point_count = 0;
+    uindex_t point_count = 0;
 
     OctreePointCloudDensityContainer* leaf = this->findLeafAtPoint(point_arg);
 
@@ -149,5 +145,8 @@ public:
 } // namespace octree
 } // namespace pcl
 
+// needed since OctreePointCloud is not instantiated with template parameters used above
+#include <pcl/octree/impl/octree_pointcloud.hpp>
+
 #define PCL_INSTANTIATE_OctreePointCloudDensity(T)                                     \
   template class PCL_EXPORTS pcl::octree::OctreePointCloudDensity<T>;
diff --git a/octree/include/pcl/octree/octree_pointcloud_singlepoint.h b/octree/include/pcl/octree/octree_pointcloud_singlepoint.h
index a307bc89..3c8e71e5 100644
--- a/octree/include/pcl/octree/octree_pointcloud_singlepoint.h
+++ b/octree/include/pcl/octree/octree_pointcloud_singlepoint.h
@@ -86,5 +86,8 @@ public:
 } // namespace octree
 } // namespace pcl
 
+// needed since OctreePointCloud is not instantiated with template parameters used above
+#include <pcl/octree/impl/octree_pointcloud.hpp>
+
 #define PCL_INSTANTIATE_OctreePointCloudSinglePoint(T)                                 \
   template class PCL_EXPORTS pcl::octree::OctreePointCloudSinglePoint<T>;
diff --git a/octree/include/pcl/octree/octree_pointcloud_voxelcentroid.h b/octree/include/pcl/octree/octree_pointcloud_voxelcentroid.h
index 1207db8e..a3111336 100644
--- a/octree/include/pcl/octree/octree_pointcloud_voxelcentroid.h
+++ b/octree/include/pcl/octree/octree_pointcloud_voxelcentroid.h
@@ -114,7 +114,7 @@ public:
   }
 
 private:
-  unsigned int point_counter_;
+  uindex_t point_counter_;
   PointT point_sum_;
 };
 
@@ -156,11 +156,11 @@ public:
    * \param pointIdx_arg
    */
   void
-  addPointIdx(const int pointIdx_arg) override
+  addPointIdx(const uindex_t pointIdx_arg) override
   {
     OctreeKey key;
 
-    assert(pointIdx_arg < static_cast<int>(this->input_->size()));
+    assert(pointIdx_arg < this->input_->size());
 
     const PointT& point = (*this->input_)[pointIdx_arg];
 
@@ -190,7 +190,8 @@ public:
    * \return "true" if voxel is found; "false" otherwise
    */
   inline bool
-  getVoxelCentroidAtPoint(const int& point_idx_arg, PointT& voxel_centroid_arg) const
+  getVoxelCentroidAtPoint(const index_t& point_idx_arg,
+                          PointT& voxel_centroid_arg) const
   {
     // get centroid at point
     return (this->getVoxelCentroidAtPoint((*this->input_)[point_idx_arg],
@@ -202,7 +203,7 @@ public:
    * elements
    * \return number of occupied voxels
    */
-  std::size_t
+  uindex_t
   getVoxelCentroids(
       typename OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::
           AlignedPointTVector& voxel_centroid_list_arg) const;
diff --git a/octree/include/pcl/octree/octree_search.h b/octree/include/pcl/octree/octree_search.h
index 83d30c6f..ed29fb2d 100644
--- a/octree/include/pcl/octree/octree_search.h
+++ b/octree/include/pcl/octree/octree_search.h
@@ -58,8 +58,8 @@ class OctreePointCloudSearch
 : public OctreePointCloud<PointT, LeafContainerT, BranchContainerT> {
 public:
   // public typedefs
-  using IndicesPtr = shared_ptr<std::vector<int>>;
-  using IndicesConstPtr = shared_ptr<const std::vector<int>>;
+  using IndicesPtr = shared_ptr<Indices>;
+  using IndicesConstPtr = shared_ptr<const Indices>;
 
   using PointCloud = pcl::PointCloud<PointT>;
   using PointCloudPtr = typename PointCloud::Ptr;
@@ -91,7 +91,7 @@ public:
    * \return "true" if leaf node exist; "false" otherwise
    */
   bool
-  voxelSearch(const PointT& point, std::vector<int>& point_idx_data);
+  voxelSearch(const PointT& point, Indices& point_idx_data);
 
   /** \brief Search for neighbors within a voxel at given point referenced by a point
    * index
@@ -100,7 +100,7 @@ public:
    * \return "true" if leaf node exist; "false" otherwise
    */
   bool
-  voxelSearch(const int index, std::vector<int>& point_idx_data);
+  voxelSearch(uindex_t index, Indices& point_idx_data);
 
   /** \brief Search for k-nearest neighbors at the query point.
    * \param[in] cloud the point cloud data
@@ -112,11 +112,11 @@ public:
    * points (must be resized to \a k a priori!)
    * \return number of neighbors found
    */
-  inline int
+  inline uindex_t
   nearestKSearch(const PointCloud& cloud,
-                 int index,
-                 int k,
-                 std::vector<int>& k_indices,
+                 uindex_t index,
+                 uindex_t k,
+                 Indices& k_indices,
                  std::vector<float>& k_sqr_distances)
   {
     return (nearestKSearch(cloud[index], k, k_indices, k_sqr_distances));
@@ -131,10 +131,10 @@ public:
    * points (must be resized to k a priori!)
    * \return number of neighbors found
    */
-  int
+  uindex_t
   nearestKSearch(const PointT& p_q,
-                 int k,
-                 std::vector<int>& k_indices,
+                 uindex_t k,
+                 Indices& k_indices,
                  std::vector<float>& k_sqr_distances);
 
   /** \brief Search for k-nearest neighbors at query point
@@ -148,10 +148,10 @@ public:
    * points (must be resized to \a k a priori!)
    * \return number of neighbors found
    */
-  int
-  nearestKSearch(int index,
-                 int k,
-                 std::vector<int>& k_indices,
+  uindex_t
+  nearestKSearch(uindex_t index,
+                 uindex_t k,
+                 Indices& k_indices,
                  std::vector<float>& k_sqr_distances);
 
   /** \brief Search for approx. nearest neighbor at the query point.
@@ -163,8 +163,8 @@ public:
    */
   inline void
   approxNearestSearch(const PointCloud& cloud,
-                      int query_index,
-                      int& result_index,
+                      uindex_t query_index,
+                      index_t& result_index,
                       float& sqr_distance)
   {
     return (approxNearestSearch(cloud[query_index], result_index, sqr_distance));
@@ -176,7 +176,7 @@ public:
    * \param[out] sqr_distance the resultant squared distance to the neighboring point
    */
   void
-  approxNearestSearch(const PointT& p_q, int& result_index, float& sqr_distance);
+  approxNearestSearch(const PointT& p_q, index_t& result_index, float& sqr_distance);
 
   /** \brief Search for approx. nearest neighbor at the query point.
    * \param[in] query_index index representing the query point in the dataset given by
@@ -187,7 +187,7 @@ public:
    * \return number of neighbors found
    */
   void
-  approxNearestSearch(int query_index, int& result_index, float& sqr_distance);
+  approxNearestSearch(uindex_t query_index, index_t& result_index, float& sqr_distance);
 
   /** \brief Search for all neighbors of query point that are within a given radius.
    * \param[in] cloud the point cloud data
@@ -199,13 +199,13 @@ public:
    * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
    * \return number of neighbors found in radius
    */
-  int
+  uindex_t
   radiusSearch(const PointCloud& cloud,
-               int index,
+               uindex_t index,
                double radius,
-               std::vector<int>& k_indices,
+               Indices& k_indices,
                std::vector<float>& k_sqr_distances,
-               unsigned int max_nn = 0)
+               index_t max_nn = 0)
   {
     return (radiusSearch(cloud[index], radius, k_indices, k_sqr_distances, max_nn));
   }
@@ -219,12 +219,12 @@ public:
    * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
    * \return number of neighbors found in radius
    */
-  int
+  uindex_t
   radiusSearch(const PointT& p_q,
                const double radius,
-               std::vector<int>& k_indices,
+               Indices& k_indices,
                std::vector<float>& k_sqr_distances,
-               unsigned int max_nn = 0) const;
+               uindex_t max_nn = 0) const;
 
   /** \brief Search for all neighbors of query point that are within a given radius.
    * \param[in] index index representing the query point in the dataset given by \a
@@ -237,12 +237,12 @@ public:
    * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
    * \return number of neighbors found in radius
    */
-  int
-  radiusSearch(int index,
+  uindex_t
+  radiusSearch(uindex_t index,
                const double radius,
-               std::vector<int>& k_indices,
+               Indices& k_indices,
                std::vector<float>& k_sqr_distances,
-               unsigned int max_nn = 0) const;
+               uindex_t max_nn = 0) const;
 
   /** \brief Get a PointT vector of centers of all voxels that intersected by a ray
    * (origin, direction).
@@ -253,11 +253,11 @@ public:
    * disable)
    * \return number of intersected voxels
    */
-  int
+  uindex_t
   getIntersectedVoxelCenters(Eigen::Vector3f origin,
                              Eigen::Vector3f direction,
                              AlignedPointTVector& voxel_center_list,
-                             int max_voxel_count = 0) const;
+                             uindex_t max_voxel_count = 0) const;
 
   /** \brief Get indices of all voxels that are intersected by a ray (origin,
    * direction).
@@ -267,11 +267,11 @@ public:
    * disable)
    * \return number of intersected voxels
    */
-  int
+  uindex_t
   getIntersectedVoxelIndices(Eigen::Vector3f origin,
                              Eigen::Vector3f direction,
-                             std::vector<int>& k_indices,
-                             int max_voxel_count = 0) const;
+                             Indices& k_indices,
+                             uindex_t max_voxel_count = 0) const;
 
   /** \brief Search for points within rectangular search area
    * Points exactly on the edges of the search rectangle are included.
@@ -280,10 +280,10 @@ public:
    * \param[out] k_indices the resultant point indices
    * \return number of points found within search area
    */
-  int
+  uindex_t
   boxSearch(const Eigen::Vector3f& min_pt,
             const Eigen::Vector3f& max_pt,
-            std::vector<int>& k_indices) const;
+            Indices& k_indices) const;
 
 protected:
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -341,7 +341,7 @@ protected:
      * \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)
+    prioPointQueueEntry(uindex_t point_idx, float point_distance)
     : point_idx_(point_idx), point_distance_(point_distance)
     {}
 
@@ -355,7 +355,7 @@ protected:
     }
 
     /** \brief Index representing a point in the dataset given by \a setInputCloud. */
-    int point_idx_;
+    uindex_t point_idx_;
 
     /** \brief Distance to query point. */
     float point_distance_;
@@ -388,10 +388,10 @@ protected:
                                     const double radiusSquared,
                                     const BranchNode* node,
                                     const OctreeKey& key,
-                                    unsigned int tree_depth,
-                                    std::vector<int>& k_indices,
+                                    uindex_t tree_depth,
+                                    Indices& k_indices,
                                     std::vector<float>& k_sqr_distances,
-                                    unsigned int max_nn) const;
+                                    uindex_t max_nn) const;
 
   /** \brief Recursive search method that explores the octree and finds the K nearest
    * neighbors
@@ -407,10 +407,10 @@ protected:
   double
   getKNearestNeighborRecursive(
       const PointT& point,
-      unsigned int K,
+      uindex_t K,
       const BranchNode* node,
       const OctreeKey& key,
-      unsigned int tree_depth,
+      uindex_t tree_depth,
       const double squared_search_radius,
       std::vector<prioPointQueueEntry>& point_candidates) const;
 
@@ -427,8 +427,8 @@ protected:
   approxNearestSearchRecursive(const PointT& point,
                                const BranchNode* node,
                                const OctreeKey& key,
-                               unsigned int tree_depth,
-                               int& result_index,
+                               uindex_t tree_depth,
+                               index_t& result_index,
                                float& sqr_distance);
 
   /** \brief Recursively search the tree for all intersected leaf nodes and return a
@@ -449,7 +449,7 @@ protected:
    * disable)
    * \return number of voxels found
    */
-  int
+  uindex_t
   getIntersectedVoxelCentersRecursive(double min_x,
                                       double min_y,
                                       double min_z,
@@ -460,7 +460,7 @@ protected:
                                       const OctreeNode* node,
                                       const OctreeKey& key,
                                       AlignedPointTVector& voxel_center_list,
-                                      int max_voxel_count) const;
+                                      uindex_t max_voxel_count) const;
 
   /** \brief Recursive search method that explores the octree and finds points within a
    * rectangular search area
@@ -476,8 +476,8 @@ protected:
                      const Eigen::Vector3f& max_pt,
                      const BranchNode* node,
                      const OctreeKey& key,
-                     unsigned int tree_depth,
-                     std::vector<int>& k_indices) const;
+                     uindex_t tree_depth,
+                     Indices& k_indices) const;
 
   /** \brief Recursively search the tree for all intersected leaf nodes and return a
    * vector of indices. This algorithm is based off the paper An Efficient Parametric
@@ -496,7 +496,7 @@ protected:
    * disable)
    * \return number of voxels found
    */
-  int
+  uindex_t
   getIntersectedVoxelIndicesRecursive(double min_x,
                                       double min_y,
                                       double min_z,
@@ -506,8 +506,8 @@ protected:
                                       unsigned char a,
                                       const OctreeNode* node,
                                       const OctreeKey& key,
-                                      std::vector<int>& k_indices,
-                                      int max_voxel_count) const;
+                                      Indices& k_indices,
+                                      uindex_t max_voxel_count) const;
 
   /** \brief Initialize raytracing algorithm
    * \param origin
diff --git a/octree/src/octree_inst.cpp b/octree/src/octree_inst.cpp
index e0d26650..2f149f0b 100644
--- a/octree/src/octree_inst.cpp
+++ b/octree/src/octree_inst.cpp
@@ -39,8 +39,8 @@
 
 // Instantiations of specific point types
 
-template class PCL_EXPORTS pcl::octree::OctreeBase<int>;
-template class PCL_EXPORTS pcl::octree::Octree2BufBase<int>;
+template class PCL_EXPORTS pcl::octree::OctreeBase<pcl::index_t>;
+template class PCL_EXPORTS pcl::octree::Octree2BufBase<pcl::index_t>;
 
 template class PCL_EXPORTS
     pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndices,
diff --git a/outofcore/CMakeLists.txt b/outofcore/CMakeLists.txt
index 5860204b..7d8073c2 100644
--- a/outofcore/CMakeLists.txt
+++ b/outofcore/CMakeLists.txt
@@ -58,6 +58,15 @@ set(visualization_incs
   "include/pcl/${SUBSYS_NAME}/visualization/scene.h"
   "include/pcl/${SUBSYS_NAME}/visualization/viewport.h"
 )
+
+# Code in subdirectory `visualization` may use deprecated declarations when using OpenGLv1
+# Add the GCC exclusive rules for -Werror only for OpenGLv1 compile to avoid build interruption
+if(VTK_RENDERING_BACKEND_OPENGL_VERSION VERSION_LESS 2)
+  if(CMAKE_COMPILER_IS_GNUCXX)
+    add_compile_options("-Wno-error=deprecated-declarations")
+  endif()
+endif()
+
 set(LIB_NAME "pcl_${SUBSYS_NAME}")
 
 include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include")
diff --git a/outofcore/include/pcl/outofcore/impl/octree_base.hpp b/outofcore/include/pcl/outofcore/impl/octree_base.hpp
index feac27c8..96f3819f 100644
--- a/outofcore/include/pcl/outofcore/impl/octree_base.hpp
+++ b/outofcore/include/pcl/outofcore/impl/octree_base.hpp
@@ -640,7 +640,7 @@ namespace pcl
           pcl::PCLPointCloud2::Ptr downsampled_cloud (new pcl::PCLPointCloud2 ());
 
           //create destination for indices
-          pcl::IndicesPtr downsampled_cloud_indices (new std::vector< int > ());
+          pcl::IndicesPtr downsampled_cloud_indices (new pcl::Indices ());
           lod_filter_ptr_->filter (*downsampled_cloud_indices);
 
           //extract the "random subset", size by setSampleSize
diff --git a/outofcore/include/pcl/outofcore/impl/octree_base_node.hpp b/outofcore/include/pcl/outofcore/impl/octree_base_node.hpp
index 3374a400..5347df01 100644
--- a/outofcore/include/pcl/outofcore/impl/octree_base_node.hpp
+++ b/outofcore/include/pcl/outofcore/impl/octree_base_node.hpp
@@ -511,7 +511,7 @@ namespace pcl
 
         //indices to store the points for each bin
         //these lists will be used to copy data to new point clouds and pass down recursively
-        std::vector < std::vector<int> > indices;
+        std::vector < pcl::Indices > indices;
         indices.resize (8);
         
         this->sortOctantIndices (input_cloud, indices, node_metadata_->getVoxelCenter ());
@@ -739,7 +739,7 @@ namespace pcl
       pcl::PCLPointCloud2::Ptr downsampled_cloud ( new pcl::PCLPointCloud2 () );
 
       //create destination for indices
-      pcl::IndicesPtr downsampled_cloud_indices ( new std::vector< int > () );
+      pcl::IndicesPtr downsampled_cloud_indices ( new pcl::Indices () );
       random_sampler.filter (*downsampled_cloud_indices);
 
       //extract the "random subset", size by setSampleSize
@@ -766,7 +766,7 @@ namespace pcl
       PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Remaining points are %u\n",__FUNCTION__, remaining_points->width*remaining_points->height);
 
       //subdivide remaining data by destination octant
-      std::vector<std::vector<int> > indices;
+      std::vector<pcl::Indices> indices;
       indices.resize (8);
 
       this->sortOctantIndices (remaining_points, indices, node_metadata_->getVoxelCenter ());
@@ -864,7 +864,7 @@ namespace pcl
       //if already has 8 children, return
       if (children_[idx] || (num_children_ == 8))
       {
-        PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode::createChild] Not allowed to create a 9th child of %s",this->node_metadata_->getMetadataFilename ().c_str ());
+        PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode::createChild] Not allowed to create a 9th child of %s\n",this->node_metadata_->getMetadataFilename ().c_str ());
         return;
       }
 
@@ -1451,7 +1451,7 @@ namespace pcl
           Eigen::Vector4f min_pt ( static_cast<float> ( min_bb[0] ), static_cast<float> ( min_bb[1] ), static_cast<float> ( min_bb[2] ), 1.0f);
           Eigen::Vector4f max_pt ( static_cast<float> ( max_bb[0] ), static_cast<float> ( max_bb[1] ) , static_cast<float>( max_bb[2] ), 1.0f );
 
-          std::vector<int> indices;
+          pcl::Indices indices;
 
           pcl::getPointsInBox ( *tmp_cloud, min_pt, max_pt, indices );
           PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in box: %d\n", __FUNCTION__, indices.size () );
@@ -1614,7 +1614,7 @@ namespace pcl
               pcl::ExtractIndices<pcl::PCLPointCloud2> extractor;
               extractor.setInputCloud (tmp_blob);
               
-              pcl::IndicesPtr downsampled_cloud_indices (new std::vector<int> ());
+              pcl::IndicesPtr downsampled_cloud_indices (new pcl::Indices ());
               random_sampler.filter (*downsampled_cloud_indices);
               extractor.setIndices (downsampled_cloud_indices);
               extractor.filter (*downsampled_points);
@@ -1973,7 +1973,7 @@ namespace pcl
     ////////////////////////////////////////////////////////////////////////////////
 
     template<typename ContainerT, typename PointT> void
-    OutofcoreOctreeBaseNode<ContainerT, PointT>::sortOctantIndices (const pcl::PCLPointCloud2::Ptr &input_cloud, std::vector< std::vector<int> > &indices, const Eigen::Vector3d &mid_xyz)
+    OutofcoreOctreeBaseNode<ContainerT, PointT>::sortOctantIndices (const pcl::PCLPointCloud2::Ptr &input_cloud, std::vector< pcl::Indices > &indices, const Eigen::Vector3d &mid_xyz)
     {
       if (indices.size () < 8)
         indices.resize (8);
@@ -1999,7 +1999,7 @@ namespace pcl
 
         if(!this->pointInBoundingBox (local_pt))
         {
-          PCL_ERROR ("pcl::outofcore::OutofcoreOctreeBaseNode::%s] Point %2.lf %.2lf %.2lf not in bounding box", __FUNCTION__, local_pt.x, local_pt.y, local_pt.z);
+          PCL_ERROR ("pcl::outofcore::OutofcoreOctreeBaseNode::%s] Point %2.lf %.2lf %.2lf not in bounding box\n", __FUNCTION__, local_pt.x, local_pt.y, local_pt.z);
         }
         
         assert (this->pointInBoundingBox (local_pt) == true);
diff --git a/outofcore/include/pcl/outofcore/octree_abstract_node_container.h b/outofcore/include/pcl/outofcore/octree_abstract_node_container.h
index bcc0a670..911be6c6 100644
--- a/outofcore/include/pcl/outofcore/octree_abstract_node_container.h
+++ b/outofcore/include/pcl/outofcore/octree_abstract_node_container.h
@@ -39,7 +39,6 @@
 #pragma once
 
 #include <mutex>
-#include <string>
 #include <vector>
 
 #include <pcl/outofcore/boost.h>
diff --git a/outofcore/include/pcl/outofcore/octree_base.h b/outofcore/include/pcl/outofcore/octree_base.h
index 8152c9d3..5cac314f 100644
--- a/outofcore/include/pcl/outofcore/octree_base.h
+++ b/outofcore/include/pcl/outofcore/octree_base.h
@@ -180,8 +180,8 @@ namespace pcl
 
         using PointCloud = pcl::PointCloud<PointT>;
 
-        using IndicesPtr = shared_ptr<std::vector<int> >;
-        using IndicesConstPtr = shared_ptr<const std::vector<int> >;
+        using IndicesPtr = shared_ptr<pcl::Indices>;
+        using IndicesConstPtr = shared_ptr<const pcl::Indices>;
 
         using PointCloudPtr = typename PointCloud::Ptr;
         using PointCloudConstPtr = typename PointCloud::ConstPtr;
@@ -257,7 +257,7 @@ namespace pcl
          *
          * \param[in] input_cloud The cloud of points to be inserted into the out-of-core octree. Note if multiple PCLPointCloud2 objects are added to the tree, this assumes that they all have exactly the same fields.
          * \param[in] skip_bb_check (default=false) whether to skip the bounding box check on insertion. Note the bounding box check is never skipped in the current implementation.
-         * \return Number of poitns successfully copied from the point cloud to the octree
+         * \return Number of points successfully copied from the point cloud to the octree
          */
         std::uint64_t
         addPointCloud (pcl::PCLPointCloud2::Ptr &input_cloud, const bool skip_bb_check = false);
diff --git a/outofcore/include/pcl/outofcore/octree_base_node.h b/outofcore/include/pcl/outofcore/octree_base_node.h
index c65d8491..dfe1002f 100644
--- a/outofcore/include/pcl/outofcore/octree_base_node.h
+++ b/outofcore/include/pcl/outofcore/octree_base_node.h
@@ -519,7 +519,7 @@ namespace pcl
          *  This could be overloaded with a parallelized implementation
          */
         void
-        sortOctantIndices (const pcl::PCLPointCloud2::Ptr &input_cloud, std::vector< std::vector<int> > &indices, const Eigen::Vector3d &mid_xyz);
+        sortOctantIndices (const pcl::PCLPointCloud2::Ptr &input_cloud, std::vector< pcl::Indices > &indices, const Eigen::Vector3d &mid_xyz);
 
         /** \brief Enlarges the shortest two sidelengths of the
          *  bounding box to a cubic shape; operation is done in
diff --git a/outofcore/include/pcl/outofcore/octree_disk_container.h b/outofcore/include/pcl/outofcore/octree_disk_container.h
index fdf60f0c..70c8083c 100644
--- a/outofcore/include/pcl/outofcore/octree_disk_container.h
+++ b/outofcore/include/pcl/outofcore/octree_disk_container.h
@@ -41,7 +41,6 @@
 
 // C++
 #include <mutex>
-#include <vector>
 #include <string>
 
 // Boost
diff --git a/outofcore/include/pcl/outofcore/octree_ram_container.h b/outofcore/include/pcl/outofcore/octree_ram_container.h
index 92a5dcf3..9894f7b9 100644
--- a/outofcore/include/pcl/outofcore/octree_ram_container.h
+++ b/outofcore/include/pcl/outofcore/octree_ram_container.h
@@ -42,7 +42,6 @@
 // C++
 #include <mutex>
 #include <random>
-#include <vector>
 
 #include <pcl/outofcore/boost.h>
 #include <pcl/outofcore/octree_abstract_node_container.h>
diff --git a/outofcore/include/pcl/outofcore/outofcore_base_data.h b/outofcore/include/pcl/outofcore/outofcore_base_data.h
index 474c692f..7b2123f4 100644
--- a/outofcore/include/pcl/outofcore/outofcore_base_data.h
+++ b/outofcore/include/pcl/outofcore/outofcore_base_data.h
@@ -43,8 +43,6 @@
 #include <pcl/outofcore/boost.h>
 #include <pcl/outofcore/cJSON.h>
 
-#include <pcl/common/eigen.h>
-
 #include <pcl/outofcore/metadata.h>
 
 //standard library
diff --git a/outofcore/include/pcl/outofcore/outofcore_breadth_first_iterator.h b/outofcore/include/pcl/outofcore/outofcore_breadth_first_iterator.h
index 86d84297..14efd5a1 100644
--- a/outofcore/include/pcl/outofcore/outofcore_breadth_first_iterator.h
+++ b/outofcore/include/pcl/outofcore/outofcore_breadth_first_iterator.h
@@ -39,6 +39,9 @@
 #pragma once
 
 #include <pcl/outofcore/outofcore_iterator_base.h>
+
+#include <deque>
+
 namespace pcl
 {
   namespace outofcore
diff --git a/outofcore/outofcore.doxy b/outofcore/outofcore.doxy
index 722c7b3d..94531bd4 100644
--- a/outofcore/outofcore.doxy
+++ b/outofcore/outofcore.doxy
@@ -14,8 +14,8 @@
 
   <b>pcl_outofcore</b> provides an interface to construct and query
   outofcore octrees via OutofcoreOctreeBase. The out of core octree
-  can be used with any PCLPointCloud2 with point types containing ``x'',
-  ``y'' and ``z'' fields. No internal checking is done to verify
+  can be used with any PCLPointCloud2 with point types containing x,
+  y and z fields. No internal checking is done to verify
   this. On the other hand, point clouds do not need to be filtered for
   NaN entries; the library will automatically ignore NaN points in the
   insertion methods.
diff --git a/outofcore/src/outofcore_node_data.cpp b/outofcore/src/outofcore_node_data.cpp
index 39f3c507..e3eb4c94 100644
--- a/outofcore/src/outofcore_node_data.cpp
+++ b/outofcore/src/outofcore_node_data.cpp
@@ -42,11 +42,10 @@
 #include <pcl/console/print.h>
 
 #include <pcl/pcl_macros.h>
-#include <pcl/common/io.h>
+#include <pcl/exceptions.h> // for PCL_THROW_EXCEPTION, PCLException
 
 #include <iostream>
 #include <fstream>
-#include <sstream>
 #include <memory>
 
 namespace pcl
diff --git a/outofcore/tools/CMakeLists.txt b/outofcore/tools/CMakeLists.txt
index 53c63e39..f33e8b44 100644
--- a/outofcore/tools/CMakeLists.txt
+++ b/outofcore/tools/CMakeLists.txt
@@ -13,8 +13,6 @@ if(NOT VTK_FOUND)
 else()
   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")
 
   set(srcs outofcore_viewer.cpp
diff --git a/outofcore/tools/outofcore_print.cpp b/outofcore/tools/outofcore_print.cpp
index da2b6fba..d8a13a5e 100644
--- a/outofcore/tools/outofcore_print.cpp
+++ b/outofcore/tools/outofcore_print.cpp
@@ -41,7 +41,6 @@
 #include <pcl/common/time.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
-#include <pcl/PCLPointCloud2.h>
 
 #include <pcl/io/pcd_io.h>
 #include <pcl/pcl_macros.h>
@@ -57,6 +56,7 @@
 #include <boost/accumulators/statistics/max.hpp>
 #include <boost/accumulators/statistics/mean.hpp>
 #include <boost/accumulators/statistics/variance.hpp>
+#include <boost/algorithm/string/replace.hpp> // for replace_first
 
 namespace ba = boost::accumulators;
 
diff --git a/outofcore/tools/outofcore_process.cpp b/outofcore/tools/outofcore_process.cpp
index d9326690..9f966cb2 100644
--- a/outofcore/tools/outofcore_process.cpp
+++ b/outofcore/tools/outofcore_process.cpp
@@ -308,7 +308,7 @@ main (int argc, char* argv[])
     boost::filesystem::path pcd_path (argv[file_arg_index]);
     if (!boost::filesystem::exists (pcd_path))
     {
-      PCL_WARN ("File %s doesn't exist", pcd_path.string ().c_str ());
+      PCL_WARN ("File %s doesn't exist\n", pcd_path.string ().c_str ());
       continue;
     }
     pcd_paths.push_back (pcd_path);
diff --git a/pcl_config.h.in b/pcl_config.h.in
index e373a8e0..e41128d2 100644
--- a/pcl_config.h.in
+++ b/pcl_config.h.in
@@ -42,8 +42,6 @@
 
 #cmakedefine HAVE_QHULL 1
 
-#cmakedefine HAVE_QHULL_2011 1
-
 #cmakedefine HAVE_CUDA 1
 
 #cmakedefine HAVE_ENSENSO 1
@@ -95,3 +93,5 @@
 /* Version of OpenGL used by VTK as rendering backend */
 #define VTK_RENDERING_BACKEND_OPENGL_VERSION ${VTK_RENDERING_BACKEND_OPENGL_VERSION}
 
+#cmakedefine HAVE_QVTK 1
+
diff --git a/people/CMakeLists.txt b/people/CMakeLists.txt
index 4778e2f1..66cfede8 100644
--- a/people/CMakeLists.txt
+++ b/people/CMakeLists.txt
@@ -8,8 +8,6 @@ if(NOT VTK_FOUND)
 else()
   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()
 
diff --git a/people/apps/main_ground_based_people_detection.cpp b/people/apps/main_ground_based_people_detection.cpp
index d79b442d..4a9f4e8f 100644
--- a/people/apps/main_ground_based_people_detection.cpp
+++ b/people/apps/main_ground_based_people_detection.cpp
@@ -179,7 +179,7 @@ int main (int argc, char** argv)
   // Ground plane estimation:
   Eigen::VectorXf ground_coeffs;
   ground_coeffs.resize(4);
-  std::vector<int> clicked_points_indices;
+  pcl::Indices clicked_points_indices;
   for (std::size_t i = 0; i < clicked_points_3d->size(); i++)
     clicked_points_indices.push_back(i);
   pcl::SampleConsensusModelPlane<PointT> model_plane(clicked_points_3d);
diff --git a/people/include/pcl/people/ground_based_people_detection_app.h b/people/include/pcl/people/ground_based_people_detection_app.h
index f33d258c..40645a6c 100644
--- a/people/include/pcl/people/ground_based_people_detection_app.h
+++ b/people/include/pcl/people/ground_based_people_detection_app.h
@@ -43,10 +43,7 @@
 #include <pcl/point_types.h>
 #include <pcl/sample_consensus/sac_model_plane.h>
 #include <pcl/sample_consensus/ransac.h>
-#include <pcl/filters/extract_indices.h>
-#include <pcl/segmentation/extract_clusters.h>
 #include <pcl/kdtree/kdtree.h>
-#include <pcl/filters/voxel_grid.h>
 #include <pcl/people/person_cluster.h>
 #include <pcl/people/head_based_subcluster.h>
 #include <pcl/people/person_classifier.h>
diff --git a/people/include/pcl/people/impl/ground_based_people_detection_app.hpp b/people/include/pcl/people/impl/ground_based_people_detection_app.hpp
index 96850a42..cc7b23b9 100644
--- a/people/include/pcl/people/impl/ground_based_people_detection_app.hpp
+++ b/people/include/pcl/people/impl/ground_based_people_detection_app.hpp
@@ -42,6 +42,9 @@
 #define PCL_PEOPLE_GROUND_BASED_PEOPLE_DETECTION_APP_HPP_
 
 #include <pcl/people/ground_based_people_detection_app.h>
+#include <pcl/filters/extract_indices.h> // for ExtractIndices
+#include <pcl/segmentation/extract_clusters.h> // for EuclideanClusterExtraction
+#include <pcl/filters/voxel_grid.h> // for VoxelGrid
 
 template <typename PointT>
 pcl::people::GroundBasedPeopleDetectionApp<PointT>::GroundBasedPeopleDetectionApp ()
@@ -356,7 +359,7 @@ pcl::people::GroundBasedPeopleDetectionApp<PointT>::compute (std::vector<pcl::pe
   filter();
 
   // Ground removal and update:
-  pcl::IndicesPtr inliers(new std::vector<int>);
+  pcl::IndicesPtr inliers(new pcl::Indices);
   typename pcl::SampleConsensusModelPlane<PointT>::Ptr ground_model (new pcl::SampleConsensusModelPlane<PointT> (cloud_filtered_));
   ground_model->selectWithinDistance(ground_coeffs_transformed_, 2 * voxel_size_, *inliers);
   no_ground_cloud_ = PointCloudPtr (new PointCloud);
diff --git a/people/include/pcl/people/impl/head_based_subcluster.hpp b/people/include/pcl/people/impl/head_based_subcluster.hpp
index ee453621..4dd2e2f9 100644
--- a/people/include/pcl/people/impl/head_based_subcluster.hpp
+++ b/people/include/pcl/people/impl/head_based_subcluster.hpp
@@ -177,10 +177,9 @@ pcl::people::HeadBasedSubclustering<PointT>::mergeClustersCloseInFloorCoordinate
           if (!used_clusters[cluster])         // if this cluster has not been used yet
           {
             used_clusters[cluster] = true;
-            for(std::vector<int>::const_iterator points_iterator = input_clusters[cluster].getIndices().indices.begin();
-                points_iterator != input_clusters[cluster].getIndices().indices.end(); ++points_iterator)
+            for(const auto& cluster_idx : input_clusters[cluster].getIndices().indices)
             {
-              point_indices.indices.push_back(*points_iterator);
+              point_indices.indices.push_back(cluster_idx);
             }
           }
         }
@@ -200,7 +199,7 @@ pcl::people::HeadBasedSubclustering<PointT>::createSubClusters (pcl::people::Per
   Eigen::Vector3f head_ground_coeffs = ground_coeffs_.head(3);        // ground plane normal (precomputed for speed)
   Eigen::Matrix3Xf maxima_projected(3,maxima_number);                 // matrix containing the projection of maxima onto the ground plane
   Eigen::VectorXi subclusters_number_of_points(maxima_number);        // subclusters number of points
-  std::vector <std::vector <int> > sub_clusters_indices;              // vector of vectors with the cluster indices for every maximum
+  std::vector <pcl::Indices> sub_clusters_indices;                    // vector of vectors with the cluster indices for every maximum
   sub_clusters_indices.resize(maxima_number);                         // resize to number of maxima
 
   // Project maxima on the ground plane:
@@ -214,10 +213,9 @@ pcl::people::HeadBasedSubclustering<PointT>::createSubClusters (pcl::people::Per
   }
 
   // Associate cluster points to one of the maximum:
-  for(std::vector<int>::const_iterator points_iterator = cluster.getIndices().indices.begin(); points_iterator != cluster.getIndices().indices.end(); ++points_iterator)
+  for(const auto& cluster_idx : cluster.getIndices().indices)
   {
-    PointT* current_point = &(*cloud_)[*points_iterator];        // current point cloud point
-    Eigen::Vector3f p_current_eigen(current_point->x, current_point->y, current_point->z);  // conversion to eigen
+    Eigen::Vector3f p_current_eigen((*cloud_)[cluster_idx].x, (*cloud_)[cluster_idx].y, (*cloud_)[cluster_idx].z);  // conversion to eigen
     float t = p_current_eigen.dot(head_ground_coeffs) / normalize_factor;       // height from the ground
     p_current_eigen -= head_ground_coeffs * t;       // projection of the point on the groundplane
 
@@ -228,7 +226,7 @@ pcl::people::HeadBasedSubclustering<PointT>::createSubClusters (pcl::people::Per
       if (((p_current_eigen - maxima_projected.col(i)).norm()) < heads_minimum_distance_)
       {
         correspondence_detected = true;
-        sub_clusters_indices[i].push_back(*points_iterator);
+        sub_clusters_indices[i].push_back(cluster_idx);
         subclusters_number_of_points(i)++;
       }
       else
diff --git a/people/include/pcl/people/impl/height_map_2d.hpp b/people/include/pcl/people/impl/height_map_2d.hpp
index e179b464..05632eb2 100644
--- a/people/include/pcl/people/impl/height_map_2d.hpp
+++ b/people/include/pcl/people/impl/height_map_2d.hpp
@@ -85,9 +85,9 @@ pcl::people::HeightMap2D<PointT>::compute (pcl::people::PersonCluster<PointT>& c
     buckets_.resize(std::size_t((cluster.getMax()(1) - cluster.getMin()(1)) / bin_size_) + 1, 0);
   buckets_cloud_indices_.resize(buckets_.size(), 0);
 
-  for(std::vector<int>::const_iterator pit = cluster.getIndices().indices.begin(); pit != cluster.getIndices().indices.end(); ++pit)
+  for(const auto& cluster_idx : cluster.getIndices().indices)
   {
-    PointT* p = &(*cloud_)[*pit];
+    PointT* p = &(*cloud_)[cluster_idx];
     int index;
     if (!vertical_)    // camera horizontal
       index = int((p->x - cluster.getMin()(0)) / bin_size_);
@@ -103,7 +103,7 @@ pcl::people::HeightMap2D<PointT>::compute (pcl::people::PersonCluster<PointT>& c
       if ((heightp * 60) > buckets_[index])   // compare the height of the new point with the existing one
       {
         buckets_[index] = heightp * 60;   // maximum height
-        buckets_cloud_indices_[index] = *pit;     // point cloud index of the point with maximum height
+        buckets_cloud_indices_[index] = cluster_idx;     // point cloud index of the point with maximum height
       }
     }
   }
diff --git a/people/include/pcl/people/impl/person_cluster.hpp b/people/include/pcl/people/impl/person_cluster.hpp
index 775981f4..2bd07820 100644
--- a/people/include/pcl/people/impl/person_cluster.hpp
+++ b/people/include/pcl/people/impl/person_cluster.hpp
@@ -85,9 +85,9 @@ pcl::people::PersonCluster<PointT>::init (
 
   points_indices_.indices = indices.indices;
 
-  for (std::vector<int>::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); pit++)
+  for (const auto& index : (points_indices_.indices))
   {
-    PointT* p = &(*input_cloud)[*pit];
+    PointT* p = &(*input_cloud)[index];
 
     min_x_ = std::min(p->x, min_x_);
     max_x_ = std::max(p->x, max_x_);
@@ -136,9 +136,9 @@ pcl::people::PersonCluster<PointT>::init (
     if (!vertical_)
     {
       head_threshold_value = min_y_ + height_ / 8.0f;    // head is suppose to be 1/8 of the human height
-      for (std::vector<int>::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); pit++)
+      for (const auto& index : (points_indices_.indices))
       {
-        PointT* p = &(*input_cloud)[*pit];
+        PointT* p = &(*input_cloud)[index];
 
         if(p->y < head_threshold_value)
         {
@@ -152,9 +152,9 @@ pcl::people::PersonCluster<PointT>::init (
     else
     {
       head_threshold_value = max_x_ - height_ / 8.0f;    // head is suppose to be 1/8 of the human height
-      for (std::vector<int>::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); pit++)
+      for (const auto& index : (points_indices_.indices))
       {
-        PointT* p = &(*input_cloud)[*pit];
+        PointT* p = &(*input_cloud)[index];
 
         if(p->x > head_threshold_value)
         {
@@ -177,9 +177,9 @@ pcl::people::PersonCluster<PointT>::init (
     float min_z = c_z_;
     float max_x = c_x_;
     float max_z = c_z_;
-    for (std::vector<int>::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); ++pit)
+    for (const auto& index : (points_indices_.indices))
     {
-      PointT* p = &(*input_cloud)[*pit];
+      PointT* p = &(*input_cloud)[index];
 
       min_x = std::min(p->x, min_x);
       max_x = std::max(p->x, max_x);
@@ -216,9 +216,9 @@ pcl::people::PersonCluster<PointT>::init (
     float min_z = c_z_;
     float max_y = c_y_;
     float max_z = c_z_;
-    for (std::vector<int>::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); ++pit)
+    for (const auto& index : (points_indices_.indices))
     {
-      PointT* p = &(*input_cloud)[*pit];
+      PointT* p = &(*input_cloud)[index];
 
       min_y = std::min(p->y, min_y);
       max_y = std::max(p->y, max_y);
@@ -409,20 +409,11 @@ void pcl::people::PersonCluster<PointT>::drawTBoundingBox (pcl::visualization::P
     coeffs.values.push_back (0.5);
   }
 
-  std::stringstream bbox_name;
-  bbox_name << "bbox_person_" << person_number;
-  viewer.removeShape (bbox_name.str());
-  viewer.addCube (coeffs, bbox_name.str());
-  viewer.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, bbox_name.str());
-  viewer.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 2, bbox_name.str());
-
-  //      std::stringstream confid;
-  //      confid << person_confidence_;
-  //      PointT position;
-  //      position.x = tcenter_[0]- 0.2;
-  //      position.y = ttop_[1];
-  //      position.z = tcenter_[2];
-  //      viewer.addText3D(confid.str().substr(0, 4), position, 0.1);
+  const std::string bbox_name = "bbox_person_" + std::to_string(person_number);
+  viewer.removeShape (bbox_name);
+  viewer.addCube (coeffs, bbox_name);
+  viewer.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, bbox_name);
+  viewer.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 2, bbox_name);
 }
 
 template <typename PointT>
diff --git a/recognition/CMakeLists.txt b/recognition/CMakeLists.txt
index 54d357e6..7e9386e7 100644
--- a/recognition/CMakeLists.txt
+++ b/recognition/CMakeLists.txt
@@ -38,20 +38,20 @@ set(incs
   "include/pcl/${SUBSYS_NAME}/dense_quantized_multi_mod_template.h"
   "include/pcl/${SUBSYS_NAME}/sparse_quantized_multi_mod_template.h"
   "include/pcl/${SUBSYS_NAME}/surface_normal_modality.h"
-  "include/pcl/${SUBSYS_NAME}/linemod/line_rgbd.h"
+  "include/pcl/${SUBSYS_NAME}/line_rgbd.h"
   "include/pcl/${SUBSYS_NAME}/implicit_shape_model.h"
-  "include/pcl/${SUBSYS_NAME}/ransac_based/auxiliary.h"
-  "include/pcl/${SUBSYS_NAME}/ransac_based/hypothesis.h"
-  "include/pcl/${SUBSYS_NAME}/ransac_based/model_library.h"
-  "include/pcl/${SUBSYS_NAME}/ransac_based/rigid_transform_space.h"
-  "include/pcl/${SUBSYS_NAME}/ransac_based/obj_rec_ransac.h"
-  "include/pcl/${SUBSYS_NAME}/ransac_based/orr_graph.h"
-  "include/pcl/${SUBSYS_NAME}/ransac_based/orr_octree_zprojection.h"
-  "include/pcl/${SUBSYS_NAME}/ransac_based/trimmed_icp.h"
-  "include/pcl/${SUBSYS_NAME}/ransac_based/orr_octree.h"
-  "include/pcl/${SUBSYS_NAME}/ransac_based/simple_octree.h"
-  "include/pcl/${SUBSYS_NAME}/ransac_based/voxel_structure.h"
-  "include/pcl/${SUBSYS_NAME}/ransac_based/bvh.h"
+  "include/pcl/${SUBSYS_NAME}/auxiliary.h"
+  "include/pcl/${SUBSYS_NAME}/hypothesis.h"
+  "include/pcl/${SUBSYS_NAME}/model_library.h"
+  "include/pcl/${SUBSYS_NAME}/rigid_transform_space.h"
+  "include/pcl/${SUBSYS_NAME}/obj_rec_ransac.h"
+  "include/pcl/${SUBSYS_NAME}/orr_graph.h"
+  "include/pcl/${SUBSYS_NAME}/orr_octree_zprojection.h"
+  "include/pcl/${SUBSYS_NAME}/trimmed_icp.h"
+  "include/pcl/${SUBSYS_NAME}/orr_octree.h"
+  "include/pcl/${SUBSYS_NAME}/simple_octree.h"
+  "include/pcl/${SUBSYS_NAME}/voxel_structure.h"
+  "include/pcl/${SUBSYS_NAME}/bvh.h"
 )
 
 set(ransac_based_incs
@@ -91,9 +91,9 @@ set(cg_incs
 )
 
 set(impl_incs
-  "include/pcl/${SUBSYS_NAME}/impl/linemod/line_rgbd.hpp"
-  "include/pcl/${SUBSYS_NAME}/impl/ransac_based/simple_octree.hpp"
-  "include/pcl/${SUBSYS_NAME}/impl/ransac_based/voxel_structure.hpp"
+  "include/pcl/${SUBSYS_NAME}/impl/line_rgbd.hpp"
+  "include/pcl/${SUBSYS_NAME}/impl/simple_octree.hpp"
+  "include/pcl/${SUBSYS_NAME}/impl/voxel_structure.hpp"
   "include/pcl/${SUBSYS_NAME}/impl/implicit_shape_model.hpp"
 )
 
diff --git a/recognition/include/pcl/recognition/3rdparty/metslib/README.md b/recognition/include/pcl/recognition/3rdparty/metslib/README.md
new file mode 100644
index 00000000..bc268bbb
--- /dev/null
+++ b/recognition/include/pcl/recognition/3rdparty/metslib/README.md
@@ -0,0 +1,15 @@
+# License
+
+PCL carries a custom (old) version of METSlib which was added (after relicensing for compatibility with PCL) under BSD 3 clause. The details for the addition are:
+* author: Mirko Maischberger <mirko.maischberger@gmail.com>
+* commit: e4d6501af048215ce84b4ee436ff0e18dba2d30d
+* URL: https://projects.coin-or.org/metslib
+
+This has been modified to work with system installed METSlib as well.
+Though METSlib is available primarily under GPLv3, it is dually licensed to be available under EPL-1.0 (a commercial friendly license) as well.
+
+# TL;DR
+This implies the following:
+* While using PCL with the METSlib shipped with it, everything is licensed under BSD
+* While using PCL with system installed METSlib, PCL adheres to the API available under the EPL-1.0 license
+* PCL doesn't use the system installed METSlib released under GPLv3 for testing/reference/linking
diff --git a/recognition/include/pcl/recognition/3rdparty/metslib/model.hh b/recognition/include/pcl/recognition/3rdparty/metslib/model.hh
index 6d9a35e6..7cb742ad 100644
--- a/recognition/include/pcl/recognition/3rdparty/metslib/model.hh
+++ b/recognition/include/pcl/recognition/3rdparty/metslib/model.hh
@@ -270,7 +270,7 @@ namespace mets {
     std::tr1::variate_generator<random_generator&, 
       std::tr1::uniform_int<std::size_t> >gen(rng, unigen);
 #endif
-    std::random_shuffle(p.pi_m.begin(), p.pi_m.end(), gen);
+    std::shuffle(p.pi_m.begin(), p.pi_m.end(), gen);
     p.update_cost();
   }
   
diff --git a/recognition/include/pcl/recognition/auxiliary.h b/recognition/include/pcl/recognition/auxiliary.h
new file mode 100644
index 00000000..0e364add
--- /dev/null
+++ b/recognition/include/pcl/recognition/auxiliary.h
@@ -0,0 +1,2 @@
+#include <pcl/recognition/ransac_based/auxiliary.h>
+PCL_DEPRECATED_HEADER(1, 15, "Please use <pcl/recognition/ransac_based/auxiliary.h> instead")
diff --git a/recognition/include/pcl/recognition/boost.h b/recognition/include/pcl/recognition/boost.h
index f129eda2..44e705be 100644
--- a/recognition/include/pcl/recognition/boost.h
+++ b/recognition/include/pcl/recognition/boost.h
@@ -42,5 +42,6 @@
 #if defined __GNUC__
 #  pragma GCC system_header 
 #endif
+PCL_DEPRECATED_HEADER(1, 15, "Please include the needed boost headers directly.")
 
 #include <boost/graph/graph_traits.hpp>
diff --git a/recognition/include/pcl/recognition/bvh.h b/recognition/include/pcl/recognition/bvh.h
new file mode 100644
index 00000000..12374a20
--- /dev/null
+++ b/recognition/include/pcl/recognition/bvh.h
@@ -0,0 +1,2 @@
+#include <pcl/recognition/ransac_based/bvh.h>
+PCL_DEPRECATED_HEADER(1, 15, "Please use <pcl/recognition/ransac_based/bvh.h> instead")
diff --git a/recognition/include/pcl/recognition/cg/hough_3d.h b/recognition/include/pcl/recognition/cg/hough_3d.h
index 883f8dc8..cd18a78b 100644
--- a/recognition/include/pcl/recognition/cg/hough_3d.h
+++ b/recognition/include/pcl/recognition/cg/hough_3d.h
@@ -40,7 +40,6 @@
 #pragma once
 
 #include <pcl/recognition/cg/correspondence_grouping.h>
-#include <pcl/recognition/boost.h>
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
 #include <pcl/point_types.h>
diff --git a/recognition/include/pcl/recognition/color_gradient_dot_modality.h b/recognition/include/pcl/recognition/color_gradient_dot_modality.h
index eec3290c..dc4c9f14 100644
--- a/recognition/include/pcl/recognition/color_gradient_dot_modality.h
+++ b/recognition/include/pcl/recognition/color_gradient_dot_modality.h
@@ -174,7 +174,7 @@ computeMaxColorGradients ()
   const int width = input_->width;
   const int height = input_->height;
 
-  color_gradients_.points.resize (width*height);
+  color_gradients_.resize (width*height);
   color_gradients_.width = width;
   color_gradients_.height = height;
 
diff --git a/recognition/include/pcl/recognition/color_gradient_modality.h b/recognition/include/pcl/recognition/color_gradient_modality.h
index 0839ddd2..52bf3927 100644
--- a/recognition/include/pcl/recognition/color_gradient_modality.h
+++ b/recognition/include/pcl/recognition/color_gradient_modality.h
@@ -750,7 +750,7 @@ computeMaxColorGradients (const typename pcl::PointCloud<pcl::RGB>::ConstPtr & c
   const int width = cloud->width;
   const int height = cloud->height;
 
-  color_gradients_.points.resize (width*height);
+  color_gradients_.resize (width*height);
   color_gradients_.width = width;
   color_gradients_.height = height;
 
@@ -837,7 +837,7 @@ computeMaxColorGradientsSobel (const typename pcl::PointCloud<pcl::RGB>::ConstPt
   const int width = cloud->width;
   const int height = cloud->height;
 
-  color_gradients_.points.resize (width*height);
+  color_gradients_.resize (width*height);
   color_gradients_.width = width;
   color_gradients_.height = height;
 
diff --git a/recognition/include/pcl/recognition/dot_modality.h b/recognition/include/pcl/recognition/dot_modality.h
index f07106e3..f177f032 100644
--- a/recognition/include/pcl/recognition/dot_modality.h
+++ b/recognition/include/pcl/recognition/dot_modality.h
@@ -37,7 +37,6 @@
 
 #pragma once
 
-#include <vector>
 #include <pcl/pcl_macros.h>
 #include <pcl/recognition/quantized_map.h>
 #include <pcl/recognition/mask_map.h>
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 12e895f9..fa533b09 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
@@ -96,7 +96,7 @@ namespace pcl
         {
           cloud_out.width = max_col - min_col + 1;
           cloud_out.height = max_row - min_row + 1;
-          cloud_out.points.resize (cloud_out.width * cloud_out.height);
+          cloud_out.resize (cloud_out.width * cloud_out.height);
           for (unsigned int u = 0; u < cloud_out.width; u++)
           {
             for (unsigned int v = 0; v < cloud_out.height; v++)
diff --git a/recognition/include/pcl/recognition/hv/greedy_verification.h b/recognition/include/pcl/recognition/hv/greedy_verification.h
index a06bb68b..40c874d7 100644
--- a/recognition/include/pcl/recognition/hv/greedy_verification.h
+++ b/recognition/include/pcl/recognition/hv/greedy_verification.h
@@ -38,7 +38,6 @@
 
 #include <pcl/pcl_macros.h>
 #include <pcl/recognition/hv/hypotheses_verification.h>
-#include <pcl/common/common.h>
 
 #include <memory>
 
diff --git a/recognition/include/pcl/recognition/hv/hv_go.h b/recognition/include/pcl/recognition/hv/hv_go.h
index e39f3eba..1fed0034 100644
--- a/recognition/include/pcl/recognition/hv/hv_go.h
+++ b/recognition/include/pcl/recognition/hv/hv_go.h
@@ -19,7 +19,6 @@
 
 #include <pcl/pcl_macros.h>
 #include <pcl/recognition/hv/hypotheses_verification.h>
-#include <pcl/common/common.h>
 #include <pcl/recognition/3rdparty/metslib/mets.hh>
 #include <pcl/features/normal_3d.h>
 
diff --git a/recognition/include/pcl/recognition/hv/hv_papazov.h b/recognition/include/pcl/recognition/hv/hv_papazov.h
index d24f1c70..6d05006f 100644
--- a/recognition/include/pcl/recognition/hv/hv_papazov.h
+++ b/recognition/include/pcl/recognition/hv/hv_papazov.h
@@ -36,9 +36,7 @@
 
 #pragma once
 
-#include <pcl/recognition/boost.h>
 #include <pcl/pcl_macros.h>
-#include <pcl/common/common.h>
 #include <pcl/recognition/hv/hypotheses_verification.h>
 #include <boost/graph/adjacency_list.hpp>
 
diff --git a/recognition/include/pcl/recognition/hv/hypotheses_verification.h b/recognition/include/pcl/recognition/hv/hypotheses_verification.h
index 9a7bf809..f6977ae9 100644
--- a/recognition/include/pcl/recognition/hv/hypotheses_verification.h
+++ b/recognition/include/pcl/recognition/hv/hypotheses_verification.h
@@ -39,7 +39,6 @@
 #include <pcl/pcl_macros.h>
 #include "pcl/recognition/hv/occlusion_reasoning.h"
 #include "pcl/recognition/impl/hv/occlusion_reasoning.hpp"
-#include <pcl/common/common.h>
 #include <pcl/search/kdtree.h>
 #include <pcl/filters/voxel_grid.h>
 
@@ -232,7 +231,7 @@ namespace pcl
         //we need to reason about occlusions before setting the model
         if (scene_cloud_ == nullptr)
         {
-          PCL_ERROR("setSceneCloud should be called before adding the model if reasoning about occlusions...");
+          PCL_ERROR("setSceneCloud should be called before adding the model if reasoning about occlusions...\n");
         }
 
         if (!occlusion_cloud_->isOrganized ())
@@ -253,7 +252,7 @@ namespace pcl
           typename pcl::PointCloud<ModelT>::Ptr filtered (new pcl::PointCloud<ModelT> ());
           typename pcl::occlusion_reasoning::ZBuffering<ModelT, SceneT> zbuffer_self_occlusion (75, 75, 1.f);
           zbuffer_self_occlusion.computeDepthMap (models[i], true);
-          std::vector<int> indices;
+          pcl::Indices indices;
           zbuffer_self_occlusion.filter (models[i], indices, 0.005f);
           pcl::copyPointCloud (*models[i], indices, *filtered);
 
diff --git a/recognition/include/pcl/recognition/hv/occlusion_reasoning.h b/recognition/include/pcl/recognition/hv/occlusion_reasoning.h
index f6887a90..57f841fd 100644
--- a/recognition/include/pcl/recognition/hv/occlusion_reasoning.h
+++ b/recognition/include/pcl/recognition/hv/occlusion_reasoning.h
@@ -36,8 +36,6 @@
 
 #pragma once
 
-#include <pcl/common/common.h>
-#include <pcl/common/transforms.h>
 #include <pcl/common/io.h>
 
 namespace pcl
@@ -67,7 +65,7 @@ namespace pcl
         computeDepthMap (typename pcl::PointCloud<SceneT>::ConstPtr & scene, bool compute_focal = false, bool smooth = false, int wsize = 3);
         void
         filter (typename pcl::PointCloud<ModelT>::ConstPtr & model, typename pcl::PointCloud<ModelT>::Ptr & filtered, float thres = 0.01);
-        void filter (typename pcl::PointCloud<ModelT>::ConstPtr & model, std::vector<int> & indices, float thres = 0.01);
+        void filter (typename pcl::PointCloud<ModelT>::ConstPtr & model, pcl::Indices & indices, float thres = 0.01);
       };
 
     template<typename ModelT, typename SceneT> typename pcl::PointCloud<ModelT>::Ptr
@@ -78,7 +76,7 @@ namespace pcl
       float cy = (static_cast<float> (organized_cloud->height) / 2.f - 0.5f);
       typename pcl::PointCloud<ModelT>::Ptr filtered (new pcl::PointCloud<ModelT> ());
 
-      std::vector<int> indices_to_keep;
+      pcl::Indices indices_to_keep;
       indices_to_keep.resize (to_be_filtered->size ());
 
       int keep = 0;
diff --git a/recognition/include/pcl/recognition/hypothesis.h b/recognition/include/pcl/recognition/hypothesis.h
new file mode 100644
index 00000000..ad8f5b64
--- /dev/null
+++ b/recognition/include/pcl/recognition/hypothesis.h
@@ -0,0 +1,2 @@
+#include <pcl/recognition/ransac_based/hypothesis.h>
+PCL_DEPRECATED_HEADER(1, 15, "Please use <pcl/recognition/ransac_based/hypothesis.h> instead")
diff --git a/recognition/include/pcl/recognition/impl/cg/hough_3d.hpp b/recognition/include/pcl/recognition/impl/cg/hough_3d.hpp
index ce0d4b63..bac7e9f9 100644
--- a/recognition/include/pcl/recognition/impl/cg/hough_3d.hpp
+++ b/recognition/include/pcl/recognition/impl/cg/hough_3d.hpp
@@ -40,6 +40,7 @@
 #ifndef PCL_RECOGNITION_HOUGH_3D_IMPL_H_
 #define PCL_RECOGNITION_HOUGH_3D_IMPL_H_
 
+#include <pcl/common/io.h> // for copyPointCloud
 #include <pcl/recognition/cg/hough_3d.h>
 #include <pcl/registration/correspondence_types.h>
 #include <pcl/registration/correspondence_rejection_sample_consensus.h>
diff --git a/recognition/include/pcl/recognition/impl/hv/greedy_verification.hpp b/recognition/include/pcl/recognition/impl/hv/greedy_verification.hpp
index 3a2690e8..9c30dad6 100644
--- a/recognition/include/pcl/recognition/impl/hv/greedy_verification.hpp
+++ b/recognition/include/pcl/recognition/impl/hv/greedy_verification.hpp
@@ -68,7 +68,7 @@ template<typename ModelT, typename SceneT>
 
       std::vector<int> explained_indices;
       std::vector<int> outliers;
-      std::vector<int> nn_indices;
+      pcl::Indices nn_indices;
       std::vector<float> nn_distances;
 
       for (std::size_t i = 0; i < recog_model->cloud_->size (); i++)
diff --git a/recognition/include/pcl/recognition/impl/hv/hv_go.hpp b/recognition/include/pcl/recognition/impl/hv/hv_go.hpp
index 8f44d987..51cff240 100644
--- a/recognition/include/pcl/recognition/impl/hv/hv_go.hpp
+++ b/recognition/include/pcl/recognition/impl/hv/hv_go.hpp
@@ -38,6 +38,7 @@
 #define PCL_RECOGNITION_IMPL_HV_GO_HPP_
 
 #include <pcl/recognition/hv/hv_go.h>
+#include <pcl/common/common.h> // for getMinMax3D
 #include <pcl/common/time.h>
 #include <pcl/point_types.h>
 
@@ -64,7 +65,7 @@ inline void extractEuclideanClustersSmooth(const typename pcl::PointCloud<PointT
   // Create a bool vector of processed point indices, and initialize it to false
   std::vector<bool> processed (cloud.size (), false);
 
-  std::vector<int> nn_indices;
+  pcl::Indices nn_indices;
   std::vector<float> nn_distances;
   // Process all points in the indices vector
   int size = static_cast<int> (cloud.size ());
@@ -173,7 +174,7 @@ mets::gol_type pcl::GlobalHypothesesVerification<ModelT, SceneT>::evaluateSoluti
   setPreviousBadInfo (bad_info);
 
   int n_active_hyp = 0;
-  for(const bool &i : active) {
+  for(const bool i : active) {
     if(i)
       n_active_hyp++;
   }
@@ -565,7 +566,7 @@ bool pcl::GlobalHypothesesVerification<ModelT, SceneT>::addModel(typename pcl::P
   std::vector<float> outliers_weight;
   std::vector<float> explained_indices_distances;
 
-  std::vector<int> nn_indices;
+  pcl::Indices nn_indices;
   std::vector<float> nn_distances;
 
   std::map<int, std::shared_ptr<std::vector<std::pair<int, float>>>> model_explains_scene_points; //which point i from the scene is explained by a points j_k with dist d_k from the model
@@ -608,7 +609,7 @@ bool pcl::GlobalHypothesesVerification<ModelT, SceneT>::addModel(typename pcl::P
   if (outliers_weight.empty ())
     recog_model->outliers_weight_ = 1.f;
 
-  pcl::IndicesPtr indices_scene (new std::vector<int>);
+  pcl::IndicesPtr indices_scene (new pcl::Indices);
   //go through the map and keep the closest model point in case that several model points explain a scene point
 
   int p = 0;
@@ -658,11 +659,11 @@ void pcl::GlobalHypothesesVerification<ModelT, SceneT>::computeClutterCue(Recogn
   {
 
     float rn_sqr = radius_neighborhood_GO_ * radius_neighborhood_GO_;
-    std::vector<int> nn_indices;
+    pcl::Indices nn_indices;
     std::vector<float> nn_distances;
 
     std::vector < std::pair<int, int> > neighborhood_indices; //first is indices to scene point and second is indices to explained_ scene points
-    for (int i = 0; i < static_cast<int> (recog_model->explained_.size ()); i++)
+    for (pcl::index_t i = 0; i < static_cast<pcl::index_t> (recog_model->explained_.size ()); i++)
     {
       if (scene_downsampled_tree_->radiusSearch ((*scene_cloud_downsampled_)[recog_model->explained_[i]], radius_neighborhood_GO_, nn_indices,
           nn_distances, std::numeric_limits<int>::max ()))
diff --git a/recognition/include/pcl/recognition/impl/hv/hv_papazov.hpp b/recognition/include/pcl/recognition/impl/hv/hv_papazov.hpp
index 70b4f30e..3efbae00 100644
--- a/recognition/include/pcl/recognition/impl/hv/hv_papazov.hpp
+++ b/recognition/include/pcl/recognition/impl/hv/hv_papazov.hpp
@@ -80,7 +80,7 @@ template<typename ModelT, typename SceneT>
 
       std::vector<int> explained_indices;
       std::vector<int> outliers;
-      std::vector<int> nn_indices;
+      pcl::Indices nn_indices;
       std::vector<float> nn_distances;
 
       for (std::size_t i = 0; i < recog_model->cloud_->size (); i++)
diff --git a/recognition/include/pcl/recognition/impl/hv/occlusion_reasoning.hpp b/recognition/include/pcl/recognition/impl/hv/occlusion_reasoning.hpp
index 3ec762b5..1a314b07 100644
--- a/recognition/include/pcl/recognition/impl/hv/occlusion_reasoning.hpp
+++ b/recognition/include/pcl/recognition/impl/hv/occlusion_reasoning.hpp
@@ -67,7 +67,7 @@ template<typename ModelT, typename SceneT> void
 pcl::occlusion_reasoning::ZBuffering<ModelT, SceneT>::filter (typename pcl::PointCloud<ModelT>::ConstPtr & model,
                                                               typename pcl::PointCloud<ModelT>::Ptr & filtered, float thres)
 {
-  std::vector<int> indices_to_keep;
+  pcl::Indices indices_to_keep;
   filter(model, indices_to_keep, thres);
   pcl::copyPointCloud (*model, indices_to_keep, *filtered);
 }
@@ -75,7 +75,7 @@ pcl::occlusion_reasoning::ZBuffering<ModelT, SceneT>::filter (typename pcl::Poin
 ///////////////////////////////////////////////////////////////////////////////////////////
 template<typename ModelT, typename SceneT> void
 pcl::occlusion_reasoning::ZBuffering<ModelT, SceneT>::filter (typename pcl::PointCloud<ModelT>::ConstPtr & model,
-                                                                      std::vector<int> & indices_to_keep, float thres)
+                                                                      pcl::Indices & indices_to_keep, float thres)
 {
 
   float cx, cy;
diff --git a/recognition/include/pcl/recognition/impl/implicit_shape_model.hpp b/recognition/include/pcl/recognition/impl/implicit_shape_model.hpp
index d4a34e28..3caef81e 100644
--- a/recognition/include/pcl/recognition/impl/implicit_shape_model.hpp
+++ b/recognition/include/pcl/recognition/impl/implicit_shape_model.hpp
@@ -42,6 +42,8 @@
 #define PCL_IMPLICIT_SHAPE_MODEL_HPP_
 
 #include "../implicit_shape_model.h"
+#include <pcl/filters/voxel_grid.h> // for VoxelGrid
+#include <pcl/filters/extract_indices.h> // for ExtractIndices
 
 #include <pcl/memory.h>  // for dynamic_pointer_cast
 
diff --git a/recognition/include/pcl/recognition/impl/line_rgbd.hpp b/recognition/include/pcl/recognition/impl/line_rgbd.hpp
new file mode 100644
index 00000000..53d67119
--- /dev/null
+++ b/recognition/include/pcl/recognition/impl/line_rgbd.hpp
@@ -0,0 +1,2 @@
+#include <pcl/recognition/impl/linemod/line_rgbd.hpp>
+PCL_DEPRECATED_HEADER(1, 15, "Please use <pcl/recognition/impl/linemod/line_rgbd.hpp> instead")
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 08fef7f8..bd2e6436 100644
--- a/recognition/include/pcl/recognition/impl/ransac_based/simple_octree.hpp
+++ b/recognition/include/pcl/recognition/impl/ransac_based/simple_octree.hpp
@@ -8,7 +8,6 @@
 #ifndef SIMPLE_OCTREE_HPP_
 #define SIMPLE_OCTREE_HPP_
 
-#include <algorithm>
 #include <cmath>
 
 
diff --git a/recognition/include/pcl/recognition/impl/simple_octree.hpp b/recognition/include/pcl/recognition/impl/simple_octree.hpp
new file mode 100644
index 00000000..0c8c0ccb
--- /dev/null
+++ b/recognition/include/pcl/recognition/impl/simple_octree.hpp
@@ -0,0 +1,2 @@
+#include <pcl/recognition/impl/ransac_based/simple_octree.hpp>
+PCL_DEPRECATED_HEADER(1, 15, "Please use <pcl/recognition/impl/ransac_based/simple_octree.hpp> instead")
diff --git a/recognition/include/pcl/recognition/impl/voxel_structure.hpp b/recognition/include/pcl/recognition/impl/voxel_structure.hpp
new file mode 100644
index 00000000..44697e8e
--- /dev/null
+++ b/recognition/include/pcl/recognition/impl/voxel_structure.hpp
@@ -0,0 +1,2 @@
+#include <pcl/recognition/impl/ransac_based/voxel_structure.hpp>
+PCL_DEPRECATED_HEADER(1, 15, "Please use <pcl/recognition/impl/ransac_based/voxel_structure.hpp> instead")
diff --git a/recognition/include/pcl/recognition/implicit_shape_model.h b/recognition/include/pcl/recognition/implicit_shape_model.h
index 44e08583..1671fe82 100644
--- a/recognition/include/pcl/recognition/implicit_shape_model.h
+++ b/recognition/include/pcl/recognition/implicit_shape_model.h
@@ -37,19 +37,14 @@
 
 #include <vector>
 #include <fstream>
-#include <limits>
 #include <Eigen/src/Core/Matrix.h>
 #include <pcl/memory.h>
-#include <pcl/pcl_base.h>
 #include <pcl/pcl_macros.h>
 #include <pcl/point_types.h>
 #include <pcl/point_representation.h>
 #include <pcl/features/feature.h>
 #include <pcl/features/spin_image.h>
-#include <pcl/filters/voxel_grid.h>
-#include <pcl/filters/extract_indices.h>
-#include <pcl/search/search.h>
-#include <pcl/kdtree/kdtree.h>
+#include <pcl/kdtree/kdtree_flann.h> // for KdTreeFLANN
 
 namespace pcl
 {
@@ -149,7 +144,7 @@ namespace pcl
         pcl::KdTreeFLANN<pcl::InterestPoint>::Ptr tree_;
 
         /** \brief Stores neighbours indices. */
-        std::vector<int> k_ind_;
+        pcl::Indices k_ind_;
 
         /** \brief Stores square distances to the corresponding neighbours. */
         std::vector<float> k_sqr_dist_;
diff --git a/recognition/include/pcl/recognition/line_rgbd.h b/recognition/include/pcl/recognition/line_rgbd.h
new file mode 100644
index 00000000..5d526ad3
--- /dev/null
+++ b/recognition/include/pcl/recognition/line_rgbd.h
@@ -0,0 +1,2 @@
+#include <pcl/recognition/linemod/line_rgbd.h>
+PCL_DEPRECATED_HEADER(1, 15, "Please use <pcl/recognition/linemod/line_rgbd.h> instead")
diff --git a/recognition/include/pcl/recognition/mask_map.h b/recognition/include/pcl/recognition/mask_map.h
index a49bb560..2e1fe810 100644
--- a/recognition/include/pcl/recognition/mask_map.h
+++ b/recognition/include/pcl/recognition/mask_map.h
@@ -77,10 +77,6 @@ public:
     return (data_.data());
   }
 
-  PCL_DEPRECATED(1, 12, "Use new version diff getDifferenceMask(mask0, mask1)")
-  static void
-  getDifferenceMask(const MaskMap& mask0, const MaskMap& mask1, MaskMap& diff_mask);
-
   PCL_NODISCARD
   static MaskMap
   getDifferenceMask(const MaskMap& mask0, const MaskMap& mask1);
diff --git a/recognition/include/pcl/recognition/model_library.h b/recognition/include/pcl/recognition/model_library.h
new file mode 100644
index 00000000..4b4ee334
--- /dev/null
+++ b/recognition/include/pcl/recognition/model_library.h
@@ -0,0 +1,2 @@
+#include <pcl/recognition/ransac_based/model_library.h>
+PCL_DEPRECATED_HEADER(1, 15, "Please use <pcl/recognition/ransac_based/model_library.h> instead")
diff --git a/recognition/include/pcl/recognition/obj_rec_ransac.h b/recognition/include/pcl/recognition/obj_rec_ransac.h
new file mode 100644
index 00000000..52b96362
--- /dev/null
+++ b/recognition/include/pcl/recognition/obj_rec_ransac.h
@@ -0,0 +1,2 @@
+#include <pcl/recognition/ransac_based/obj_rec_ransac.h>
+PCL_DEPRECATED_HEADER(1, 15, "Please use <pcl/recognition/ransac_based/obj_rec_ransac.h> instead")
diff --git a/recognition/include/pcl/recognition/orr_graph.h b/recognition/include/pcl/recognition/orr_graph.h
new file mode 100644
index 00000000..a43e35c9
--- /dev/null
+++ b/recognition/include/pcl/recognition/orr_graph.h
@@ -0,0 +1,2 @@
+#include <pcl/recognition/ransac_based/orr_graph.h>
+PCL_DEPRECATED_HEADER(1, 15, "Please use <pcl/recognition/ransac_based/orr_graph.h> instead")
diff --git a/recognition/include/pcl/recognition/orr_octree.h b/recognition/include/pcl/recognition/orr_octree.h
new file mode 100644
index 00000000..b0f43fc2
--- /dev/null
+++ b/recognition/include/pcl/recognition/orr_octree.h
@@ -0,0 +1,2 @@
+#include <pcl/recognition/ransac_based/orr_octree.h>
+PCL_DEPRECATED_HEADER(1, 15, "Please use <pcl/recognition/ransac_based/orr_octree.h> instead")
diff --git a/recognition/include/pcl/recognition/orr_octree_zprojection.h b/recognition/include/pcl/recognition/orr_octree_zprojection.h
new file mode 100644
index 00000000..536f8a53
--- /dev/null
+++ b/recognition/include/pcl/recognition/orr_octree_zprojection.h
@@ -0,0 +1,2 @@
+#include <pcl/recognition/ransac_based/orr_octree_zprojection.h>
+PCL_DEPRECATED_HEADER(1, 15, "Please use <pcl/recognition/ransac_based/orr_octree_zprojection.h> instead")
diff --git a/recognition/include/pcl/recognition/ransac_based/auxiliary.h b/recognition/include/pcl/recognition/ransac_based/auxiliary.h
index 0a553edb..a597ea60 100644
--- a/recognition/include/pcl/recognition/ransac_based/auxiliary.h
+++ b/recognition/include/pcl/recognition/ransac_based/auxiliary.h
@@ -37,9 +37,8 @@
 
 #pragma once
 
-#include <cmath>
-#include <cstdlib>
-#include <pcl/common/eigen.h>
+#include <Eigen/Core> // for Matrix
+#include <Eigen/Geometry> // for AngleAxis
 #include <pcl/point_types.h>
 
 #define AUX_PI_FLOAT            3.14159265358979323846f
diff --git a/recognition/include/pcl/recognition/ransac_based/obj_rec_ransac.h b/recognition/include/pcl/recognition/ransac_based/obj_rec_ransac.h
index 2a28c8d3..fb1b6f3b 100644
--- a/recognition/include/pcl/recognition/ransac_based/obj_rec_ransac.h
+++ b/recognition/include/pcl/recognition/ransac_based/obj_rec_ransac.h
@@ -48,8 +48,6 @@
 #include <pcl/recognition/ransac_based/orr_graph.h>
 #include <pcl/recognition/ransac_based/auxiliary.h>
 #include <pcl/recognition/ransac_based/bvh.h>
-#include <pcl/registration/transformation_estimation_svd.h>
-#include <pcl/correspondence.h>
 #include <pcl/pcl_exports.h>
 #include <pcl/point_cloud.h>
 #include <cmath>
diff --git a/recognition/include/pcl/recognition/ransac_based/orr_octree.h b/recognition/include/pcl/recognition/ransac_based/orr_octree.h
index d43aabd2..58d9781a 100644
--- a/recognition/include/pcl/recognition/ransac_based/orr_octree.h
+++ b/recognition/include/pcl/recognition/ransac_based/orr_octree.h
@@ -49,8 +49,6 @@
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
 #include <pcl/pcl_exports.h>
-#include <cstdlib>
-#include <ctime>
 #include <vector>
 #include <list>
 #include <set>
diff --git a/recognition/include/pcl/recognition/ransac_based/trimmed_icp.h b/recognition/include/pcl/recognition/ransac_based/trimmed_icp.h
index 1d4ee835..8fa2a820 100644
--- a/recognition/include/pcl/recognition/ransac_based/trimmed_icp.h
+++ b/recognition/include/pcl/recognition/ransac_based/trimmed_icp.h
@@ -113,7 +113,7 @@ namespace pcl
 
           // Some variables for the closest point search
           pcl::PointXYZ transformed_source_point;
-          std::vector<int> target_index (1);
+          pcl::Indices target_index (1);
           std::vector<float> sqr_dist_to_target (1);
           float old_energy, energy = std::numeric_limits<float>::max ();
 
diff --git a/recognition/include/pcl/recognition/ransac_based/voxel_structure.h b/recognition/include/pcl/recognition/ransac_based/voxel_structure.h
index 78622a77..8fad5f5e 100644
--- a/recognition/include/pcl/recognition/ransac_based/voxel_structure.h
+++ b/recognition/include/pcl/recognition/ransac_based/voxel_structure.h
@@ -38,8 +38,6 @@
 
 #pragma once
 
-#include <cstdlib>
-
 namespace pcl
 {
   namespace recognition
diff --git a/recognition/include/pcl/recognition/rigid_transform_space.h b/recognition/include/pcl/recognition/rigid_transform_space.h
new file mode 100644
index 00000000..2cc30436
--- /dev/null
+++ b/recognition/include/pcl/recognition/rigid_transform_space.h
@@ -0,0 +1,2 @@
+#include <pcl/recognition/ransac_based/rigid_transform_space.h>
+PCL_DEPRECATED_HEADER(1, 15, "Please use <pcl/recognition/ransac_based/rigid_transform_space.h> instead")
diff --git a/recognition/include/pcl/recognition/simple_octree.h b/recognition/include/pcl/recognition/simple_octree.h
new file mode 100644
index 00000000..aa47e0d8
--- /dev/null
+++ b/recognition/include/pcl/recognition/simple_octree.h
@@ -0,0 +1,2 @@
+#include <pcl/recognition/ransac_based/simple_octree.h>
+PCL_DEPRECATED_HEADER(1, 15, "Please use <pcl/recognition/ransac_based/simple_octree.h> instead")
diff --git a/recognition/include/pcl/recognition/trimmed_icp.h b/recognition/include/pcl/recognition/trimmed_icp.h
new file mode 100644
index 00000000..cabf17ba
--- /dev/null
+++ b/recognition/include/pcl/recognition/trimmed_icp.h
@@ -0,0 +1,2 @@
+#include <pcl/recognition/ransac_based/trimmed_icp.h>
+PCL_DEPRECATED_HEADER(1, 15, "Please use <pcl/recognition/ransac_based/trimmed_icp.h> instead")
diff --git a/recognition/include/pcl/recognition/voxel_structure.h b/recognition/include/pcl/recognition/voxel_structure.h
new file mode 100644
index 00000000..a78e541c
--- /dev/null
+++ b/recognition/include/pcl/recognition/voxel_structure.h
@@ -0,0 +1,2 @@
+#include <pcl/recognition/ransac_based/voxel_structure.h>
+PCL_DEPRECATED_HEADER(1, 15, "Please use <pcl/recognition/ransac_based/voxel_structure.h> instead")
diff --git a/recognition/src/mask_map.cpp b/recognition/src/mask_map.cpp
index bf492833..849a2826 100644
--- a/recognition/src/mask_map.cpp
+++ b/recognition/src/mask_map.cpp
@@ -54,15 +54,6 @@ pcl::MaskMap::resize(const std::size_t width, const std::size_t height)
   height_ = height;
 }
 
-//////////////////////////////////////////////////////////////////////////////////////////////
-void
-pcl::MaskMap::getDifferenceMask(const MaskMap& mask0,
-                                const MaskMap& mask1,
-                                MaskMap& diff_mask)
-{
-  diff_mask = getDifferenceMask(mask0, mask1);
-}
-
 //////////////////////////////////////////////////////////////////////////////////////////////
 pcl::MaskMap
 pcl::MaskMap::getDifferenceMask(const MaskMap& mask0, const MaskMap& mask1)
diff --git a/recognition/src/ransac_based/orr_octree.cpp b/recognition/src/ransac_based/orr_octree.cpp
index b7867bbb..e938be6a 100644
--- a/recognition/src/ransac_based/orr_octree.cpp
+++ b/recognition/src/ransac_based/orr_octree.cpp
@@ -41,6 +41,7 @@
 #include <pcl/common/common.h>
 #include <pcl/common/random.h>
 #include <cmath>
+#include <ctime> // for time
 #include <algorithm>
 #include <list>
 
diff --git a/registration/include/pcl/registration/bfgs.h b/registration/include/pcl/registration/bfgs.h
index cf302061..5b2d9e72 100644
--- a/registration/include/pcl/registration/bfgs.h
+++ b/registration/include/pcl/registration/bfgs.h
@@ -1,90 +1,86 @@
 #pragma once
+#include <pcl/pcl_macros.h>
 
 #if defined __GNUC__
-#  pragma GCC system_header 
+#pragma GCC system_header
 #endif
 
-#include <pcl/registration/eigen.h>
+#include <unsupported/Eigen/Polynomials> // for PolynomialSolver, PolynomialSolverBase
 
-namespace Eigen
-{
-  template< typename _Scalar >
-  class PolynomialSolver<_Scalar,2> : public PolynomialSolverBase<_Scalar,2>
+namespace Eigen {
+template <typename _Scalar>
+class PolynomialSolver<_Scalar, 2> : public PolynomialSolverBase<_Scalar, 2> {
+public:
+  using PS_Base = PolynomialSolverBase<_Scalar, 2>;
+  EIGEN_POLYNOMIAL_SOLVER_BASE_INHERITED_TYPES(PS_Base)
+
+public:
+  virtual ~PolynomialSolver() {}
+
+  template <typename OtherPolynomial>
+  inline PolynomialSolver(const OtherPolynomial& poly, bool& hasRealRoot)
   {
-    public:
-      using PS_Base = PolynomialSolverBase<_Scalar,2>;
-      EIGEN_POLYNOMIAL_SOLVER_BASE_INHERITED_TYPES( PS_Base )
-        
-    public:
-
-      virtual ~PolynomialSolver () {}
-
-      template< typename OtherPolynomial >
-      inline PolynomialSolver( const OtherPolynomial& poly, bool& hasRealRoot )
-      {
-        compute( poly, hasRealRoot );
-      }
-      
-      /** Computes the complex roots of a new polynomial. */
-      template< typename OtherPolynomial >
-      void compute( const OtherPolynomial& poly, bool& hasRealRoot)
-      {
-        const Scalar ZERO(0);
-        Scalar a2(2 * poly[2]);
-        assert( ZERO != poly[poly.size()-1] );
-        Scalar discriminant ((poly[1] * poly[1]) - (4 * poly[0] * poly[2]));
-        if (ZERO < discriminant)
-        {
-          Scalar discriminant_root (std::sqrt (discriminant));
-          m_roots[0] = (-poly[1] - discriminant_root) / (a2) ;
-          m_roots[1] = (-poly[1] + discriminant_root) / (a2) ;
-          hasRealRoot = true;
-        }
-        else {
-          if (ZERO == discriminant)
-          {
-            m_roots.resize (1);
-            m_roots[0] = -poly[1] / a2;
-            hasRealRoot = true;
-          }
-          else
-          {
-            Scalar discriminant_root (std::sqrt (-discriminant));
-            m_roots[0] = RootType (-poly[1] / a2, -discriminant_root / a2);
-            m_roots[1] = RootType (-poly[1] / a2,  discriminant_root / a2);
-            hasRealRoot = false;
-          }
-        }
+    compute(poly, hasRealRoot);
+  }
+
+  /** Computes the complex roots of a new polynomial. */
+  template <typename OtherPolynomial>
+  void
+  compute(const OtherPolynomial& poly, bool& hasRealRoot)
+  {
+    const Scalar ZERO(0);
+    Scalar a2(2 * poly[2]);
+    assert(ZERO != poly[poly.size() - 1]);
+    Scalar discriminant((poly[1] * poly[1]) - (4 * poly[0] * poly[2]));
+    if (ZERO < discriminant) {
+      Scalar discriminant_root(std::sqrt(discriminant));
+      m_roots[0] = (-poly[1] - discriminant_root) / (a2);
+      m_roots[1] = (-poly[1] + discriminant_root) / (a2);
+      hasRealRoot = true;
+    }
+    else {
+      if (ZERO == discriminant) {
+        m_roots.resize(1);
+        m_roots[0] = -poly[1] / a2;
+        hasRealRoot = true;
       }
-      
-      template< typename OtherPolynomial >
-      void compute( const OtherPolynomial& poly)
-      {
-        bool hasRealRoot;
-        compute(poly, hasRealRoot);
+      else {
+        Scalar discriminant_root(std::sqrt(-discriminant));
+        m_roots[0] = RootType(-poly[1] / a2, -discriminant_root / a2);
+        m_roots[1] = RootType(-poly[1] / a2, discriminant_root / a2);
+        hasRealRoot = false;
       }
+    }
+  }
 
-    protected:
-      using                   PS_Base::m_roots;
-  };
-}
+  template <typename OtherPolynomial>
+  void
+  compute(const OtherPolynomial& poly)
+  {
+    bool hasRealRoot;
+    compute(poly, hasRealRoot);
+  }
+
+protected:
+  using PS_Base::m_roots;
+};
+} // namespace Eigen
 
 namespace BFGSSpace {
-  enum Status {
-    NegativeGradientEpsilon = -3,
-    NotStarted = -2,
-    Running = -1,
-    Success = 0,
-    NoProgress = 1
-  };
+enum Status {
+  NegativeGradientEpsilon = -3,
+  NotStarted = -2,
+  Running = -1,
+  Success = 0,
+  NoProgress = 1
+};
 }
 
-template<typename _Scalar, int NX=Eigen::Dynamic>
-struct BFGSDummyFunctor
-{
+template <typename _Scalar, int NX = Eigen::Dynamic>
+struct BFGSDummyFunctor {
   using Scalar = _Scalar;
   enum { InputsAtCompileTime = NX };
-  using VectorType = Eigen::Matrix<Scalar,InputsAtCompileTime,1>;
+  using VectorType = Eigen::Matrix<Scalar, InputsAtCompileTime, 1>;
 
   const int m_inputs;
 
@@ -92,49 +88,59 @@ struct BFGSDummyFunctor
   BFGSDummyFunctor(int inputs) : m_inputs(inputs) {}
 
   virtual ~BFGSDummyFunctor() {}
-  int inputs() const { return m_inputs; }
+  int
+  inputs() const
+  {
+    return m_inputs;
+  }
 
-  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; };
+  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;
+  };
 };
 
 /**
- * BFGS stands for Broyden–Fletcher–Goldfarb–Shanno (BFGS) method for solving 
- * unconstrained nonlinear optimization problems. 
+ * BFGS stands for Broyden–Fletcher–Goldfarb–Shanno (BFGS) method for solving
+ * unconstrained nonlinear optimization problems.
  * For further details please visit: http://en.wikipedia.org/wiki/BFGS_method
  * The method provided here is almost similar to the one provided by GSL.
  * It reproduces Fletcher's original algorithm in Practical Methods of Optimization
- * algorithms : 2.6.2 and 2.6.4 and uses the same politics in GSL with cubic 
- * interpolation whenever it is possible else falls to quadratic interpolation for 
+ * algorithms : 2.6.2 and 2.6.4 and uses the same politics in GSL with cubic
+ * interpolation whenever it is possible else falls to quadratic interpolation for
  * alpha parameter.
  */
-template<typename FunctorType>
-class BFGS
-{
+template <typename FunctorType>
+class BFGS {
 public:
   using Scalar = typename FunctorType::Scalar;
   using FVectorType = typename FunctorType::VectorType;
 
-  BFGS(FunctorType &_functor) 
-      : pnorm(0), g0norm(0), iter(-1), functor(_functor) {  }
+  BFGS(FunctorType& _functor) : pnorm(0), g0norm(0), iter(-1), functor(_functor) {}
 
   using Index = Eigen::DenseIndex;
 
   struct Parameters {
     Parameters()
     : max_iters(400)
-      , bracket_iters(100)
-      , section_iters(100)
-      , rho(0.01)
-      , sigma(0.01)
-      , tau1(9)
-      , tau2(0.05)
-      , tau3(0.5)
-      , step_size(1)
-      , order(3) {}
-    Index max_iters;   // maximum number of function evaluation
+    , bracket_iters(100)
+    , section_iters(100)
+    , rho(0.01)
+    , sigma(0.01)
+    , tau1(9)
+    , tau2(0.05)
+    , tau3(0.5)
+    , step_size(1)
+    , order(3)
+    {}
+    Index max_iters; // maximum number of function evaluation
     Index bracket_iters;
     Index section_iters;
     Scalar rho;
@@ -146,35 +152,68 @@ public:
     Index order;
   };
 
-  BFGSSpace::Status minimize(FVectorType &x);
-  BFGSSpace::Status minimizeInit(FVectorType &x);
-  BFGSSpace::Status minimizeOneStep(FVectorType &x);
-  BFGSSpace::Status testGradient();
+  BFGSSpace::Status
+  minimize(FVectorType& x);
+  BFGSSpace::Status
+  minimizeInit(FVectorType& x);
+  BFGSSpace::Status
+  minimizeOneStep(FVectorType& x);
+  BFGSSpace::Status
+  testGradient();
   PCL_DEPRECATED(1, 13, "Use `testGradient()` instead")
   BFGSSpace::Status testGradient(Scalar) { return testGradient(); }
-  void resetParameters(void) { parameters = Parameters(); }
-  
+  void
+  resetParameters(void)
+  {
+    parameters = Parameters();
+  }
+
   Parameters parameters;
   Scalar f;
   FVectorType gradient;
+
 private:
-  
-  BFGS& operator=(const BFGS&);
-  BFGSSpace::Status lineSearch (Scalar rho, Scalar sigma, 
-                                Scalar tau1, Scalar tau2, Scalar tau3,
-                                int order, Scalar alpha1, Scalar &alpha_new);
-  Scalar interpolate (Scalar a, Scalar fa, Scalar fpa,
-                      Scalar b, Scalar fb, Scalar fpb, Scalar xmin, Scalar xmax,
-                      int order);  
-  void checkExtremum (const Eigen::Matrix<Scalar, 4, 1>& coefficients, Scalar x, Scalar& xmin, Scalar& fmin);
-  void moveTo (Scalar alpha);
-  Scalar slope ();
-  Scalar applyF (Scalar alpha);
-  Scalar applyDF (Scalar alpha);
-  void applyFDF (Scalar alpha, Scalar &f, Scalar &df);
-  void updatePosition (Scalar alpha, FVectorType& x, Scalar& f, FVectorType& g);
-  void changeDirection ();
-  
+  BFGS&
+  operator=(const BFGS&);
+  BFGSSpace::Status
+  lineSearch(Scalar rho,
+             Scalar sigma,
+             Scalar tau1,
+             Scalar tau2,
+             Scalar tau3,
+             int order,
+             Scalar alpha1,
+             Scalar& alpha_new);
+  Scalar
+  interpolate(Scalar a,
+              Scalar fa,
+              Scalar fpa,
+              Scalar b,
+              Scalar fb,
+              Scalar fpb,
+              Scalar xmin,
+              Scalar xmax,
+              int order);
+  void
+  checkExtremum(const Eigen::Matrix<Scalar, 4, 1>& coefficients,
+                Scalar x,
+                Scalar& xmin,
+                Scalar& fmin);
+  void
+  moveTo(Scalar alpha);
+  Scalar
+  slope();
+  Scalar
+  applyF(Scalar alpha);
+  Scalar
+  applyDF(Scalar alpha);
+  void
+  applyFDF(Scalar alpha, Scalar& f, Scalar& df);
+  void
+  updatePosition(Scalar alpha, FVectorType& x, Scalar& f, FVectorType& g);
+  void
+  changeDirection();
+
   Scalar delta_f, fp0;
   FVectorType x0, dx0, dg0, g0, dx, p;
   Scalar pnorm, g0norm;
@@ -183,7 +222,7 @@ private:
   Scalar df_alpha;
   FVectorType x_alpha;
   FVectorType g_alpha;
-  
+
   // cache "keys"
   Scalar f_cache_key;
   Scalar df_cache_key;
@@ -191,169 +230,195 @@ private:
   Scalar g_cache_key;
 
   Index iter;
-  FunctorType &functor;
+  FunctorType& functor;
 };
 
-
-template<typename FunctorType> void
-BFGS<FunctorType>::checkExtremum(const Eigen::Matrix<Scalar, 4, 1>& coefficients, Scalar x, Scalar& xmin, Scalar& fmin)
+template <typename FunctorType>
+void
+BFGS<FunctorType>::checkExtremum(const Eigen::Matrix<Scalar, 4, 1>& coefficients,
+                                 Scalar x,
+                                 Scalar& xmin,
+                                 Scalar& fmin)
 {
   Scalar y = Eigen::poly_eval(coefficients, x);
-  if(y < fmin) { xmin = x; fmin = y; }
+  if (y < fmin) {
+    xmin = x;
+    fmin = y;
+  }
 }
 
-template<typename FunctorType> void
+template <typename FunctorType>
+void
 BFGS<FunctorType>::moveTo(Scalar alpha)
 {
   x_alpha = x0 + alpha * p;
   x_cache_key = alpha;
 }
 
-template<typename FunctorType> typename BFGS<FunctorType>::Scalar
+template <typename FunctorType>
+typename BFGS<FunctorType>::Scalar
 BFGS<FunctorType>::slope()
 {
-  return (g_alpha.dot (p));
+  return (g_alpha.dot(p));
 }
 
-template<typename FunctorType> typename BFGS<FunctorType>::Scalar
+template <typename FunctorType>
+typename BFGS<FunctorType>::Scalar
 BFGS<FunctorType>::applyF(Scalar alpha)
 {
-  if (alpha == f_cache_key) return f_alpha;
-  moveTo (alpha);
-  f_alpha = functor (x_alpha);
+  if (alpha == f_cache_key)
+    return f_alpha;
+  moveTo(alpha);
+  f_alpha = functor(x_alpha);
   f_cache_key = alpha;
   return (f_alpha);
 }
 
-template<typename FunctorType> typename BFGS<FunctorType>::Scalar
+template <typename FunctorType>
+typename BFGS<FunctorType>::Scalar
 BFGS<FunctorType>::applyDF(Scalar alpha)
 {
-  if (alpha == df_cache_key) return df_alpha;
-  moveTo (alpha);
-  if(alpha != g_cache_key)
-  {
-    functor.df (x_alpha, g_alpha);
+  if (alpha == df_cache_key)
+    return df_alpha;
+  moveTo(alpha);
+  if (alpha != g_cache_key) {
+    functor.df(x_alpha, g_alpha);
     g_cache_key = alpha;
   }
-  df_alpha = slope ();
+  df_alpha = slope();
   df_cache_key = alpha;
   return (df_alpha);
 }
 
-template<typename FunctorType> void
+template <typename FunctorType>
+void
 BFGS<FunctorType>::applyFDF(Scalar alpha, Scalar& f, Scalar& df)
 {
-  if(alpha == f_cache_key && alpha == df_cache_key)
-  {
+  if (alpha == f_cache_key && alpha == df_cache_key) {
     f = f_alpha;
     df = df_alpha;
     return;
   }
 
-  if(alpha == f_cache_key || alpha == df_cache_key)
-  {
-    f = applyF (alpha);
-    df = applyDF (alpha);
+  if (alpha == f_cache_key || alpha == df_cache_key) {
+    f = applyF(alpha);
+    df = applyDF(alpha);
     return;
   }
 
-  moveTo (alpha);
-  functor.fdf (x_alpha, f_alpha, g_alpha);
+  moveTo(alpha);
+  functor.fdf(x_alpha, f_alpha, g_alpha);
   f_cache_key = alpha;
   g_cache_key = alpha;
-  df_alpha = slope ();
+  df_alpha = slope();
   df_cache_key = alpha;
   f = f_alpha;
   df = df_alpha;
 }
 
-template<typename FunctorType> void
-BFGS<FunctorType>::updatePosition (Scalar alpha, FVectorType &x, Scalar &f, FVectorType &g)
+template <typename FunctorType>
+void
+BFGS<FunctorType>::updatePosition(Scalar alpha,
+                                  FVectorType& x,
+                                  Scalar& f,
+                                  FVectorType& g)
 {
-  { 
-    Scalar f_alpha, df_alpha; 
-    applyFDF (alpha, f_alpha, df_alpha); 
-  } ;
+  {
+    Scalar f_alpha, df_alpha;
+    applyFDF(alpha, f_alpha, df_alpha);
+  };
 
   f = f_alpha;
   x = x_alpha;
   g = g_alpha;
-}  
+}
 
-template<typename FunctorType> void
-BFGS<FunctorType>::changeDirection ()
+template <typename FunctorType>
+void
+BFGS<FunctorType>::changeDirection()
 {
   x_alpha = x0;
   x_cache_key = 0.0;
   f_cache_key = 0.0;
   g_alpha = g0;
   g_cache_key = 0.0;
-  df_alpha = slope ();
+  df_alpha = slope();
   df_cache_key = 0.0;
 }
 
-template<typename FunctorType> BFGSSpace::Status
-BFGS<FunctorType>::minimize(FVectorType  &x)
+template <typename FunctorType>
+BFGSSpace::Status
+BFGS<FunctorType>::minimize(FVectorType& x)
 {
   BFGSSpace::Status status = minimizeInit(x);
   do {
     status = minimizeOneStep(x);
     iter++;
-  } while (status==BFGSSpace::Success && iter < parameters.max_iters);
+  } while (status == BFGSSpace::Success && iter < parameters.max_iters);
   return status;
 }
 
-template<typename FunctorType> BFGSSpace::Status
-BFGS<FunctorType>::minimizeInit(FVectorType  &x)
+template <typename FunctorType>
+BFGSSpace::Status
+BFGS<FunctorType>::minimizeInit(FVectorType& x)
 {
   iter = 0;
   delta_f = 0;
-  dx.setZero ();
+  dx.setZero();
   functor.fdf(x, f, gradient);
   x0 = x;
   g0 = gradient;
-  g0norm = g0.norm ();
-  p = gradient * -1/g0norm;
-  pnorm = p.norm ();
+  g0norm = g0.norm();
+  p = gradient * -1 / g0norm;
+  pnorm = p.norm();
   fp0 = -g0norm;
 
   {
-    x_alpha = x0; x_cache_key = 0;
-    
-    f_alpha = f; f_cache_key = 0;
-    
-    g_alpha = g0; g_cache_key = 0;
-    
-    df_alpha = slope (); df_cache_key = 0;
+    x_alpha = x0;
+    x_cache_key = 0;
+
+    f_alpha = f;
+    f_cache_key = 0;
+
+    g_alpha = g0;
+    g_cache_key = 0;
+
+    df_alpha = slope();
+    df_cache_key = 0;
   }
 
   return BFGSSpace::NotStarted;
 }
 
-template<typename FunctorType> BFGSSpace::Status
-BFGS<FunctorType>::minimizeOneStep(FVectorType  &x)
+template <typename FunctorType>
+BFGSSpace::Status
+BFGS<FunctorType>::minimizeOneStep(FVectorType& x)
 {
   Scalar alpha = 0.0, alpha1;
   Scalar f0 = f;
-  if (pnorm == 0.0 || g0norm == 0.0 || fp0 == 0)
-  {
-    dx.setZero ();
+  if (pnorm == 0.0 || g0norm == 0.0 || fp0 == 0) {
+    dx.setZero();
     return BFGSSpace::NoProgress;
   }
 
-  if (delta_f < 0)
-  {
-    Scalar del = std::max (-delta_f, 10 * std::numeric_limits<Scalar>::epsilon() * std::abs(f0));
-    alpha1 = std::min (1.0, 2.0 * del / (-fp0));
+  if (delta_f < 0) {
+    Scalar del =
+        std::max(-delta_f, 10 * std::numeric_limits<Scalar>::epsilon() * std::abs(f0));
+    alpha1 = std::min(1.0, 2.0 * del / (-fp0));
   }
   else
     alpha1 = std::abs(parameters.step_size);
 
-  BFGSSpace::Status status = lineSearch(parameters.rho, parameters.sigma, 
-                                        parameters.tau1, parameters.tau2, parameters.tau3, 
-                                        parameters.order, alpha1, alpha);
+  BFGSSpace::Status status = lineSearch(parameters.rho,
+                                        parameters.sigma,
+                                        parameters.tau1,
+                                        parameters.tau2,
+                                        parameters.tau3,
+                                        parameters.order,
+                                        alpha1,
+                                        alpha);
 
-  if(status != BFGSSpace::Success)
+  if (status != BFGSSpace::Success)
     return status;
 
   updatePosition(alpha, x, f, gradient);
@@ -375,36 +440,34 @@ BFGS<FunctorType>::minimizeOneStep(FVectorType  &x)
 
     /* dg0 = g - g0 */
     dg0 = gradient - g0;
-    dxg = dx0.dot (gradient);
-    dgg = dg0.dot (gradient);
-    dxdg = dx0.dot (dg0);
-    dgnorm = dg0.norm ();
+    dxg = dx0.dot(gradient);
+    dgg = dg0.dot(gradient);
+    dxdg = dx0.dot(dg0);
+    dgnorm = dg0.norm();
 
-    if (dxdg != 0)
-    {
+    if (dxdg != 0) {
       B = dxg / dxdg;
       A = -(1.0 + dgnorm * dgnorm / dxdg) * B + dgg / dxdg;
     }
-    else
-    {
+    else {
       B = 0;
       A = 0;
     }
 
     p = -A * dx0;
-    p+= gradient;
-    p+= -B * dg0 ;
+    p += gradient;
+    p += -B * dg0;
   }
 
   g0 = gradient;
   x0 = x;
-  g0norm = g0.norm ();
-  pnorm = p.norm ();
-  
-  Scalar dir = ((p.dot (gradient)) > 0) ? -1.0 : 1.0;
-  p*= dir / pnorm;
-  pnorm = p.norm ();
-  fp0 = p.dot (g0);
+  g0norm = g0.norm();
+  pnorm = p.norm();
+
+  Scalar dir = ((p.dot(gradient)) > 0) ? -1.0 : 1.0;
+  p *= dir / pnorm;
+  pnorm = p.norm();
+  fp0 = p.dot(g0);
 
   changeDirection();
   return BFGSSpace::Success;
@@ -417,23 +480,32 @@ BFGS<FunctorType>::testGradient()
   return functor.checkGradient(gradient);
 }
 
-template<typename FunctorType> typename BFGS<FunctorType>::Scalar 
-BFGS<FunctorType>::interpolate (Scalar a, Scalar fa, Scalar fpa,
-                                Scalar b, Scalar fb, Scalar fpb, 
-                                Scalar xmin, Scalar xmax,
-                                int order)
+template <typename FunctorType>
+typename BFGS<FunctorType>::Scalar
+BFGS<FunctorType>::interpolate(Scalar a,
+                               Scalar fa,
+                               Scalar fpa,
+                               Scalar b,
+                               Scalar fb,
+                               Scalar fpb,
+                               Scalar xmin,
+                               Scalar xmax,
+                               int order)
 {
   /* Map [a,b] to [0,1] */
   Scalar y, alpha, ymin, ymax, fmin;
 
   ymin = (xmin - a) / (b - a);
   ymax = (xmax - a) / (b - a);
-  
+
   // Ensure ymin <= ymax
-  if (ymin > ymax) { Scalar tmp = ymin; ymin = ymax; ymax = tmp; };
+  if (ymin > ymax) {
+    Scalar tmp = ymin;
+    ymin = ymax;
+    ymax = tmp;
+  };
 
-  if (order > 2 && !(fpb != fpa) && fpb != std::numeric_limits<Scalar>::infinity ())
-  {
+  if (order > 2 && !(fpb != fpa) && fpb != std::numeric_limits<Scalar>::infinity()) {
     fpa = fpa * (b - a);
     fpb = fpb * (b - a);
 
@@ -443,109 +515,131 @@ BFGS<FunctorType>::interpolate (Scalar a, Scalar fa, Scalar fpa,
     Scalar y0, y1;
     Eigen::Matrix<Scalar, 4, 1> coefficients;
     coefficients << c0, c1, c2, c3;
-    
-    y = ymin; 
+
+    y = ymin;
     // Evaluate the cubic polyinomial at ymin;
-    fmin = Eigen::poly_eval (coefficients, ymin);
-    checkExtremum (coefficients, ymax, y, fmin);
+    fmin = Eigen::poly_eval(coefficients, ymin);
+    checkExtremum(coefficients, ymax, y, fmin);
     {
       // Solve quadratic polynomial for the derivate
       Eigen::Matrix<Scalar, 3, 1> coefficients2;
       coefficients2 << c1, 2 * c2, 3 * c3;
       bool real_roots;
-      Eigen::PolynomialSolver<Scalar, 2> solver (coefficients2, real_roots);
-      if(real_roots)
-      {
-        if ((solver.roots ()).size () == 2)  /* found 2 roots */
+      Eigen::PolynomialSolver<Scalar, 2> solver(coefficients2, real_roots);
+      if (real_roots) {
+        if ((solver.roots()).size() == 2) /* found 2 roots */
         {
-          y0 = std::real (solver.roots () [0]);
-          y1 = std::real (solver.roots () [1]);
-          if(y0 > y1) { Scalar tmp (y0); y0 = y1; y1 = tmp; }
-          if (y0 > ymin && y0 < ymax) 
-            checkExtremum (coefficients, y0, y, fmin);
-          if (y1 > ymin && y1 < ymax) 
-            checkExtremum (coefficients, y1, y, fmin);
+          y0 = std::real(solver.roots()[0]);
+          y1 = std::real(solver.roots()[1]);
+          if (y0 > y1) {
+            Scalar tmp(y0);
+            y0 = y1;
+            y1 = tmp;
+          }
+          if (y0 > ymin && y0 < ymax)
+            checkExtremum(coefficients, y0, y, fmin);
+          if (y1 > ymin && y1 < ymax)
+            checkExtremum(coefficients, y1, y, fmin);
         }
-        else if ((solver.roots ()).size () == 1)  /* found 1 root */
+        else if ((solver.roots()).size() == 1) /* found 1 root */
         {
-          y0 = std::real (solver.roots () [0]);
-          if (y0 > ymin && y0 < ymax) 
-            checkExtremum (coefficients, y0, y, fmin);
+          y0 = std::real(solver.roots()[0]);
+          if (y0 > ymin && y0 < ymax)
+            checkExtremum(coefficients, y0, y, fmin);
         }
       }
     }
-  } 
-  else 
-  {
+  }
+  else {
     fpa = fpa * (b - a);
-    Scalar fl = fa + ymin*(fpa + ymin*(fb - fa -fpa));
-    Scalar fh = fa + ymax*(fpa + ymax*(fb - fa -fpa));
-    Scalar c = 2 * (fb - fa - fpa);       /* curvature */
-    y = ymin; fmin = fl;
-    
-    if (fh < fmin) { y = ymax; fmin = fh; } 
-    
-    if (c > a)  /* positive curvature required for a minimum */
+    Scalar fl = fa + ymin * (fpa + ymin * (fb - fa - fpa));
+    Scalar fh = fa + ymax * (fpa + ymax * (fb - fa - fpa));
+    Scalar c = 2 * (fb - fa - fpa); /* curvature */
+    y = ymin;
+    fmin = fl;
+
+    if (fh < fmin) {
+      y = ymax;
+      fmin = fh;
+    }
+
+    if (c > a) /* positive curvature required for a minimum */
     {
-      Scalar z = -fpa / c;      /* location of minimum */
+      Scalar z = -fpa / c; /* location of minimum */
       if (z > ymin && z < ymax) {
-        Scalar f = fa + z*(fpa + z*(fb - fa -fpa));
-        if (f < fmin) { y = z; fmin = f; };
+        Scalar f = fa + z * (fpa + z * (fb - fa - fpa));
+        if (f < fmin) {
+          y = z;
+          fmin = f;
+        };
       }
     }
   }
-  
+
   alpha = a + y * (b - a);
   return alpha;
 }
 
-template<typename FunctorType> BFGSSpace::Status 
-BFGS<FunctorType>::lineSearch(Scalar rho, Scalar sigma, 
-                              Scalar tau1, Scalar tau2, Scalar tau3,
-                              int order, Scalar alpha1, Scalar &alpha_new)
+template <typename FunctorType>
+BFGSSpace::Status
+BFGS<FunctorType>::lineSearch(Scalar rho,
+                              Scalar sigma,
+                              Scalar tau1,
+                              Scalar tau2,
+                              Scalar tau3,
+                              int order,
+                              Scalar alpha1,
+                              Scalar& alpha_new)
 {
   Scalar f0, fp0, falpha, falpha_prev, fpalpha, fpalpha_prev, delta, alpha_next;
   Scalar alpha = alpha1, alpha_prev = 0.0;
   Scalar a, b, fa, fb, fpa, fpb;
   Index i = 0;
 
-  applyFDF (0.0, f0, fp0);
+  applyFDF(0.0, f0, fp0);
 
   falpha_prev = f0;
   fpalpha_prev = fp0;
 
   /* Avoid uninitialized variables morning */
-  a = 0.0; b = alpha;
-  fa = f0; fb = 0.0;
-  fpa = fp0; fpb = 0.0;
-
-  /* Begin bracketing */  
-
-  while (i++ < parameters.bracket_iters)
-  {
-    falpha = applyF (alpha);
-
-    if (falpha > f0 + alpha * rho * fp0 || falpha >= falpha_prev)
-    {
-      a = alpha_prev; fa = falpha_prev; fpa = fpalpha_prev;
-      b = alpha; fb = falpha; fpb = std::numeric_limits<Scalar>::quiet_NaN ();
+  a = 0.0;
+  b = alpha;
+  fa = f0;
+  fb = 0.0;
+  fpa = fp0;
+  fpb = 0.0;
+
+  /* Begin bracketing */
+
+  while (i++ < parameters.bracket_iters) {
+    falpha = applyF(alpha);
+
+    if (falpha > f0 + alpha * rho * fp0 || falpha >= falpha_prev) {
+      a = alpha_prev;
+      fa = falpha_prev;
+      fpa = fpalpha_prev;
+      b = alpha;
+      fb = falpha;
+      fpb = std::numeric_limits<Scalar>::quiet_NaN();
       break;
-    } 
+    }
 
-    fpalpha = applyDF (alpha);
+    fpalpha = applyDF(alpha);
 
     /* Fletcher's sigma test */
-    if (std::abs (fpalpha) <= -sigma * fp0)
-    {
+    if (std::abs(fpalpha) <= -sigma * fp0) {
       alpha_new = alpha;
       return BFGSSpace::Success;
     }
 
-    if (fpalpha >= 0)
-    {
-      a = alpha; fa = falpha; fpa = fpalpha;
-      b = alpha_prev; fb = falpha_prev; fpb = fpalpha_prev;
-      break;                /* goto sectioning */
+    if (fpalpha >= 0) {
+      a = alpha;
+      fa = falpha;
+      fpa = fpalpha;
+      b = alpha_prev;
+      fb = falpha_prev;
+      fpb = fpalpha_prev;
+      break; /* goto sectioning */
     }
 
     delta = alpha - alpha_prev;
@@ -554,9 +648,15 @@ BFGS<FunctorType>::lineSearch(Scalar rho, Scalar sigma,
       Scalar lower = alpha + delta;
       Scalar upper = alpha + tau1 * delta;
 
-      alpha_next = interpolate (alpha_prev, falpha_prev, fpalpha_prev,
-                                alpha, falpha, fpalpha, lower, upper, order);
-
+      alpha_next = interpolate(alpha_prev,
+                               falpha_prev,
+                               fpalpha_prev,
+                               alpha,
+                               falpha,
+                               fpalpha,
+                               lower,
+                               upper,
+                               order);
     }
 
     alpha_prev = alpha;
@@ -565,45 +665,47 @@ BFGS<FunctorType>::lineSearch(Scalar rho, Scalar sigma,
     alpha = alpha_next;
   }
   /*  Sectioning of bracket [a,b] */
-  while (i++ < parameters.section_iters)
-  {
+  while (i++ < parameters.section_iters) {
     delta = b - a;
-      
+
     {
       Scalar lower = a + tau2 * delta;
       Scalar upper = b - tau3 * delta;
-        
-      alpha = interpolate (a, fa, fpa, b, fb, fpb, lower, upper, order);
+
+      alpha = interpolate(a, fa, fpa, b, fb, fpb, lower, upper, order);
     }
-    falpha = applyF (alpha);
-    if ((a-alpha)*fpa <= std::numeric_limits<Scalar>::epsilon ()) {
+    falpha = applyF(alpha);
+    if ((a - alpha) * fpa <= std::numeric_limits<Scalar>::epsilon()) {
       /* roundoff prevents progress */
       return BFGSSpace::NoProgress;
     };
 
-    if (falpha > f0 + rho * alpha * fp0 || falpha >= fa)
-    {
+    if (falpha > f0 + rho * alpha * fp0 || falpha >= fa) {
       /*  a_next = a; */
-      b = alpha; fb = falpha; fpb = std::numeric_limits<Scalar>::quiet_NaN ();
+      b = alpha;
+      fb = falpha;
+      fpb = std::numeric_limits<Scalar>::quiet_NaN();
     }
-    else
-    {
-      fpalpha = applyDF (alpha);
-          
-      if (std::abs(fpalpha) <= -sigma * fp0)
-      {
+    else {
+      fpalpha = applyDF(alpha);
+
+      if (std::abs(fpalpha) <= -sigma * fp0) {
         alpha_new = alpha;
-        return BFGSSpace::Success;  /* terminate */
+        return BFGSSpace::Success; /* terminate */
       }
-          
-      if ( ((b-a) >= 0 && fpalpha >= 0) || ((b-a) <=0 && fpalpha <= 0))
-      {
-        b = a; fb = fa; fpb = fpa;
-        a = alpha; fa = falpha; fpa = fpalpha;
+
+      if (((b - a) >= 0 && fpalpha >= 0) || ((b - a) <= 0 && fpalpha <= 0)) {
+        b = a;
+        fb = fa;
+        fpb = fpa;
+        a = alpha;
+        fa = falpha;
+        fpa = fpalpha;
       }
-      else
-      {
-        a = alpha; fa = falpha; fpa = fpalpha;
+      else {
+        a = alpha;
+        fa = falpha;
+        fpa = fpalpha;
       }
     }
   }
diff --git a/registration/include/pcl/registration/boost.h b/registration/include/pcl/registration/boost.h
index ae726dcc..6a6f17d1 100644
--- a/registration/include/pcl/registration/boost.h
+++ b/registration/include/pcl/registration/boost.h
@@ -38,14 +38,13 @@
  */
 
 #pragma once
-
+PCL_DEPRECATED_HEADER(1, 15, "Please include the needed boost headers directly.")
 #if defined __GNUC__
-#  pragma GCC system_header 
+#pragma GCC system_header
 #endif
 
 //#include <boost/graph/adjacency_list.hpp>
-#include <boost/graph/graph_traits.hpp>
 #include <boost/graph/dijkstra_shortest_paths.hpp>
-#include <boost/property_map/property_map.hpp>
-
+#include <boost/graph/graph_traits.hpp>
 #include <boost/noncopyable.hpp>
+#include <boost/property_map/property_map.hpp>
diff --git a/registration/include/pcl/registration/boost_graph.h b/registration/include/pcl/registration/boost_graph.h
index 0c18798a..c2c6b9fd 100644
--- a/registration/include/pcl/registration/boost_graph.h
+++ b/registration/include/pcl/registration/boost_graph.h
@@ -40,61 +40,50 @@
 #pragma once
 
 #include <boost/graph/adjacency_list.hpp>
+
 #include <Eigen/StdVector>
+
 #include <list>
 
-namespace boost
-{
+namespace boost {
 
-  struct eigen_vecS
-  {
-  };
+struct eigen_vecS {};
 
-  template <class ValueType>
-    struct container_gen<eigen_vecS, ValueType>
-    {
-      using type = std::vector<ValueType, Eigen::aligned_allocator<ValueType> >;
-    };
+template <class ValueType>
+struct container_gen<eigen_vecS, ValueType> {
+  using type = std::vector<ValueType, Eigen::aligned_allocator<ValueType>>;
+};
 
-  template <>
-    struct parallel_edge_traits<eigen_vecS>
-    {
-      using type = allow_parallel_edge_tag;
-    };
+template <>
+struct parallel_edge_traits<eigen_vecS> {
+  using type = allow_parallel_edge_tag;
+};
 
-  namespace detail
-  {
-    template <>
-      struct is_random_access<eigen_vecS>
-      {
-        enum { value = true };
-        using type = mpl::true_;
-      };
-  }
+namespace detail {
+template <>
+struct is_random_access<eigen_vecS> {
+  enum { value = true };
+  using type = mpl::true_;
+};
+} // namespace detail
 
-  struct eigen_listS
-  {
-  };
+struct eigen_listS {};
 
-  template <class ValueType>
-    struct container_gen<eigen_listS, ValueType>
-    {
-      using type = std::list<ValueType, Eigen::aligned_allocator<ValueType> >;
-    };
+template <class ValueType>
+struct container_gen<eigen_listS, ValueType> {
+  using type = std::list<ValueType, Eigen::aligned_allocator<ValueType>>;
+};
 
-  template <>
-    struct parallel_edge_traits<eigen_listS>
-    {
-      using type = allow_parallel_edge_tag;
-    };
+template <>
+struct parallel_edge_traits<eigen_listS> {
+  using type = allow_parallel_edge_tag;
+};
 
-  namespace detail
-  {
-    template <>
-      struct is_random_access<eigen_listS>
-      {
-        enum { value = false };
-        using type = mpl::false_;
-      };
-  }
-}
+namespace detail {
+template <>
+struct is_random_access<eigen_listS> {
+  enum { value = false };
+  using type = mpl::false_;
+};
+} // namespace detail
+} // namespace boost
diff --git a/registration/include/pcl/registration/convergence_criteria.h b/registration/include/pcl/registration/convergence_criteria.h
index ab64c576..afbe2a39 100644
--- a/registration/include/pcl/registration/convergence_criteria.h
+++ b/registration/include/pcl/registration/convergence_criteria.h
@@ -42,46 +42,42 @@
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
 
-namespace pcl
-{
-  namespace registration
-  {
-    /** \brief @b ConvergenceCriteria represents an abstract base class for
-      * different convergence criteria used in registration loops.
-      *
-      * This should be used as part of an Iterative Closest Point (ICP)-like
-      * method, to verify if the algorithm has reached convergence.
-      *
-      * Typical convergence criteria that could inherit from this include:
-      * 
-      *  * a maximum number of iterations has been reached
-      *  * the transformation (R, t) cannot be further updated (the difference between current and previous is smaller than a threshold)
-      *  * the Mean Squared Error (MSE) between the current set of correspondences and the previous one is smaller than some threshold
-      *
-      * \author Radu B. Rusu
-      * \ingroup registration
-      */
-    class PCL_EXPORTS ConvergenceCriteria
-    {
-      public:
-        using Ptr = shared_ptr<ConvergenceCriteria>;
-        using ConstPtr = shared_ptr<const ConvergenceCriteria>;
+namespace pcl {
+namespace registration {
+/** \brief @b ConvergenceCriteria represents an abstract base class for
+ * different convergence criteria used in registration loops.
+ *
+ * This should be used as part of an Iterative Closest Point (ICP)-like
+ * method, to verify if the algorithm has reached convergence.
+ *
+ * Typical convergence criteria that could inherit from this include:
+ *
+ *  * a maximum number of iterations has been reached
+ *  * the transformation (R, t) cannot be further updated (the difference between
+ * current and previous is smaller than a threshold)
+ *  * the Mean Squared Error (MSE) between the current set of correspondences and the
+ * previous one is smaller than some threshold
+ *
+ * \author Radu B. Rusu
+ * \ingroup registration
+ */
+class PCL_EXPORTS ConvergenceCriteria {
+public:
+  using Ptr = shared_ptr<ConvergenceCriteria>;
+  using ConstPtr = shared_ptr<const ConvergenceCriteria>;
 
-        /** \brief Empty constructor. */
-        ConvergenceCriteria () {}
+  /** \brief Empty constructor. */
+  ConvergenceCriteria() {}
 
-        /** \brief Empty destructor. */
-        virtual ~ConvergenceCriteria () {}
+  /** \brief Empty destructor. */
+  virtual ~ConvergenceCriteria() {}
 
-        /** \brief Check if convergence has been reached. Pure virtual. */
-        virtual bool
-        hasConverged () = 0;
+  /** \brief Check if convergence has been reached. Pure virtual. */
+  virtual bool
+  hasConverged() = 0;
 
-        /** \brief Bool operator. */
-        operator bool ()
-        {
-          return (hasConverged ());
-        }
-     };
-  }
-}
+  /** \brief Bool operator. */
+  operator bool() { return (hasConverged()); }
+};
+} // namespace registration
+} // namespace pcl
diff --git a/registration/include/pcl/registration/correspondence_estimation.h b/registration/include/pcl/registration/correspondence_estimation.h
index f016f298..f180d8d5 100644
--- a/registration/include/pcl/registration/correspondence_estimation.h
+++ b/registration/include/pcl/registration/correspondence_estimation.h
@@ -40,397 +40,411 @@
 
 #pragma once
 
-#include <string>
-
-#include <pcl/pcl_base.h>
-#include <pcl/common/transforms.h>
+#include <pcl/common/io.h> // for getFields
+#include <pcl/registration/correspondence_types.h>
 #include <pcl/search/kdtree.h>
 #include <pcl/memory.h>
+#include <pcl/pcl_base.h>
 #include <pcl/pcl_macros.h>
 
-#include <pcl/registration/correspondence_types.h>
+#include <string>
+
+namespace pcl {
+namespace registration {
+/** \brief Abstract @b CorrespondenceEstimationBase class.
+ * All correspondence estimation methods should inherit from this.
+ * \author Radu B. Rusu
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename Scalar = float>
+class CorrespondenceEstimationBase : public PCLBase<PointSource> {
+public:
+  using Ptr =
+      shared_ptr<CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>>;
+  using ConstPtr =
+      shared_ptr<const CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>>;
+
+  // using PCLBase<PointSource>::initCompute;
+  using PCLBase<PointSource>::deinitCompute;
+  using PCLBase<PointSource>::input_;
+  using PCLBase<PointSource>::indices_;
+  using PCLBase<PointSource>::setIndices;
+
+  using KdTree = pcl::search::KdTree<PointTarget>;
+  using KdTreePtr = typename KdTree::Ptr;
+
+  using KdTreeReciprocal = pcl::search::KdTree<PointSource>;
+  using KdTreeReciprocalPtr = typename KdTree::Ptr;
+
+  using PointCloudSource = pcl::PointCloud<PointSource>;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+
+  using PointCloudTarget = pcl::PointCloud<PointTarget>;
+  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
+  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
+
+  using PointRepresentationConstPtr = typename KdTree::PointRepresentationConstPtr;
+
+  /** \brief Empty constructor. */
+  CorrespondenceEstimationBase()
+  : corr_name_("CorrespondenceEstimationBase")
+  , tree_(new pcl::search::KdTree<PointTarget>)
+  , tree_reciprocal_(new pcl::search::KdTree<PointSource>)
+  , target_()
+  , point_representation_()
+  , input_transformed_()
+  , target_cloud_updated_(true)
+  , source_cloud_updated_(true)
+  , force_no_recompute_(false)
+  , force_no_recompute_reciprocal_(false)
+  {}
+
+  /** \brief Empty destructor */
+  ~CorrespondenceEstimationBase() {}
+
+  /** \brief Provide a pointer to the input source
+   * (e.g., the point cloud that we want to align to the target)
+   *
+   * \param[in] cloud the input point cloud source
+   */
+  inline void
+  setInputSource(const PointCloudSourceConstPtr& cloud)
+  {
+    source_cloud_updated_ = true;
+    PCLBase<PointSource>::setInputCloud(cloud);
+    input_fields_ = pcl::getFields<PointSource>();
+  }
+
+  /** \brief Get a pointer to the input point cloud dataset target. */
+  inline PointCloudSourceConstPtr const
+  getInputSource()
+  {
+    return (input_);
+  }
+
+  /** \brief Provide a pointer to the input target
+   * (e.g., the point cloud that we want to align the input source to)
+   * \param[in] cloud the input point cloud target
+   */
+  inline void
+  setInputTarget(const PointCloudTargetConstPtr& cloud);
+
+  /** \brief Get a pointer to the input point cloud dataset target. */
+  inline PointCloudTargetConstPtr const
+  getInputTarget()
+  {
+    return (target_);
+  }
+
+  /** \brief See if this rejector requires source normals */
+  virtual bool
+  requiresSourceNormals() const
+  {
+    return (false);
+  }
+
+  /** \brief Abstract method for setting the source normals */
+  virtual void setSourceNormals(pcl::PCLPointCloud2::ConstPtr /*cloud2*/)
+  {
+    PCL_WARN("[pcl::registration::%s::setSourceNormals] This class does not require "
+             "input source normals\n",
+             getClassName().c_str());
+  }
+
+  /** \brief See if this rejector requires target normals */
+  virtual bool
+  requiresTargetNormals() const
+  {
+    return (false);
+  }
+
+  /** \brief Abstract method for setting the target normals */
+  virtual void setTargetNormals(pcl::PCLPointCloud2::ConstPtr /*cloud2*/)
+  {
+    PCL_WARN("[pcl::registration::%s::setTargetNormals] This class does not require "
+             "input target normals\n",
+             getClassName().c_str());
+  }
+
+  /** \brief Provide a pointer to the vector of indices that represent the
+   * input source point cloud.
+   * \param[in] indices a pointer to the vector of indices
+   */
+  inline void
+  setIndicesSource(const IndicesPtr& indices)
+  {
+    setIndices(indices);
+  }
+
+  /** \brief Get a pointer to the vector of indices used for the source dataset. */
+  inline IndicesPtr const
+  getIndicesSource()
+  {
+    return (indices_);
+  }
+
+  /** \brief Provide a pointer to the vector of indices that represent the input target
+   * point cloud. \param[in] indices a pointer to the vector of indices
+   */
+  inline void
+  setIndicesTarget(const IndicesPtr& indices)
+  {
+    target_cloud_updated_ = true;
+    target_indices_ = indices;
+  }
+
+  /** \brief Get a pointer to the vector of indices used for the target dataset. */
+  inline IndicesPtr const
+  getIndicesTarget()
+  {
+    return (target_indices_);
+  }
 
-namespace pcl
-{
-  namespace registration
+  /** \brief Provide a pointer to the search object used to find correspondences in
+   * the target cloud.
+   * \param[in] tree a pointer to the spatial search object.
+   * \param[in] force_no_recompute If set to true, this tree will NEVER be
+   * recomputed, regardless of calls to setInputTarget. Only use if you are
+   * confident that the tree will be set correctly.
+   */
+  inline void
+  setSearchMethodTarget(const KdTreePtr& tree, bool force_no_recompute = false)
+  {
+    tree_ = tree;
+    force_no_recompute_ = force_no_recompute;
+    // Since we just set a new tree, we need to check for updates
+    target_cloud_updated_ = true;
+  }
+
+  /** \brief Get a pointer to the search method used to find correspondences in the
+   * target cloud. */
+  inline KdTreePtr
+  getSearchMethodTarget() const
+  {
+    return (tree_);
+  }
+
+  /** \brief Provide a pointer to the search object used to find correspondences in
+   * the source cloud (usually used by reciprocal correspondence finding).
+   * \param[in] tree a pointer to the spatial search object.
+   * \param[in] force_no_recompute If set to true, this tree will NEVER be
+   * recomputed, regardless of calls to setInputSource. Only use if you are
+   * extremely confident that the tree will be set correctly.
+   */
+  inline void
+  setSearchMethodSource(const KdTreeReciprocalPtr& tree,
+                        bool force_no_recompute = false)
+  {
+    tree_reciprocal_ = tree;
+    force_no_recompute_reciprocal_ = force_no_recompute;
+    // Since we just set a new tree, we need to check for updates
+    source_cloud_updated_ = true;
+  }
+
+  /** \brief Get a pointer to the search method used to find correspondences in the
+   * source cloud. */
+  inline KdTreeReciprocalPtr
+  getSearchMethodSource() const
+  {
+    return (tree_reciprocal_);
+  }
+
+  /** \brief Determine the correspondences between input and target cloud.
+   * \param[out] correspondences the found correspondences (index of query point, index
+   * of target point, distance) \param[in] max_distance maximum allowed distance between
+   * correspondences
+   */
+  virtual void
+  determineCorrespondences(
+      pcl::Correspondences& correspondences,
+      double max_distance = std::numeric_limits<double>::max()) = 0;
+
+  /** \brief Determine the reciprocal correspondences between input and target cloud.
+   * A correspondence is considered reciprocal if both Src_i has Tgt_i as a
+   * correspondence, and Tgt_i has Src_i as one.
+   *
+   * \param[out] correspondences the found correspondences (index of query and target
+   * point, distance) \param[in] max_distance maximum allowed distance between
+   * correspondences
+   */
+  virtual void
+  determineReciprocalCorrespondences(
+      pcl::Correspondences& correspondences,
+      double max_distance = std::numeric_limits<double>::max()) = 0;
+
+  /** \brief Provide a boost shared pointer to the PointRepresentation to be used
+   * when searching for nearest neighbors.
+   *
+   * \param[in] point_representation the PointRepresentation to be used by the
+   * k-D tree for nearest neighbor search
+   */
+  inline void
+  setPointRepresentation(const PointRepresentationConstPtr& point_representation)
+  {
+    point_representation_ = point_representation;
+  }
+
+  /** \brief Clone and cast to CorrespondenceEstimationBase */
+  virtual typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::Ptr
+  clone() const = 0;
+
+protected:
+  /** \brief The correspondence estimation method name. */
+  std::string corr_name_;
+
+  /** \brief A pointer to the spatial search object used for the target dataset. */
+  KdTreePtr tree_;
+
+  /** \brief A pointer to the spatial search object used for the source dataset. */
+  KdTreeReciprocalPtr tree_reciprocal_;
+
+  /** \brief The input point cloud dataset target. */
+  PointCloudTargetConstPtr target_;
+
+  /** \brief The target point cloud dataset indices. */
+  IndicesPtr target_indices_;
+
+  /** \brief The point representation used (internal). */
+  PointRepresentationConstPtr point_representation_;
+
+  /** \brief The transformed input source point cloud dataset. */
+  PointCloudTargetPtr input_transformed_;
+
+  /** \brief The types of input point fields available. */
+  std::vector<pcl::PCLPointField> input_fields_;
+
+  /** \brief Abstract class get name method. */
+  inline const std::string&
+  getClassName() const
+  {
+    return (corr_name_);
+  }
+
+  /** \brief Internal computation initialization. */
+  bool
+  initCompute();
+
+  /** \brief Internal computation initialization for reciprocal correspondences. */
+  bool
+  initComputeReciprocal();
+
+  /** \brief Variable that stores whether we have a new target cloud, meaning we need to
+   * pre-process it again. This way, we avoid rebuilding the kd-tree for the target
+   * cloud every time the determineCorrespondences () method is called. */
+  bool target_cloud_updated_;
+  /** \brief Variable that stores whether we have a new source cloud, meaning we need to
+   * pre-process it again. This way, we avoid rebuilding the reciprocal kd-tree for the
+   * source cloud every time the determineCorrespondences () method is called. */
+  bool source_cloud_updated_;
+  /** \brief A flag which, if set, means the tree operating on the target cloud
+   * will never be recomputed*/
+  bool force_no_recompute_;
+
+  /** \brief A flag which, if set, means the tree operating on the source cloud
+   * will never be recomputed*/
+  bool force_no_recompute_reciprocal_;
+};
+
+/** \brief @b CorrespondenceEstimation represents the base class for
+ * determining correspondences between target and query point
+ * sets/features.
+ *
+ * Code example:
+ *
+ * \code
+ * pcl::PointCloud<pcl::PointXYZRGBA>::Ptr source, target;
+ * // ... read or fill in source and target
+ * pcl::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ> est;
+ * est.setInputSource (source);
+ * est.setInputTarget (target);
+ *
+ * pcl::Correspondences all_correspondences;
+ * // Determine all reciprocal correspondences
+ * est.determineReciprocalCorrespondences (all_correspondences);
+ * \endcode
+ *
+ * \author Radu B. Rusu, Michael Dixon, Dirk Holz
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename Scalar = float>
+class CorrespondenceEstimation
+: public CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> {
+public:
+  using Ptr = shared_ptr<CorrespondenceEstimation<PointSource, PointTarget, Scalar>>;
+  using ConstPtr =
+      shared_ptr<const CorrespondenceEstimation<PointSource, PointTarget, Scalar>>;
+
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
+      point_representation_;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
+      input_transformed_;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::tree_;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
+      tree_reciprocal_;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::corr_name_;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_indices_;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::getClassName;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
+      initComputeReciprocal;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::input_;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::indices_;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::input_fields_;
+  using PCLBase<PointSource>::deinitCompute;
+
+  using KdTree = pcl::search::KdTree<PointTarget>;
+  using KdTreePtr = typename KdTree::Ptr;
+
+  using PointCloudSource = pcl::PointCloud<PointSource>;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+
+  using PointCloudTarget = pcl::PointCloud<PointTarget>;
+  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
+  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
+
+  using PointRepresentationConstPtr = typename KdTree::PointRepresentationConstPtr;
+
+  /** \brief Empty constructor. */
+  CorrespondenceEstimation() { corr_name_ = "CorrespondenceEstimation"; }
+
+  /** \brief Empty destructor */
+  ~CorrespondenceEstimation() {}
+
+  /** \brief Determine the correspondences between input and target cloud.
+   * \param[out] correspondences the found correspondences (index of query point, index
+   * of target point, distance) \param[in] max_distance maximum allowed distance between
+   * correspondences
+   */
+  void
+  determineCorrespondences(
+      pcl::Correspondences& correspondences,
+      double max_distance = std::numeric_limits<double>::max()) override;
+
+  /** \brief Determine the reciprocal correspondences between input and target cloud.
+   * A correspondence is considered reciprocal if both Src_i has Tgt_i as a
+   * correspondence, and Tgt_i has Src_i as one.
+   *
+   * \param[out] correspondences the found correspondences (index of query and target
+   * point, distance) \param[in] max_distance maximum allowed distance between
+   * correspondences
+   */
+  void
+  determineReciprocalCorrespondences(
+      pcl::Correspondences& correspondences,
+      double max_distance = std::numeric_limits<double>::max()) override;
+
+  /** \brief Clone and cast to CorrespondenceEstimationBase */
+  typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::Ptr
+  clone() const override
   {
-    /** \brief Abstract @b CorrespondenceEstimationBase class. 
-      * All correspondence estimation methods should inherit from this.
-      * \author Radu B. Rusu
-      * \ingroup registration
-      */
-    template <typename PointSource, typename PointTarget, typename Scalar = float>
-    class CorrespondenceEstimationBase: public PCLBase<PointSource>
-    {
-      public:
-        using Ptr = shared_ptr<CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> >;
-        using ConstPtr = shared_ptr<const CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> >;
-
-        // using PCLBase<PointSource>::initCompute;
-        using PCLBase<PointSource>::deinitCompute;
-        using PCLBase<PointSource>::input_;
-        using PCLBase<PointSource>::indices_;
-        using PCLBase<PointSource>::setIndices;
-
-        using KdTree = pcl::search::KdTree<PointTarget>;
-        using KdTreePtr = typename KdTree::Ptr;
-
-        using KdTreeReciprocal = pcl::search::KdTree<PointSource>;
-        using KdTreeReciprocalPtr = typename KdTree::Ptr;
-
-        using PointCloudSource = pcl::PointCloud<PointSource>;
-        using PointCloudSourcePtr = typename PointCloudSource::Ptr;
-        using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
-
-        using PointCloudTarget = pcl::PointCloud<PointTarget>;
-        using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
-        using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
-
-        using PointRepresentationConstPtr = typename KdTree::PointRepresentationConstPtr;
-
-        /** \brief Empty constructor. */
-        CorrespondenceEstimationBase () 
-          : corr_name_ ("CorrespondenceEstimationBase")
-          , tree_ (new pcl::search::KdTree<PointTarget>)
-          , tree_reciprocal_ (new pcl::search::KdTree<PointSource>)
-          , target_ ()
-          , point_representation_ ()
-          , input_transformed_ ()
-          , target_cloud_updated_ (true)
-          , source_cloud_updated_ (true)
-          , force_no_recompute_ (false)
-          , force_no_recompute_reciprocal_ (false)
-        {
-        }
-      
-        /** \brief Empty destructor */
-        ~CorrespondenceEstimationBase () {}
-
-        /** \brief Provide a pointer to the input source 
-          * (e.g., the point cloud that we want to align to the target)
-          *
-          * \param[in] cloud the input point cloud source
-          */
-        inline void 
-        setInputSource (const PointCloudSourceConstPtr &cloud)
-        {
-          source_cloud_updated_ = true;
-          PCLBase<PointSource>::setInputCloud (cloud);
-          input_fields_ = pcl::getFields<PointSource> ();
-        }
-
-        /** \brief Get a pointer to the input point cloud dataset target. */
-        inline PointCloudSourceConstPtr const 
-        getInputSource () 
-        { 
-          return (input_ ); 
-        }
-
-        /** \brief Provide a pointer to the input target 
-          * (e.g., the point cloud that we want to align the input source to)
-          * \param[in] cloud the input point cloud target
-          */
-        inline void 
-        setInputTarget (const PointCloudTargetConstPtr &cloud);
-
-        /** \brief Get a pointer to the input point cloud dataset target. */
-        inline PointCloudTargetConstPtr const 
-        getInputTarget () { return (target_ ); }
-
-
-        /** \brief See if this rejector requires source normals */
-        virtual bool
-        requiresSourceNormals () const
-        { return (false); }
-
-        /** \brief Abstract method for setting the source normals */
-        virtual void
-        setSourceNormals (pcl::PCLPointCloud2::ConstPtr /*cloud2*/)
-        {
-          PCL_WARN ("[pcl::registration::%s::setSourceNormals] This class does not require input source normals", getClassName ().c_str ());
-        }
-        
-        /** \brief See if this rejector requires target normals */
-        virtual bool
-        requiresTargetNormals () const
-        { return (false); }
-
-        /** \brief Abstract method for setting the target normals */
-        virtual void
-        setTargetNormals (pcl::PCLPointCloud2::ConstPtr /*cloud2*/)
-        {
-          PCL_WARN ("[pcl::registration::%s::setTargetNormals] This class does not require input target normals", getClassName ().c_str ());
-        }
-
-        /** \brief Provide a pointer to the vector of indices that represent the 
-          * input source point cloud.
-          * \param[in] indices a pointer to the vector of indices 
-          */
-        inline void
-        setIndicesSource (const IndicesPtr &indices)
-        {
-          setIndices (indices);
-        }
-
-        /** \brief Get a pointer to the vector of indices used for the source dataset. */
-        inline IndicesPtr const 
-        getIndicesSource () { return (indices_); }
-
-        /** \brief Provide a pointer to the vector of indices that represent the input target point cloud.
-          * \param[in] indices a pointer to the vector of indices 
-          */
-        inline void
-        setIndicesTarget (const IndicesPtr &indices)
-        {
-          target_cloud_updated_ = true;
-          target_indices_ = indices;
-        }
-
-        /** \brief Get a pointer to the vector of indices used for the target dataset. */
-        inline IndicesPtr const 
-        getIndicesTarget () { return (target_indices_); }
-
-        /** \brief Provide a pointer to the search object used to find correspondences in
-          * the target cloud.
-          * \param[in] tree a pointer to the spatial search object.
-          * \param[in] force_no_recompute If set to true, this tree will NEVER be 
-          * recomputed, regardless of calls to setInputTarget. Only use if you are 
-          * confident that the tree will be set correctly.
-          */
-        inline void
-        setSearchMethodTarget (const KdTreePtr &tree, 
-                               bool force_no_recompute = false) 
-        { 
-          tree_ = tree; 
-          if (force_no_recompute)
-          {
-            force_no_recompute_ = true;
-          }
-          // Since we just set a new tree, we need to check for updates
-          target_cloud_updated_ = true;
-        }
-
-        /** \brief Get a pointer to the search method used to find correspondences in the
-          * target cloud. */
-        inline KdTreePtr
-        getSearchMethodTarget () const
-        {
-          return (tree_);
-        }
-
-        /** \brief Provide a pointer to the search object used to find correspondences in
-          * the source cloud (usually used by reciprocal correspondence finding).
-          * \param[in] tree a pointer to the spatial search object.
-          * \param[in] force_no_recompute If set to true, this tree will NEVER be 
-          * recomputed, regardless of calls to setInputSource. Only use if you are 
-          * extremely confident that the tree will be set correctly.
-          */
-        inline void
-        setSearchMethodSource (const KdTreeReciprocalPtr &tree, 
-                               bool force_no_recompute = false) 
-        { 
-          tree_reciprocal_ = tree; 
-          if ( force_no_recompute )
-          {
-            force_no_recompute_reciprocal_ = true;
-          }
-          // Since we just set a new tree, we need to check for updates
-          source_cloud_updated_ = true;
-        }
-
-        /** \brief Get a pointer to the search method used to find correspondences in the
-          * source cloud. */
-        inline KdTreeReciprocalPtr
-        getSearchMethodSource () const
-        {
-          return (tree_reciprocal_);
-        }
-
-        /** \brief Determine the correspondences between input and target cloud.
-          * \param[out] correspondences the found correspondences (index of query point, index of target point, distance)
-          * \param[in] max_distance maximum allowed distance between correspondences
-          */
-        virtual void 
-        determineCorrespondences (pcl::Correspondences &correspondences,
-                                  double max_distance = std::numeric_limits<double>::max ()) = 0;
-
-        /** \brief Determine the reciprocal correspondences between input and target cloud.
-          * A correspondence is considered reciprocal if both Src_i has Tgt_i as a 
-          * correspondence, and Tgt_i has Src_i as one.
-          *
-          * \param[out] correspondences the found correspondences (index of query and target point, distance)
-          * \param[in] max_distance maximum allowed distance between correspondences
-          */
-        virtual void 
-        determineReciprocalCorrespondences (pcl::Correspondences &correspondences,
-                                            double max_distance = std::numeric_limits<double>::max ()) = 0;
-
-        /** \brief Provide a boost shared pointer to the PointRepresentation to be used 
-          * when searching for nearest neighbors.
-          *
-          * \param[in] point_representation the PointRepresentation to be used by the 
-          * k-D tree for nearest neighbor search
-          */
-        inline void
-        setPointRepresentation (const PointRepresentationConstPtr &point_representation)
-        {
-          point_representation_ = point_representation;
-        }
-
-        /** \brief Clone and cast to CorrespondenceEstimationBase */
-        virtual typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::Ptr clone () const = 0;
-
-      protected:
-        /** \brief The correspondence estimation method name. */
-        std::string corr_name_;
-
-        /** \brief A pointer to the spatial search object used for the target dataset. */
-        KdTreePtr tree_;
-
-        /** \brief A pointer to the spatial search object used for the source dataset. */
-        KdTreeReciprocalPtr tree_reciprocal_;
-
-
-        
-        /** \brief The input point cloud dataset target. */
-        PointCloudTargetConstPtr target_;
-
-        /** \brief The target point cloud dataset indices. */
-        IndicesPtr target_indices_;
-
-        /** \brief The point representation used (internal). */
-        PointRepresentationConstPtr point_representation_;
-
-        /** \brief The transformed input source point cloud dataset. */
-        PointCloudTargetPtr input_transformed_;
-
-        /** \brief The types of input point fields available. */
-        std::vector<pcl::PCLPointField> input_fields_;
-
-        /** \brief Abstract class get name method. */
-        inline const std::string& 
-        getClassName () const { return (corr_name_); }
-
-        /** \brief Internal computation initialization. */
-        bool
-        initCompute ();
-        
-        /** \brief Internal computation initialization for reciprocal correspondences. */
-        bool
-        initComputeReciprocal ();
-
-        /** \brief Variable that stores whether we have a new target cloud, meaning we need to pre-process it again.
-         * This way, we avoid rebuilding the kd-tree for the target cloud every time the determineCorrespondences () method
-         * is called. */
-        bool target_cloud_updated_;
-        /** \brief Variable that stores whether we have a new source cloud, meaning we need to pre-process it again.
-         * This way, we avoid rebuilding the reciprocal kd-tree for the source cloud every time the determineCorrespondences () method
-         * is called. */
-        bool source_cloud_updated_;
-        /** \brief A flag which, if set, means the tree operating on the target cloud 
-         * will never be recomputed*/
-        bool force_no_recompute_;
-        
-        /** \brief A flag which, if set, means the tree operating on the source cloud 
-         * will never be recomputed*/
-        bool force_no_recompute_reciprocal_;
-
-     };
-
-    /** \brief @b CorrespondenceEstimation represents the base class for
-      * determining correspondences between target and query point
-      * sets/features.
-      *
-      * Code example:
-      *
-      * \code
-      * pcl::PointCloud<pcl::PointXYZRGBA>::Ptr source, target;
-      * // ... read or fill in source and target
-      * pcl::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ> est;
-      * est.setInputSource (source);
-      * est.setInputTarget (target);
-      *
-      * pcl::Correspondences all_correspondences;
-      * // Determine all reciprocal correspondences
-      * est.determineReciprocalCorrespondences (all_correspondences);
-      * \endcode
-      *
-      * \author Radu B. Rusu, Michael Dixon, Dirk Holz
-      * \ingroup registration
-      */
-    template <typename PointSource, typename PointTarget, typename Scalar = float>
-    class CorrespondenceEstimation : public CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>
-    {
-      public:
-        using Ptr = shared_ptr<CorrespondenceEstimation<PointSource, PointTarget, Scalar> >;
-        using ConstPtr = shared_ptr<const CorrespondenceEstimation<PointSource, PointTarget, Scalar> >;
-
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::point_representation_;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::input_transformed_;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::tree_;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::tree_reciprocal_;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::corr_name_;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_indices_;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::getClassName;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initComputeReciprocal;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::input_;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::indices_;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::input_fields_;
-        using PCLBase<PointSource>::deinitCompute;
-
-        using KdTree = pcl::search::KdTree<PointTarget>;
-        using KdTreePtr = typename KdTree::Ptr;
-
-        using PointCloudSource = pcl::PointCloud<PointSource>;
-        using PointCloudSourcePtr = typename PointCloudSource::Ptr;
-        using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
-
-        using PointCloudTarget = pcl::PointCloud<PointTarget>;
-        using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
-        using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
-
-        using PointRepresentationConstPtr = typename KdTree::PointRepresentationConstPtr;
-
-        /** \brief Empty constructor. */
-        CorrespondenceEstimation () 
-        {
-          corr_name_  = "CorrespondenceEstimation";
-        }
-      
-        /** \brief Empty destructor */
-        ~CorrespondenceEstimation () {}
-
-        /** \brief Determine the correspondences between input and target cloud.
-          * \param[out] correspondences the found correspondences (index of query point, index of target point, distance)
-          * \param[in] max_distance maximum allowed distance between correspondences
-          */
-        void 
-        determineCorrespondences (pcl::Correspondences &correspondences,
-                                  double max_distance = std::numeric_limits<double>::max ()) override;
-
-        /** \brief Determine the reciprocal correspondences between input and target cloud.
-          * A correspondence is considered reciprocal if both Src_i has Tgt_i as a 
-          * correspondence, and Tgt_i has Src_i as one.
-          *
-          * \param[out] correspondences the found correspondences (index of query and target point, distance)
-          * \param[in] max_distance maximum allowed distance between correspondences
-          */
-        void 
-        determineReciprocalCorrespondences (pcl::Correspondences &correspondences,
-                                            double max_distance = std::numeric_limits<double>::max ()) override;
-
-        
-        /** \brief Clone and cast to CorrespondenceEstimationBase */
-        typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::Ptr
-        clone () const override
-        {
-          Ptr copy (new CorrespondenceEstimation<PointSource, PointTarget, Scalar> (*this));
-          return (copy);
-        }
-     };
+    Ptr copy(new CorrespondenceEstimation<PointSource, PointTarget, Scalar>(*this));
+    return (copy);
   }
-}
+};
+} // namespace registration
+} // namespace pcl
 
 #include <pcl/registration/impl/correspondence_estimation.hpp>
diff --git a/registration/include/pcl/registration/correspondence_estimation_backprojection.h b/registration/include/pcl/registration/correspondence_estimation_backprojection.h
index 526e81b8..344d692a 100644
--- a/registration/include/pcl/registration/correspondence_estimation_backprojection.h
+++ b/registration/include/pcl/registration/correspondence_estimation_backprojection.h
@@ -39,187 +39,220 @@
 
 #pragma once
 
-#include <pcl/registration/correspondence_types.h>
 #include <pcl/registration/correspondence_estimation.h>
+#include <pcl/registration/correspondence_types.h>
+
+namespace pcl {
+namespace registration {
+/** \brief @b CorrespondenceEstimationBackprojection computes
+ * correspondences as points in the target cloud which have minimum
+ * \author Suat Gedikli
+ * \ingroup registration
+ */
+template <typename PointSource,
+          typename PointTarget,
+          typename NormalT,
+          typename Scalar = float>
+class CorrespondenceEstimationBackProjection
+: public CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> {
+public:
+  using Ptr = shared_ptr<CorrespondenceEstimationBackProjection<PointSource,
+                                                                PointTarget,
+                                                                NormalT,
+                                                                Scalar>>;
+  using ConstPtr = shared_ptr<const CorrespondenceEstimationBackProjection<PointSource,
+                                                                           PointTarget,
+                                                                           NormalT,
+                                                                           Scalar>>;
+
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
+      initComputeReciprocal;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
+      input_transformed_;
+  using PCLBase<PointSource>::deinitCompute;
+  using PCLBase<PointSource>::input_;
+  using PCLBase<PointSource>::indices_;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::getClassName;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
+      point_representation_;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_indices_;
+
+  using KdTree = pcl::search::KdTree<PointTarget>;
+  using KdTreePtr = typename KdTree::Ptr;
+
+  using PointCloudSource = pcl::PointCloud<PointSource>;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+
+  using PointCloudTarget = pcl::PointCloud<PointTarget>;
+  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
+  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
+
+  using PointCloudNormals = pcl::PointCloud<NormalT>;
+  using NormalsPtr = typename PointCloudNormals::Ptr;
+  using NormalsConstPtr = typename PointCloudNormals::ConstPtr;
+
+  /** \brief Empty constructor.
+   *
+   * \note
+   * Sets the number of neighbors to be considered in the target point cloud (k_) to 10.
+   */
+  CorrespondenceEstimationBackProjection()
+  : source_normals_(), source_normals_transformed_(), target_normals_(), k_(10)
+  {
+    corr_name_ = "CorrespondenceEstimationBackProjection";
+  }
+
+  /** \brief Empty destructor */
+  virtual ~CorrespondenceEstimationBackProjection() {}
 
-namespace pcl
-{
-  namespace registration
+  /** \brief Set the normals computed on the source point cloud
+   * \param[in] normals the normals computed for the source cloud
+   */
+  inline void
+  setSourceNormals(const NormalsConstPtr& normals)
   {
-    /** \brief @b CorrespondenceEstimationBackprojection computes
-      * correspondences as points in the target cloud which have minimum
-      * \author Suat Gedikli
-      * \ingroup registration
-      */
-    template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar = float>
-    class CorrespondenceEstimationBackProjection : public CorrespondenceEstimationBase <PointSource, PointTarget, Scalar>
-    {
-      public:
-        using Ptr = shared_ptr<CorrespondenceEstimationBackProjection<PointSource, PointTarget, NormalT, Scalar> >;
-        using ConstPtr = shared_ptr<const CorrespondenceEstimationBackProjection<PointSource, PointTarget, NormalT, Scalar> >;
-
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initComputeReciprocal;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::input_transformed_;
-        using PCLBase<PointSource>::deinitCompute;
-        using PCLBase<PointSource>::input_;
-        using PCLBase<PointSource>::indices_;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::getClassName;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::point_representation_;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_indices_;
-
-        using KdTree = pcl::search::KdTree<PointTarget>;
-        using KdTreePtr = typename KdTree::Ptr;
-
-        using PointCloudSource = pcl::PointCloud<PointSource>;
-        using PointCloudSourcePtr = typename PointCloudSource::Ptr;
-        using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
-
-        using PointCloudTarget = pcl::PointCloud<PointTarget>;
-        using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
-        using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
-
-        using PointCloudNormals = pcl::PointCloud<NormalT>;
-        using NormalsPtr = typename PointCloudNormals::Ptr;
-        using NormalsConstPtr = typename PointCloudNormals::ConstPtr;
-
-        /** \brief Empty constructor. 
-          *
-          * \note
-          * Sets the number of neighbors to be considered in the target point cloud (k_) to 10.
-          */
-        CorrespondenceEstimationBackProjection ()
-          : source_normals_ ()
-          , source_normals_transformed_ ()
-          , target_normals_ ()
-          , k_ (10)
-        {
-          corr_name_ = "CorrespondenceEstimationBackProjection";
-        }
-      
-        /** \brief Empty destructor */
-        virtual ~CorrespondenceEstimationBackProjection () {}
-
-        /** \brief Set the normals computed on the source point cloud
-          * \param[in] normals the normals computed for the source cloud
-          */
-        inline void
-        setSourceNormals (const NormalsConstPtr &normals) { source_normals_ = normals; }
-
-        /** \brief Get the normals of the source point cloud
-          */
-        inline NormalsConstPtr
-        getSourceNormals () const { return (source_normals_); }
-
-        /** \brief Set the normals computed on the target point cloud
-          * \param[in] normals the normals computed for the target cloud
-          */
-        inline void
-        setTargetNormals (const NormalsConstPtr &normals) { target_normals_ = normals; }
-
-        /** \brief Get the normals of the target point cloud
-          */
-        inline NormalsConstPtr
-        getTargetNormals () const { return (target_normals_); }
-
-
-        /** \brief See if this rejector requires source normals */
-        bool
-        requiresSourceNormals () const
-        { return (true); }
-
-        /** \brief Blob method for setting the source normals */
-        void
-        setSourceNormals (pcl::PCLPointCloud2::ConstPtr cloud2)
-        { 
-          NormalsPtr cloud (new PointCloudNormals);
-          fromPCLPointCloud2 (*cloud2, *cloud);
-          setSourceNormals (cloud);
-        }
-        
-        /** \brief See if this rejector requires target normals*/
-        bool
-        requiresTargetNormals () const
-        { return (true); }
-
-        /** \brief Method for setting the target normals */
-        void
-        setTargetNormals (pcl::PCLPointCloud2::ConstPtr cloud2)
-        { 
-          NormalsPtr cloud (new PointCloudNormals);
-          fromPCLPointCloud2 (*cloud2, *cloud);
-          setTargetNormals (cloud);
-        }
-
-        /** \brief Determine the correspondences between input and target cloud.
-          * \param[out] correspondences the found correspondences (index of query point, index of target point, distance)
-          * \param[in] max_distance maximum distance between the normal on the source point cloud and the corresponding point in the target
-          * point cloud
-          */
-        void 
-        determineCorrespondences (pcl::Correspondences &correspondences,
-                                  double max_distance = std::numeric_limits<double>::max ());
-
-        /** \brief Determine the reciprocal correspondences between input and target cloud.
-          * A correspondence is considered reciprocal if both Src_i has Tgt_i as a 
-          * correspondence, and Tgt_i has Src_i as one.
-          *
-          * \param[out] correspondences the found correspondences (index of query and target point, distance)
-          * \param[in] max_distance maximum allowed distance between correspondences
-          */
-        virtual void 
-        determineReciprocalCorrespondences (pcl::Correspondences &correspondences,
-                                            double max_distance = std::numeric_limits<double>::max ());
-
-        /** \brief Set the number of nearest neighbours to be considered in the target 
-          * point cloud. By default, we use k = 10 nearest neighbors.
-          *
-          * \param[in] k the number of nearest neighbours to be considered
-          */
-        inline void
-        setKSearch (unsigned int k) { k_ = k; }
-
-        /** \brief Get the number of nearest neighbours considered in the target point 
-          * cloud for computing correspondences. By default we use k = 10 nearest 
-          * neighbors.
-          */
-        inline unsigned int
-        getKSearch () const { return (k_); }
-        
-        /** \brief Clone and cast to CorrespondenceEstimationBase */
-        virtual typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::Ptr
-        clone () const
-        {
-          Ptr copy (new CorrespondenceEstimationBackProjection<PointSource, PointTarget, NormalT, Scalar> (*this));
-          return (copy);
-        }
-
-      protected:
-
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::corr_name_;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::tree_;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::tree_reciprocal_;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_;
-
-        /** \brief Internal computation initialization. */
-        bool
-        initCompute ();
-
-      private:
-
-        /** \brief The normals computed at each point in the source cloud */
-        NormalsConstPtr source_normals_; 
-
-        /** \brief The normals computed at each point in the source cloud */
-        NormalsPtr source_normals_transformed_;
-
-        /** \brief The normals computed at each point in the target cloud */
-        NormalsConstPtr target_normals_; 
-
-        /** \brief The number of neighbours to be considered in the target point cloud */
-        unsigned int k_;
-    };
+    source_normals_ = normals;
   }
-}
+
+  /** \brief Get the normals of the source point cloud
+   */
+  inline NormalsConstPtr
+  getSourceNormals() const
+  {
+    return (source_normals_);
+  }
+
+  /** \brief Set the normals computed on the target point cloud
+   * \param[in] normals the normals computed for the target cloud
+   */
+  inline void
+  setTargetNormals(const NormalsConstPtr& normals)
+  {
+    target_normals_ = normals;
+  }
+
+  /** \brief Get the normals of the target point cloud
+   */
+  inline NormalsConstPtr
+  getTargetNormals() const
+  {
+    return (target_normals_);
+  }
+
+  /** \brief See if this rejector requires source normals */
+  bool
+  requiresSourceNormals() const
+  {
+    return (true);
+  }
+
+  /** \brief Blob method for setting the source normals */
+  void
+  setSourceNormals(pcl::PCLPointCloud2::ConstPtr cloud2)
+  {
+    NormalsPtr cloud(new PointCloudNormals);
+    fromPCLPointCloud2(*cloud2, *cloud);
+    setSourceNormals(cloud);
+  }
+
+  /** \brief See if this rejector requires target normals*/
+  bool
+  requiresTargetNormals() const
+  {
+    return (true);
+  }
+
+  /** \brief Method for setting the target normals */
+  void
+  setTargetNormals(pcl::PCLPointCloud2::ConstPtr cloud2)
+  {
+    NormalsPtr cloud(new PointCloudNormals);
+    fromPCLPointCloud2(*cloud2, *cloud);
+    setTargetNormals(cloud);
+  }
+
+  /** \brief Determine the correspondences between input and target cloud.
+   * \param[out] correspondences the found correspondences (index of query point, index
+   * of target point, distance) \param[in] max_distance maximum distance between the
+   * normal on the source point cloud and the corresponding point in the target point
+   * cloud
+   */
+  void
+  determineCorrespondences(pcl::Correspondences& correspondences,
+                           double max_distance = std::numeric_limits<double>::max());
+
+  /** \brief Determine the reciprocal correspondences between input and target cloud.
+   * A correspondence is considered reciprocal if both Src_i has Tgt_i as a
+   * correspondence, and Tgt_i has Src_i as one.
+   *
+   * \param[out] correspondences the found correspondences (index of query and target
+   * point, distance) \param[in] max_distance maximum allowed distance between
+   * correspondences
+   */
+  virtual void
+  determineReciprocalCorrespondences(
+      pcl::Correspondences& correspondences,
+      double max_distance = std::numeric_limits<double>::max());
+
+  /** \brief Set the number of nearest neighbours to be considered in the target
+   * point cloud. By default, we use k = 10 nearest neighbors.
+   *
+   * \param[in] k the number of nearest neighbours to be considered
+   */
+  inline void
+  setKSearch(unsigned int k)
+  {
+    k_ = k;
+  }
+
+  /** \brief Get the number of nearest neighbours considered in the target point
+   * cloud for computing correspondences. By default we use k = 10 nearest
+   * neighbors.
+   */
+  inline unsigned int
+  getKSearch() const
+  {
+    return (k_);
+  }
+
+  /** \brief Clone and cast to CorrespondenceEstimationBase */
+  virtual typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::Ptr
+  clone() const
+  {
+    Ptr copy(new CorrespondenceEstimationBackProjection<PointSource,
+                                                        PointTarget,
+                                                        NormalT,
+                                                        Scalar>(*this));
+    return (copy);
+  }
+
+protected:
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::corr_name_;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::tree_;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
+      tree_reciprocal_;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_;
+
+  /** \brief Internal computation initialization. */
+  bool
+  initCompute();
+
+private:
+  /** \brief The normals computed at each point in the source cloud */
+  NormalsConstPtr source_normals_;
+
+  /** \brief The normals computed at each point in the source cloud */
+  NormalsPtr source_normals_transformed_;
+
+  /** \brief The normals computed at each point in the target cloud */
+  NormalsConstPtr target_normals_;
+
+  /** \brief The number of neighbours to be considered in the target point cloud */
+  unsigned int k_;
+};
+} // namespace registration
+} // namespace pcl
 
 #include <pcl/registration/impl/correspondence_estimation_backprojection.hpp>
diff --git a/registration/include/pcl/registration/correspondence_estimation_normal_shooting.h b/registration/include/pcl/registration/correspondence_estimation_normal_shooting.h
index 4e8f4b8b..b73128f3 100644
--- a/registration/include/pcl/registration/correspondence_estimation_normal_shooting.h
+++ b/registration/include/pcl/registration/correspondence_estimation_normal_shooting.h
@@ -40,179 +40,207 @@
 
 #pragma once
 
-#include <pcl/registration/correspondence_types.h>
 #include <pcl/registration/correspondence_estimation.h>
+#include <pcl/registration/correspondence_types.h>
+
+namespace pcl {
+namespace registration {
+/** \brief @b CorrespondenceEstimationNormalShooting computes
+ * correspondences as points in the target cloud which have minimum
+ * distance to normals computed on the input cloud
+ *
+ * Code example:
+ *
+ * \code
+ * pcl::PointCloud<pcl::PointNormal>::Ptr source, target;
+ * // ... read or fill in source and target
+ * pcl::CorrespondenceEstimationNormalShooting
+ *   <pcl::PointNormal, pcl::PointNormal, pcl::PointNormal> est;
+ * est.setInputSource (source);
+ * est.setSourceNormals (source);
+ *
+ * est.setInputTarget (target);
+ *
+ * // Test the first 10 correspondences for each point in source, and return the best
+ * est.setKSearch (10);
+ *
+ * pcl::Correspondences all_correspondences;
+ * // Determine all correspondences
+ * est.determineCorrespondences (all_correspondences);
+ * \endcode
+ *
+ * \author Aravindhan K. Krishnan, Radu B. Rusu
+ * \ingroup registration
+ */
+template <typename PointSource,
+          typename PointTarget,
+          typename NormalT,
+          typename Scalar = float>
+class CorrespondenceEstimationNormalShooting
+: public CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> {
+public:
+  using Ptr = shared_ptr<CorrespondenceEstimationNormalShooting<PointSource,
+                                                                PointTarget,
+                                                                NormalT,
+                                                                Scalar>>;
+  using ConstPtr = shared_ptr<const CorrespondenceEstimationNormalShooting<PointSource,
+                                                                           PointTarget,
+                                                                           NormalT,
+                                                                           Scalar>>;
+
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
+      initComputeReciprocal;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
+      input_transformed_;
+  using PCLBase<PointSource>::deinitCompute;
+  using PCLBase<PointSource>::input_;
+  using PCLBase<PointSource>::indices_;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::getClassName;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
+      point_representation_;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_indices_;
+
+  using KdTree = pcl::search::KdTree<PointTarget>;
+  using KdTreePtr = typename KdTree::Ptr;
+
+  using PointCloudSource = pcl::PointCloud<PointSource>;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+
+  using PointCloudTarget = pcl::PointCloud<PointTarget>;
+  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
+  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
+
+  using PointCloudNormals = pcl::PointCloud<NormalT>;
+  using NormalsPtr = typename PointCloudNormals::Ptr;
+  using NormalsConstPtr = typename PointCloudNormals::ConstPtr;
+
+  /** \brief Empty constructor.
+   *
+   * \note
+   * Sets the number of neighbors to be considered in the target point cloud (k_) to 10.
+   */
+  CorrespondenceEstimationNormalShooting()
+  : source_normals_(), source_normals_transformed_(), k_(10)
+  {
+    corr_name_ = "CorrespondenceEstimationNormalShooting";
+  }
+
+  /** \brief Empty destructor */
+  ~CorrespondenceEstimationNormalShooting() {}
+
+  /** \brief Set the normals computed on the source point cloud
+   * \param[in] normals the normals computed for the source cloud
+   */
+  inline void
+  setSourceNormals(const NormalsConstPtr& normals)
+  {
+    source_normals_ = normals;
+  }
 
-namespace pcl
-{
-  namespace registration
+  /** \brief Get the normals of the source point cloud
+   */
+  inline NormalsConstPtr
+  getSourceNormals() const
   {
-    /** \brief @b CorrespondenceEstimationNormalShooting computes
-      * correspondences as points in the target cloud which have minimum
-      * distance to normals computed on the input cloud
-      *
-      * Code example:
-      *
-      * \code
-      * pcl::PointCloud<pcl::PointNormal>::Ptr source, target;
-      * // ... read or fill in source and target
-      * pcl::CorrespondenceEstimationNormalShooting<pcl::PointNormal, pcl::PointNormal, pcl::PointNormal> est;
-      * est.setInputSource (source);
-      * est.setSourceNormals (source);
-      *
-      * est.setInputTarget (target);
-      *
-      * // Test the first 10 correspondences for each point in source, and return the best
-      * est.setKSearch (10);
-      *
-      * pcl::Correspondences all_correspondences;
-      * // Determine all correspondences
-      * est.determineCorrespondences (all_correspondences);
-      * \endcode
-      * 
-      * \author Aravindhan K. Krishnan, Radu B. Rusu
-      * \ingroup registration
-      */
-    template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar = float>
-    class CorrespondenceEstimationNormalShooting : public CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>
-    {
-      public:
-        using Ptr = shared_ptr<CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar> >;
-        using ConstPtr = shared_ptr<const CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar> >;
-
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initComputeReciprocal;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::input_transformed_;
-        using PCLBase<PointSource>::deinitCompute;
-        using PCLBase<PointSource>::input_;
-        using PCLBase<PointSource>::indices_;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::getClassName;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::point_representation_;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_indices_;
-
-        using KdTree = pcl::search::KdTree<PointTarget>;
-        using KdTreePtr = typename KdTree::Ptr;
-
-        using PointCloudSource = pcl::PointCloud<PointSource>;
-        using PointCloudSourcePtr = typename PointCloudSource::Ptr;
-        using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
-
-        using PointCloudTarget = pcl::PointCloud<PointTarget>;
-        using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
-        using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
-
-        using PointCloudNormals = pcl::PointCloud<NormalT>;
-        using NormalsPtr = typename PointCloudNormals::Ptr;
-        using NormalsConstPtr = typename PointCloudNormals::ConstPtr;
-
-        /** \brief Empty constructor. 
-          *
-          * \note
-          * Sets the number of neighbors to be considered in the target point cloud (k_) to 10.
-          */
-        CorrespondenceEstimationNormalShooting ()
-          : source_normals_ ()
-          , source_normals_transformed_ ()
-          , k_ (10)
-        {
-          corr_name_ = "CorrespondenceEstimationNormalShooting";
-        }
-
-        /** \brief Empty destructor */
-        ~CorrespondenceEstimationNormalShooting () {}
-
-        /** \brief Set the normals computed on the source point cloud
-          * \param[in] normals the normals computed for the source cloud
-          */
-        inline void
-        setSourceNormals (const NormalsConstPtr &normals) { source_normals_ = normals; }
-
-        /** \brief Get the normals of the source point cloud
-          */
-        inline NormalsConstPtr
-        getSourceNormals () const { return (source_normals_); }
-
-
-        /** \brief See if this rejector requires source normals */
-        bool
-        requiresSourceNormals () const override
-        { return (true); }
-
-        /** \brief Blob method for setting the source normals */
-        void
-        setSourceNormals (pcl::PCLPointCloud2::ConstPtr cloud2) override
-        { 
-          NormalsPtr cloud (new PointCloudNormals);
-          fromPCLPointCloud2 (*cloud2, *cloud);
-          setSourceNormals (cloud);
-        }
-
-        /** \brief Determine the correspondences between input and target cloud.
-          * \param[out] correspondences the found correspondences (index of query point, index of target point, distance)
-          * \param[in] max_distance maximum distance between the normal on the source point cloud and the corresponding point in the target
-          * point cloud
-          */
-        void 
-        determineCorrespondences (pcl::Correspondences &correspondences,
-                                  double max_distance = std::numeric_limits<double>::max ()) override;
-
-        /** \brief Determine the reciprocal correspondences between input and target cloud.
-          * A correspondence is considered reciprocal if both Src_i has Tgt_i as a 
-          * correspondence, and Tgt_i has Src_i as one.
-          *
-          * \param[out] correspondences the found correspondences (index of query and target point, distance)
-          * \param[in] max_distance maximum allowed distance between correspondences
-          */
-        void 
-        determineReciprocalCorrespondences (pcl::Correspondences &correspondences,
-                                            double max_distance = std::numeric_limits<double>::max ()) override;
-
-        /** \brief Set the number of nearest neighbours to be considered in the target 
-          * point cloud. By default, we use k = 10 nearest neighbors.
-          *
-          * \param[in] k the number of nearest neighbours to be considered
-          */
-        inline void
-        setKSearch (unsigned int k) { k_ = k; }
-
-        /** \brief Get the number of nearest neighbours considered in the target point 
-          * cloud for computing correspondences. By default we use k = 10 nearest 
-          * neighbors.
-          */
-        inline unsigned int
-        getKSearch () const { return (k_); }
-
-        /** \brief Clone and cast to CorrespondenceEstimationBase */
-        typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::Ptr
-        clone () const override
-        {
-          Ptr copy (new CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar> (*this));
-          return (copy);
-        }
-
-      protected:
-
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::corr_name_;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::tree_;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::tree_reciprocal_;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_;
-
-        /** \brief Internal computation initialization. */
-        bool
-        initCompute ();
-
-       private:
-
-        /** \brief The normals computed at each point in the source cloud */
-        NormalsConstPtr source_normals_;
-
-        /** \brief The normals computed at each point in the source cloud */
-        NormalsPtr source_normals_transformed_;
-
-        /** \brief The number of neighbours to be considered in the target point cloud */
-        unsigned int k_;
-    };
+    return (source_normals_);
   }
-}
+
+  /** \brief See if this rejector requires source normals */
+  bool
+  requiresSourceNormals() const override
+  {
+    return (true);
+  }
+
+  /** \brief Blob method for setting the source normals */
+  void
+  setSourceNormals(pcl::PCLPointCloud2::ConstPtr cloud2) override
+  {
+    NormalsPtr cloud(new PointCloudNormals);
+    fromPCLPointCloud2(*cloud2, *cloud);
+    setSourceNormals(cloud);
+  }
+
+  /** \brief Determine the correspondences between input and target cloud.
+   * \param[out] correspondences the found correspondences (index of query point, index
+   * of target point, distance) \param[in] max_distance maximum distance between the
+   * normal on the source point cloud and the corresponding point in the target point
+   * cloud
+   */
+  void
+  determineCorrespondences(
+      pcl::Correspondences& correspondences,
+      double max_distance = std::numeric_limits<double>::max()) override;
+
+  /** \brief Determine the reciprocal correspondences between input and target cloud.
+   * A correspondence is considered reciprocal if both Src_i has Tgt_i as a
+   * correspondence, and Tgt_i has Src_i as one.
+   *
+   * \param[out] correspondences the found correspondences (index of query and target
+   * point, distance) \param[in] max_distance maximum allowed distance between
+   * correspondences
+   */
+  void
+  determineReciprocalCorrespondences(
+      pcl::Correspondences& correspondences,
+      double max_distance = std::numeric_limits<double>::max()) override;
+
+  /** \brief Set the number of nearest neighbours to be considered in the target
+   * point cloud. By default, we use k = 10 nearest neighbors.
+   *
+   * \param[in] k the number of nearest neighbours to be considered
+   */
+  inline void
+  setKSearch(unsigned int k)
+  {
+    k_ = k;
+  }
+
+  /** \brief Get the number of nearest neighbours considered in the target point
+   * cloud for computing correspondences. By default we use k = 10 nearest
+   * neighbors.
+   */
+  inline unsigned int
+  getKSearch() const
+  {
+    return (k_);
+  }
+
+  /** \brief Clone and cast to CorrespondenceEstimationBase */
+  typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::Ptr
+  clone() const override
+  {
+    Ptr copy(new CorrespondenceEstimationNormalShooting<PointSource,
+                                                        PointTarget,
+                                                        NormalT,
+                                                        Scalar>(*this));
+    return (copy);
+  }
+
+protected:
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::corr_name_;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::tree_;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
+      tree_reciprocal_;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_;
+
+  /** \brief Internal computation initialization. */
+  bool
+  initCompute();
+
+private:
+  /** \brief The normals computed at each point in the source cloud */
+  NormalsConstPtr source_normals_;
+
+  /** \brief The normals computed at each point in the source cloud */
+  NormalsPtr source_normals_transformed_;
+
+  /** \brief The number of neighbours to be considered in the target point cloud */
+  unsigned int k_;
+};
+} // namespace registration
+} // namespace pcl
 
 #include <pcl/registration/impl/correspondence_estimation_normal_shooting.hpp>
diff --git a/registration/include/pcl/registration/correspondence_estimation_organized_projection.h b/registration/include/pcl/registration/correspondence_estimation_organized_projection.h
index d8458183..97bf14ab 100644
--- a/registration/include/pcl/registration/correspondence_estimation_organized_projection.h
+++ b/registration/include/pcl/registration/correspondence_estimation_organized_projection.h
@@ -39,170 +39,201 @@
 
 #pragma once
 
+#include <pcl/registration/correspondence_estimation.h>
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
-#include <pcl/registration/correspondence_estimation.h>
 
-namespace pcl
-{
-  namespace registration
+namespace pcl {
+namespace registration {
+/** \brief CorrespondenceEstimationOrganizedProjection computes correspondences by
+ * projecting the source point cloud onto the target point cloud using the camera
+ * intrinsic and extrinsic parameters. The correspondences can be trimmed by a depth
+ * threshold and by a distance threshold. It is not as precise as a nearest neighbor
+ * search, but it is much faster, as it avoids the usage of any additional structures
+ * (i.e., kd-trees). \note The target point cloud must be organized (no restrictions on
+ * the source) and the target point cloud must be given in the camera coordinate frame.
+ * Any other transformation is specified by the src_to_tgt_transformation_ variable.
+ * \author Alex Ichim
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename Scalar = float>
+class CorrespondenceEstimationOrganizedProjection
+: public CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> {
+public:
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
+      input_transformed_;
+  using PCLBase<PointSource>::deinitCompute;
+  using PCLBase<PointSource>::input_;
+  using PCLBase<PointSource>::indices_;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::getClassName;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
+      point_representation_;
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
+      target_cloud_updated_;
+
+  using PointCloudSource = pcl::PointCloud<PointSource>;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+
+  using PointCloudTarget = pcl::PointCloud<PointTarget>;
+  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
+  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
+
+  using Ptr = shared_ptr<
+      CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar>>;
+  using ConstPtr =
+      shared_ptr<const CorrespondenceEstimationOrganizedProjection<PointSource,
+                                                                   PointTarget,
+                                                                   Scalar>>;
+
+  /** \brief Empty constructor that sets all the intrinsic calibration to the default
+   * Kinect values. */
+  CorrespondenceEstimationOrganizedProjection()
+  : fx_(525.f)
+  , fy_(525.f)
+  , cx_(319.5f)
+  , cy_(239.5f)
+  , src_to_tgt_transformation_(Eigen::Matrix4f::Identity())
+  , depth_threshold_(std::numeric_limits<float>::max())
+  , projection_matrix_(Eigen::Matrix3f::Identity())
+  {}
+
+  /** \brief Sets the focal length parameters of the target camera.
+   * \param[in] fx the focal length in pixels along the x-axis of the image
+   * \param[in] fy the focal length in pixels along the y-axis of the image
+   */
+  inline void
+  setFocalLengths(const float fx, const float fy)
+  {
+    fx_ = fx;
+    fy_ = fy;
+  }
+
+  /** \brief Reads back the focal length parameters of the target camera.
+   * \param[out] fx the focal length in pixels along the x-axis of the image
+   * \param[out] fy the focal length in pixels along the y-axis of the image
+   */
+  inline void
+  getFocalLengths(float& fx, float& fy) const
+  {
+    fx = fx_;
+    fy = fy_;
+  }
+
+  /** \brief Sets the camera center parameters of the target camera.
+   * \param[in] cx the x-coordinate of the camera center
+   * \param[in] cy the y-coordinate of the camera center
+   */
+  inline void
+  setCameraCenters(const float cx, const float cy)
+  {
+    cx_ = cx;
+    cy_ = cy;
+  }
+
+  /** \brief Reads back the camera center parameters of the target camera.
+   * \param[out] cx the x-coordinate of the camera center
+   * \param[out] cy the y-coordinate of the camera center
+   */
+  inline void
+  getCameraCenters(float& cx, float& cy) const
+  {
+    cx = cx_;
+    cy = cy_;
+  }
+
+  /** \brief Sets the transformation from the source point cloud to the target point
+   * cloud. \note The target point cloud must be in its local camera coordinates, so use
+   * this transformation to correct for that. \param[in] src_to_tgt_transformation the
+   * transformation
+   */
+  inline void
+  setSourceTransformation(const Eigen::Matrix4f& src_to_tgt_transformation)
+  {
+    src_to_tgt_transformation_ = src_to_tgt_transformation;
+  }
+
+  /** \brief Reads back the transformation from the source point cloud to the target
+   * point cloud. \note The target point cloud must be in its local camera coordinates,
+   * so use this transformation to correct for that. \return the transformation
+   */
+  inline Eigen::Matrix4f
+  getSourceTransformation() const
+  {
+    return (src_to_tgt_transformation_);
+  }
+
+  /** \brief Sets the depth threshold; after projecting the source points in the image
+   * space of the target camera, this threshold is applied on the depths of
+   * corresponding dexels to eliminate the ones that are too far from each other.
+   * \param[in] depth_threshold the depth threshold
+   */
+  inline void
+  setDepthThreshold(const float depth_threshold)
+  {
+    depth_threshold_ = depth_threshold;
+  }
+
+  /** \brief Reads back the depth threshold; after projecting the source points in the
+   * image space of the target camera, this threshold is applied on the depths of
+   * corresponding dexels to eliminate the ones that are too far from each other.
+   * \return the depth threshold
+   */
+  inline float
+  getDepthThreshold() const
   {
-    /** \brief CorrespondenceEstimationOrganizedProjection computes correspondences by projecting the source point cloud
-      * onto the target point cloud using the camera intrinsic and extrinsic parameters. The correspondences can be trimmed
-      * by a depth threshold and by a distance threshold.
-      * It is not as precise as a nearest neighbor search, but it is much faster, as it avoids the usage of any additional
-      * structures (i.e., kd-trees).
-      * \note The target point cloud must be organized (no restrictions on the source) and the target point cloud must be
-      * given in the camera coordinate frame. Any other transformation is specified by the src_to_tgt_transformation_
-      * variable.
-      * \author Alex Ichim
-      */
-    template <typename PointSource, typename PointTarget, typename Scalar = float>
-    class CorrespondenceEstimationOrganizedProjection : public CorrespondenceEstimationBase <PointSource, PointTarget, Scalar>
-    {
-      public:
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::input_transformed_;
-        using PCLBase<PointSource>::deinitCompute;
-        using PCLBase<PointSource>::input_;
-        using PCLBase<PointSource>::indices_;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::getClassName;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::point_representation_;
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_cloud_updated_;
-
-        using PointCloudSource = pcl::PointCloud<PointSource>;
-        using PointCloudSourcePtr = typename PointCloudSource::Ptr;
-        using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
-
-        using PointCloudTarget = pcl::PointCloud<PointTarget>;
-        using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
-        using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
-
-        using Ptr = shared_ptr< CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar> >;
-        using ConstPtr = shared_ptr< const CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar> >;
-
-
-
-        /** \brief Empty constructor that sets all the intrinsic calibration to the default Kinect values. */
-        CorrespondenceEstimationOrganizedProjection ()
-          : fx_ (525.f)
-          , fy_ (525.f)
-          , cx_ (319.5f)
-          , cy_ (239.5f)
-          , src_to_tgt_transformation_ (Eigen::Matrix4f::Identity ())
-          , depth_threshold_ (std::numeric_limits<float>::max ())
-          , projection_matrix_ (Eigen::Matrix3f::Identity ())
-        { }
-
-
-        /** \brief Sets the focal length parameters of the target camera.
-          * \param[in] fx the focal length in pixels along the x-axis of the image
-          * \param[in] fy the focal length in pixels along the y-axis of the image
-          */
-        inline void
-        setFocalLengths (const float fx, const float fy)
-        { fx_ = fx; fy_ = fy; }
-
-        /** \brief Reads back the focal length parameters of the target camera.
-          * \param[out] fx the focal length in pixels along the x-axis of the image
-          * \param[out] fy the focal length in pixels along the y-axis of the image
-          */
-        inline void
-        getFocalLengths (float &fx, float &fy) const
-        { fx = fx_; fy = fy_; }
-
-
-        /** \brief Sets the camera center parameters of the target camera.
-          * \param[in] cx the x-coordinate of the camera center
-          * \param[in] cy the y-coordinate of the camera center
-          */
-        inline void
-        setCameraCenters (const float cx, const float cy)
-        { cx_ = cx; cy_ = cy; }
-
-        /** \brief Reads back the camera center parameters of the target camera.
-          * \param[out] cx the x-coordinate of the camera center
-          * \param[out] cy the y-coordinate of the camera center
-          */
-        inline void
-        getCameraCenters (float &cx, float &cy) const
-        { cx = cx_; cy = cy_; }
-
-        /** \brief Sets the transformation from the source point cloud to the target point cloud.
-          * \note The target point cloud must be in its local camera coordinates, so use this transformation to correct
-          * for that.
-          * \param[in] src_to_tgt_transformation the transformation
-          */
-        inline void
-        setSourceTransformation (const Eigen::Matrix4f &src_to_tgt_transformation)
-        { src_to_tgt_transformation_ = src_to_tgt_transformation; }
-
-        /** \brief Reads back the transformation from the source point cloud to the target point cloud.
-          * \note The target point cloud must be in its local camera coordinates, so use this transformation to correct
-          * for that.
-          * \return the transformation
-          */
-        inline Eigen::Matrix4f
-        getSourceTransformation () const
-        { return (src_to_tgt_transformation_); }
-
-        /** \brief Sets the depth threshold; after projecting the source points in the image space of the target camera,
-          * this threshold is applied on the depths of corresponding dexels to eliminate the ones that are too far from
-          * each other.
-          * \param[in] depth_threshold the depth threshold
-          */
-        inline void
-        setDepthThreshold (const float depth_threshold)
-        { depth_threshold_ = depth_threshold; }
-
-        /** \brief Reads back the depth threshold; after projecting the source points in the image space of the target
-          * camera, this threshold is applied on the depths of corresponding dexels to eliminate the ones that are too
-          * far from each other.
-          * \return the depth threshold
-          */
-        inline float
-        getDepthThreshold () const
-        { return (depth_threshold_); }
-
-        /** \brief Computes the correspondences, applying a maximum Euclidean distance threshold.
-          * \param correspondences
-          * \param[in] max_distance Euclidean distance threshold above which correspondences will be rejected
-          */
-        void
-        determineCorrespondences (Correspondences &correspondences, double max_distance);
-
-        /** \brief Computes the correspondences, applying a maximum Euclidean distance threshold.
-          * \param correspondences
-          * \param[in] max_distance Euclidean distance threshold above which correspondences will be rejected
-          */
-        void
-        determineReciprocalCorrespondences (Correspondences &correspondences, double max_distance);
-        
-        /** \brief Clone and cast to CorrespondenceEstimationBase */
-        virtual typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::Ptr
-        clone () const
-        {
-          Ptr copy (new CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar> (*this));
-          return (copy);
-        }
-
-      protected:
-        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_;
-
-        bool
-        initCompute ();
-
-        float fx_, fy_;
-        float cx_, cy_;
-        Eigen::Matrix4f src_to_tgt_transformation_;
-        float depth_threshold_;
-
-        Eigen::Matrix3f projection_matrix_;
-
-      public:
-        PCL_MAKE_ALIGNED_OPERATOR_NEW
-    };
+    return (depth_threshold_);
   }
-}
+
+  /** \brief Computes the correspondences, applying a maximum Euclidean distance
+   * threshold.
+   * \param[out] correspondences the found correspondences (index of query point, index
+   * of target point, distance)
+   * \param[in] max_distance Euclidean distance threshold above which correspondences
+   * will be rejected
+   */
+  void
+  determineCorrespondences(Correspondences& correspondences, double max_distance);
+
+  /** \brief Computes the correspondences, applying a maximum Euclidean distance
+   * threshold.
+   * \param[out] correspondences the found correspondences (index of query and target
+   * point, distance)
+   * \param[in] max_distance Euclidean distance threshold above which correspondences
+   * will be rejected
+   */
+  void
+  determineReciprocalCorrespondences(Correspondences& correspondences,
+                                     double max_distance);
+
+  /** \brief Clone and cast to CorrespondenceEstimationBase */
+  virtual typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::Ptr
+  clone() const
+  {
+    Ptr copy(new CorrespondenceEstimationOrganizedProjection<PointSource,
+                                                             PointTarget,
+                                                             Scalar>(*this));
+    return (copy);
+  }
+
+protected:
+  using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_;
+
+  bool
+  initCompute();
+
+  float fx_, fy_;
+  float cx_, cy_;
+  Eigen::Matrix4f src_to_tgt_transformation_;
+  float depth_threshold_;
+
+  Eigen::Matrix3f projection_matrix_;
+
+public:
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+};
+} // namespace registration
+} // namespace pcl
 
 #include <pcl/registration/impl/correspondence_estimation_organized_projection.hpp>
diff --git a/registration/include/pcl/registration/correspondence_rejection.h b/registration/include/pcl/registration/correspondence_rejection.h
index 580d2589..938d2844 100644
--- a/registration/include/pcl/registration/correspondence_rejection.h
+++ b/registration/include/pcl/registration/correspondence_rejection.h
@@ -40,366 +40,393 @@
 
 #pragma once
 
-#include <pcl/registration/correspondence_types.h>
-#include <pcl/registration/correspondence_sorting.h>
 #include <pcl/console/print.h>
-#include <pcl/common/transforms.h>
-#include <pcl/point_cloud.h>
+#include <pcl/registration/correspondence_sorting.h>
+#include <pcl/registration/correspondence_types.h>
 #include <pcl/search/kdtree.h>
+#include <pcl/point_cloud.h>
+
+namespace pcl {
+namespace registration {
+/** @b CorrespondenceRejector represents the base class for correspondence rejection
+ * methods \author Dirk Holz \ingroup registration
+ */
+class CorrespondenceRejector {
+public:
+  using Ptr = shared_ptr<CorrespondenceRejector>;
+  using ConstPtr = shared_ptr<const CorrespondenceRejector>;
+
+  /** \brief Empty constructor. */
+  CorrespondenceRejector() {}
+
+  /** \brief Empty destructor. */
+  virtual ~CorrespondenceRejector() {}
+
+  /** \brief Provide a pointer to the vector of the input correspondences.
+   * \param[in] correspondences the const shared pointer to a correspondence vector
+   */
+  virtual inline void
+  setInputCorrespondences(const CorrespondencesConstPtr& correspondences)
+  {
+    input_correspondences_ = correspondences;
+  };
+
+  /** \brief Get a pointer to the vector of the input correspondences.
+   * \return correspondences the const shared pointer to a correspondence vector
+   */
+  inline CorrespondencesConstPtr
+  getInputCorrespondences()
+  {
+    return input_correspondences_;
+  };
+
+  /** \brief Run correspondence rejection
+   * \param[out] correspondences Vector of correspondences that have not been rejected.
+   */
+  inline void
+  getCorrespondences(pcl::Correspondences& correspondences)
+  {
+    if (!input_correspondences_ || (input_correspondences_->empty()))
+      return;
+
+    applyRejection(correspondences);
+  }
+
+  /** \brief Get a list of valid correspondences after rejection from the original set
+   * of correspondences. Pure virtual. Compared to \a getCorrespondences this function
+   * is stateless, i.e., input correspondences do not need to be provided beforehand,
+   * but are directly provided in the function call.
+   * \param[in] original_correspondences the set of initial correspondences given
+   * \param[out] remaining_correspondences the resultant filtered set of remaining
+   * correspondences
+   */
+  virtual inline void
+  getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
+                              pcl::Correspondences& remaining_correspondences) = 0;
+
+  /** \brief Determine the indices of query points of
+   * correspondences that have been rejected, i.e., the difference
+   * between the input correspondences (set via \a setInputCorrespondences)
+   * and the given correspondence vector.
+   * \param[in] correspondences Vector of correspondences after rejection
+   * \param[out] indices Vector of query point indices of those correspondences
+   * that have been rejected.
+   */
+  inline void
+  getRejectedQueryIndices(const pcl::Correspondences& correspondences,
+                          pcl::Indices& indices)
+  {
+    if (!input_correspondences_ || input_correspondences_->empty()) {
+      PCL_WARN("[pcl::registration::%s::getRejectedQueryIndices] Input correspondences "
+               "not set (lookup of rejected correspondences _not_ possible).\n",
+               getClassName().c_str());
+      return;
+    }
+
+    pcl::getRejectedQueryIndices(*input_correspondences_, correspondences, indices);
+  }
+
+  /** \brief Get a string representation of the name of this class. */
+  inline const std::string&
+  getClassName() const
+  {
+    return (rejection_name_);
+  }
+
+  /** \brief See if this rejector requires source points */
+  virtual bool
+  requiresSourcePoints() const
+  {
+    return (false);
+  }
+
+  /** \brief Abstract method for setting the source cloud */
+  virtual void setSourcePoints(pcl::PCLPointCloud2::ConstPtr /*cloud2*/)
+  {
+    PCL_WARN("[pcl::registration::%s::setSourcePoints] This class does not require an "
+             "input source cloud\n",
+             getClassName().c_str());
+  }
+
+  /** \brief See if this rejector requires source normals */
+  virtual bool
+  requiresSourceNormals() const
+  {
+    return (false);
+  }
+
+  /** \brief Abstract method for setting the source normals */
+  virtual void setSourceNormals(pcl::PCLPointCloud2::ConstPtr /*cloud2*/)
+  {
+    PCL_WARN("[pcl::registration::%s::setSourceNormals] This class does not require "
+             "input source normals\n",
+             getClassName().c_str());
+  }
+  /** \brief See if this rejector requires a target cloud */
+  virtual bool
+  requiresTargetPoints() const
+  {
+    return (false);
+  }
+
+  /** \brief Abstract method for setting the target cloud */
+  virtual void setTargetPoints(pcl::PCLPointCloud2::ConstPtr /*cloud2*/)
+  {
+    PCL_WARN("[pcl::registration::%s::setTargetPoints] This class does not require an "
+             "input target cloud\n",
+             getClassName().c_str());
+  }
+
+  /** \brief See if this rejector requires target normals */
+  virtual bool
+  requiresTargetNormals() const
+  {
+    return (false);
+  }
+
+  /** \brief Abstract method for setting the target normals */
+  virtual void setTargetNormals(pcl::PCLPointCloud2::ConstPtr /*cloud2*/)
+  {
+    PCL_WARN("[pcl::registration::%s::setTargetNormals] This class does not require "
+             "input target normals\n",
+             getClassName().c_str());
+  }
+
+protected:
+  /** \brief The name of the rejection method. */
+  std::string rejection_name_;
+
+  /** \brief The input correspondences. */
+  CorrespondencesConstPtr input_correspondences_;
+
+  /** \brief Abstract rejection method. */
+  virtual void
+  applyRejection(Correspondences& correspondences) = 0;
+};
+
+/** @b DataContainerInterface provides a generic interface for computing correspondence
+ * scores between correspondent points in the input and target clouds
+ * \ingroup registration
+ */
+class DataContainerInterface {
+public:
+  using Ptr = shared_ptr<DataContainerInterface>;
+  using ConstPtr = shared_ptr<const DataContainerInterface>;
+
+  virtual ~DataContainerInterface() = default;
+  virtual double
+  getCorrespondenceScore(int index) = 0;
+  virtual double
+  getCorrespondenceScore(const pcl::Correspondence&) = 0;
+  virtual double
+  getCorrespondenceScoreFromNormals(const pcl::Correspondence&) = 0;
+};
+
+/** @b DataContainer is a container for the input and target point clouds and implements
+ * the interface to compute correspondence scores between correspondent points in the
+ * input and target clouds \ingroup registration
+ */
+template <typename PointT, typename NormalT = pcl::PointNormal>
+class DataContainer : public DataContainerInterface {
+  using PointCloud = pcl::PointCloud<PointT>;
+  using PointCloudPtr = typename PointCloud::Ptr;
+  using PointCloudConstPtr = typename PointCloud::ConstPtr;
+
+  using KdTreePtr = typename pcl::search::KdTree<PointT>::Ptr;
+
+  using Normals = pcl::PointCloud<NormalT>;
+  using NormalsPtr = typename Normals::Ptr;
+  using NormalsConstPtr = typename Normals::ConstPtr;
+
+public:
+  /** \brief Empty constructor. */
+  DataContainer(bool needs_normals = false)
+  : input_()
+  , input_transformed_()
+  , target_()
+  , input_normals_()
+  , input_normals_transformed_()
+  , target_normals_()
+  , tree_(new pcl::search::KdTree<PointT>)
+  , class_name_("DataContainer")
+  , needs_normals_(needs_normals)
+  , target_cloud_updated_(true)
+  , force_no_recompute_(false)
+  {}
+
+  /** \brief Empty destructor */
+  ~DataContainer() {}
+
+  /** \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
+   */
+  inline void
+  setInputSource(const PointCloudConstPtr& cloud)
+  {
+    input_ = cloud;
+  }
+
+  /** \brief Get a pointer to the input point cloud dataset target. */
+  inline PointCloudConstPtr const
+  getInputSource()
+  {
+    return (input_);
+  }
+
+  /** \brief Provide a target point cloud dataset (must contain XYZ
+   * data!), used to compute the correspondence distance.
+   * \param[in] target a cloud containing XYZ data
+   */
+  inline void
+  setInputTarget(const PointCloudConstPtr& target)
+  {
+    target_ = target;
+    target_cloud_updated_ = true;
+  }
+
+  /** \brief Get a pointer to the input point cloud dataset target. */
+  inline PointCloudConstPtr const
+  getInputTarget()
+  {
+    return (target_);
+  }
+
+  /** \brief Provide a pointer to the search object used to find correspondences in
+   * the target cloud.
+   * \param[in] tree a pointer to the spatial search object.
+   * \param[in] force_no_recompute If set to true, this tree will NEVER be
+   * recomputed, regardless of calls to setInputTarget. Only use if you are
+   * confident that the tree will be set correctly.
+   */
+  inline void
+  setSearchMethodTarget(const KdTreePtr& tree, bool force_no_recompute = false)
+  {
+    tree_ = tree;
+    force_no_recompute_ = force_no_recompute;
+    target_cloud_updated_ = true;
+  }
+
+  /** \brief Set the normals computed on the input point cloud
+   * \param[in] normals the normals computed for the input cloud
+   */
+  inline void
+  setInputNormals(const NormalsConstPtr& normals)
+  {
+    input_normals_ = normals;
+  }
+
+  /** \brief Get the normals computed on the input point cloud */
+  inline NormalsConstPtr
+  getInputNormals()
+  {
+    return (input_normals_);
+  }
+
+  /** \brief Set the normals computed on the target point cloud
+   * \param[in] normals the normals computed for the input cloud
+   */
+  inline void
+  setTargetNormals(const NormalsConstPtr& normals)
+  {
+    target_normals_ = normals;
+  }
+
+  /** \brief Get the normals computed on the target point cloud */
+  inline NormalsConstPtr
+  getTargetNormals()
+  {
+    return (target_normals_);
+  }
+
+  /** \brief Get the correspondence score for a point in the input cloud
+   * \param[in] index index of the point in the input cloud
+   */
+  inline double
+  getCorrespondenceScore(int index) override
+  {
+    if (target_cloud_updated_ && !force_no_recompute_) {
+      tree_->setInputCloud(target_);
+    }
+    pcl::Indices indices(1);
+    std::vector<float> distances(1);
+    if (tree_->nearestKSearch((*input_)[index], 1, indices, distances))
+      return (distances[0]);
+    return (std::numeric_limits<double>::max());
+  }
+
+  /** \brief Get the correspondence score for a given pair of correspondent points
+   * \param[in] corr Correspondent points
+   */
+  inline double
+  getCorrespondenceScore(const pcl::Correspondence& corr) override
+  {
+    // Get the source and the target feature from the list
+    const PointT& src = (*input_)[corr.index_query];
+    const PointT& tgt = (*target_)[corr.index_match];
+
+    return ((src.getVector4fMap() - tgt.getVector4fMap()).squaredNorm());
+  }
+
+  /** \brief Get the correspondence score for a given pair of correspondent points based
+   * on the angle between the normals. The normmals for the in put and target clouds
+   * must be set before using this function \param[in] corr Correspondent points
+   */
+  inline double
+  getCorrespondenceScoreFromNormals(const pcl::Correspondence& corr) override
+  {
+    // assert ( (input_normals_->size () != 0) && (target_normals_->size () != 0) &&
+    // "Normals are not set for the input and target point clouds");
+    assert(input_normals_ && target_normals_ &&
+           "Normals are not set for the input and target point clouds");
+    const NormalT& src = (*input_normals_)[corr.index_query];
+    const NormalT& tgt = (*target_normals_)[corr.index_match];
+    return (double((src.normal[0] * tgt.normal[0]) + (src.normal[1] * tgt.normal[1]) +
+                   (src.normal[2] * tgt.normal[2])));
+  }
+
+private:
+  /** \brief The input point cloud dataset */
+  PointCloudConstPtr input_;
+
+  /** \brief The input transformed point cloud dataset */
+  PointCloudPtr input_transformed_;
+
+  /** \brief The target point cloud datase. */
+  PointCloudConstPtr target_;
+
+  /** \brief Normals to the input point cloud */
+  NormalsConstPtr input_normals_;
+
+  /** \brief Normals to the input point cloud */
+  NormalsPtr input_normals_transformed_;
+
+  /** \brief Normals to the target point cloud */
+  NormalsConstPtr target_normals_;
+
+  /** \brief A pointer to the spatial search object. */
+  KdTreePtr tree_;
+
+  /** \brief The name of the rejection method. */
+  std::string class_name_;
+
+  /** \brief Should the current data container use normals? */
+  bool needs_normals_;
+
+  /** \brief Variable that stores whether we have a new target cloud, meaning we need to
+   * pre-process it again. This way, we avoid rebuilding the kd-tree */
+  bool target_cloud_updated_;
+
+  /** \brief A flag which, if set, means the tree operating on the target cloud
+   * will never be recomputed*/
+  bool force_no_recompute_;
 
-namespace pcl
-{
-  namespace registration
+  /** \brief Get a string representation of the name of this class. */
+  inline const std::string&
+  getClassName() const
   {
-    /** @b CorrespondenceRejector represents the base class for correspondence rejection methods
-      * \author Dirk Holz
-      * \ingroup registration
-      */
-    class CorrespondenceRejector
-    {
-      public:
-        using Ptr = shared_ptr<CorrespondenceRejector>;
-        using ConstPtr = shared_ptr<const CorrespondenceRejector>;
-
-        /** \brief Empty constructor. */
-        CorrespondenceRejector () 
-        {}
-
-        /** \brief Empty destructor. */
-        virtual ~CorrespondenceRejector () {}
-
-        /** \brief Provide a pointer to the vector of the input correspondences.
-          * \param[in] correspondences the const shared pointer to a correspondence vector
-          */
-        virtual inline void 
-        setInputCorrespondences (const CorrespondencesConstPtr &correspondences) 
-        { 
-          input_correspondences_ = correspondences; 
-        };
-
-        /** \brief Get a pointer to the vector of the input correspondences.
-          * \return correspondences the const shared pointer to a correspondence vector
-          */
-        inline CorrespondencesConstPtr 
-        getInputCorrespondences () { return input_correspondences_; };
-
-        /** \brief Run correspondence rejection
-          * \param[out] correspondences Vector of correspondences that have not been rejected.
-          */
-        inline void 
-        getCorrespondences (pcl::Correspondences &correspondences)
-        {
-          if (!input_correspondences_ || (input_correspondences_->empty ()))
-            return;
-
-          applyRejection (correspondences);
-        }
-
-        /** \brief Get a list of valid correspondences after rejection from the original set of correspondences.
-          * Pure virtual. Compared to \a getCorrespondences this function is
-          * stateless, i.e., input correspondences do not need to be provided beforehand,
-          * but are directly provided in the function call.
-          * \param[in] original_correspondences the set of initial correspondences given
-          * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences
-          */
-        virtual inline void 
-        getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, 
-                                     pcl::Correspondences& remaining_correspondences) = 0;
-
-        /** \brief Determine the indices of query points of
-          * correspondences that have been rejected, i.e., the difference
-          * between the input correspondences (set via \a setInputCorrespondences)
-          * and the given correspondence vector.
-          * \param[in] correspondences Vector of correspondences after rejection
-          * \param[out] indices Vector of query point indices of those correspondences
-          * that have been rejected.
-          */
-        inline void 
-        getRejectedQueryIndices (const pcl::Correspondences &correspondences, 
-                                 std::vector<int>& indices)
-        {
-          if (!input_correspondences_ || input_correspondences_->empty ())
-          {
-            PCL_WARN ("[pcl::registration::%s::getRejectedQueryIndices] Input correspondences not set (lookup of rejected correspondences _not_ possible).\n", getClassName ().c_str ());
-            return;
-          }
-
-          pcl::getRejectedQueryIndices(*input_correspondences_, correspondences, indices);
-        }
-
-        /** \brief Get a string representation of the name of this class. */
-        inline const std::string& 
-        getClassName () const { return (rejection_name_); }
-
-
-        /** \brief See if this rejector requires source points */
-        virtual bool
-        requiresSourcePoints () const
-        { return (false); }
-
-        /** \brief Abstract method for setting the source cloud */
-        virtual void
-        setSourcePoints (pcl::PCLPointCloud2::ConstPtr /*cloud2*/)
-        {
-          PCL_WARN ("[pcl::registration::%s::setSourcePoints] This class does not require an input source cloud", getClassName ().c_str ());
-        }
-        
-        /** \brief See if this rejector requires source normals */
-        virtual bool
-        requiresSourceNormals () const
-        { return (false); }
-
-        /** \brief Abstract method for setting the source normals */
-        virtual void
-        setSourceNormals (pcl::PCLPointCloud2::ConstPtr /*cloud2*/)
-        { 
-          PCL_WARN ("[pcl::registration::%s::setSourceNormals] This class does not require input source normals", getClassName ().c_str ());
-        }
-        /** \brief See if this rejector requires a target cloud */
-        virtual bool
-        requiresTargetPoints () const
-        { return (false); }
-
-        /** \brief Abstract method for setting the target cloud */
-        virtual void
-        setTargetPoints (pcl::PCLPointCloud2::ConstPtr /*cloud2*/)
-        {
-          PCL_WARN ("[pcl::registration::%s::setTargetPoints] This class does not require an input target cloud", getClassName ().c_str ());
-        }
-        
-        /** \brief See if this rejector requires target normals */
-        virtual bool
-        requiresTargetNormals () const
-        { return (false); }
-
-        /** \brief Abstract method for setting the target normals */
-        virtual void
-        setTargetNormals (pcl::PCLPointCloud2::ConstPtr /*cloud2*/)
-        {
-          PCL_WARN ("[pcl::registration::%s::setTargetNormals] This class does not require input target normals", getClassName ().c_str ());
-        }
-
-      protected:
-
-        /** \brief The name of the rejection method. */
-        std::string rejection_name_;
-
-        /** \brief The input correspondences. */
-        CorrespondencesConstPtr input_correspondences_;
-
-        /** \brief Abstract rejection method. */
-        virtual void 
-        applyRejection (Correspondences &correspondences) = 0;
-    };
-
-    /** @b DataContainerInterface provides a generic interface for computing correspondence scores between correspondent
-      * points in the input and target clouds
-      * \ingroup registration
-      */
-    class DataContainerInterface
-    {
-      public:
-        using Ptr = shared_ptr<DataContainerInterface>;
-        using ConstPtr = shared_ptr<const DataContainerInterface>;
-
-        virtual ~DataContainerInterface () = default;
-        virtual double getCorrespondenceScore (int index) = 0;
-        virtual double getCorrespondenceScore (const pcl::Correspondence &) = 0;
-        virtual double getCorrespondenceScoreFromNormals (const pcl::Correspondence &) = 0;
-     };
-
-    /** @b DataContainer is a container for the input and target point clouds and implements the interface 
-      * to compute correspondence scores between correspondent points in the input and target clouds
-      * \ingroup registration
-      */
-    template <typename PointT, typename NormalT = pcl::PointNormal>
-    class DataContainer : public DataContainerInterface
-    {
-      using PointCloud = pcl::PointCloud<PointT>;
-      using PointCloudPtr = typename PointCloud::Ptr;
-      using PointCloudConstPtr = typename PointCloud::ConstPtr;
-
-      using KdTreePtr = typename pcl::search::KdTree<PointT>::Ptr;
-      
-      using Normals = pcl::PointCloud<NormalT>;
-      using NormalsPtr = typename Normals::Ptr;
-      using NormalsConstPtr = typename Normals::ConstPtr;
-
-      public:
-
-        /** \brief Empty constructor. */
-        DataContainer (bool needs_normals = false) 
-          : input_ ()
-          , input_transformed_ ()
-          , target_ ()
-          , input_normals_ ()
-          , input_normals_transformed_ ()
-          , target_normals_ ()
-          , tree_ (new pcl::search::KdTree<PointT>)
-          , class_name_ ("DataContainer")
-          , needs_normals_ (needs_normals)
-          , target_cloud_updated_ (true)
-          , force_no_recompute_ (false)
-        {
-        }
-      
-        /** \brief Empty destructor */
-        ~DataContainer () {}
-
-        /** \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
-          */
-        inline void 
-        setInputSource (const PointCloudConstPtr &cloud)
-        {
-          input_ = cloud;
-        }
-
-        /** \brief Get a pointer to the input point cloud dataset target. */
-        inline PointCloudConstPtr const 
-        getInputSource () { return (input_); }
-
-        /** \brief Provide a target point cloud dataset (must contain XYZ
-          * data!), used to compute the correspondence distance.  
-          * \param[in] target a cloud containing XYZ data
-          */
-        inline void 
-        setInputTarget (const PointCloudConstPtr &target)
-        {
-          target_ = target;
-          target_cloud_updated_ = true;
-        }
-
-        /** \brief Get a pointer to the input point cloud dataset target. */
-        inline PointCloudConstPtr const 
-        getInputTarget () { return (target_); }
-        
-        /** \brief Provide a pointer to the search object used to find correspondences in
-          * the target cloud.
-          * \param[in] tree a pointer to the spatial search object.
-          * \param[in] force_no_recompute If set to true, this tree will NEVER be 
-          * recomputed, regardless of calls to setInputTarget. Only use if you are 
-          * confident that the tree will be set correctly.
-          */
-        inline void
-        setSearchMethodTarget (const KdTreePtr &tree, 
-                               bool force_no_recompute = false) 
-        { 
-          tree_ = tree; 
-          if (force_no_recompute)
-          {
-            force_no_recompute_ = true;
-          }
-          target_cloud_updated_ = true;
-        }
-
-        /** \brief Set the normals computed on the input point cloud
-          * \param[in] normals the normals computed for the input cloud
-          */
-        inline void
-        setInputNormals (const NormalsConstPtr &normals) { input_normals_ = normals; }
-
-        /** \brief Get the normals computed on the input point cloud */
-        inline NormalsConstPtr
-        getInputNormals () { return (input_normals_); }
-
-        /** \brief Set the normals computed on the target point cloud
-          * \param[in] normals the normals computed for the input cloud
-          */
-        inline void
-        setTargetNormals (const NormalsConstPtr &normals) { target_normals_ = normals; }
-        
-        /** \brief Get the normals computed on the target point cloud */
-        inline NormalsConstPtr
-        getTargetNormals () { return (target_normals_); }
-
-        /** \brief Get the correspondence score for a point in the input cloud
-          * \param[in] index index of the point in the input cloud
-          */
-        inline double 
-        getCorrespondenceScore (int index) override
-        {
-          if ( target_cloud_updated_ && !force_no_recompute_ )
-          {
-            tree_->setInputCloud (target_);
-          }
-          std::vector<int> indices (1);
-          std::vector<float> distances (1);
-          if (tree_->nearestKSearch ((*input_)[index], 1, indices, distances))
-            return (distances[0]);
-          return (std::numeric_limits<double>::max ());
-        }
-
-        /** \brief Get the correspondence score for a given pair of correspondent points
-          * \param[in] corr Correspondent points
-          */
-        inline double 
-        getCorrespondenceScore (const pcl::Correspondence &corr) override
-        {
-          // Get the source and the target feature from the list
-          const PointT &src = (*input_)[corr.index_query];
-          const PointT &tgt = (*target_)[corr.index_match];
-
-          return ((src.getVector4fMap () - tgt.getVector4fMap ()).squaredNorm ());
-        }
-        
-        /** \brief Get the correspondence score for a given pair of correspondent points based on 
-          * the angle between the normals. The normmals for the in put and target clouds must be 
-          * set before using this function
-          * \param[in] corr Correspondent points
-          */
-        inline double
-        getCorrespondenceScoreFromNormals (const pcl::Correspondence &corr) override
-        {
-          //assert ( (input_normals_->size () != 0) && (target_normals_->size () != 0) && "Normals are not set for the input and target point clouds");
-          assert (input_normals_ && target_normals_ && "Normals are not set for the input and target point clouds");
-          const NormalT &src = (*input_normals_)[corr.index_query];
-          const NormalT &tgt = (*target_normals_)[corr.index_match];
-          return (double ((src.normal[0] * tgt.normal[0]) + (src.normal[1] * tgt.normal[1]) + (src.normal[2] * tgt.normal[2])));
-        }
-
-     private:
-        /** \brief The input point cloud dataset */
-        PointCloudConstPtr input_;
-
-        /** \brief The input transformed point cloud dataset */
-        PointCloudPtr input_transformed_;
-
-        /** \brief The target point cloud datase. */
-        PointCloudConstPtr target_;
-
-        /** \brief Normals to the input point cloud */
-        NormalsConstPtr input_normals_;
-
-        /** \brief Normals to the input point cloud */
-        NormalsPtr input_normals_transformed_;
-
-        /** \brief Normals to the target point cloud */
-        NormalsConstPtr target_normals_;
-
-        /** \brief A pointer to the spatial search object. */
-        KdTreePtr tree_;
-
-        /** \brief The name of the rejection method. */
-        std::string class_name_;
-
-        /** \brief Should the current data container use normals? */
-        bool needs_normals_;
-
-        /** \brief Variable that stores whether we have a new target cloud, meaning we need to pre-process it again.
-         * This way, we avoid rebuilding the kd-tree */
-        bool target_cloud_updated_;
-
-        /** \brief A flag which, if set, means the tree operating on the target cloud 
-         * will never be recomputed*/
-        bool force_no_recompute_;
-
-
-
-        /** \brief Get a string representation of the name of this class. */
-        inline const std::string& 
-        getClassName () const { return (class_name_); }
-    };
+    return (class_name_);
   }
-}
+};
+} // namespace registration
+} // namespace pcl
diff --git a/registration/include/pcl/registration/correspondence_rejection_distance.h b/registration/include/pcl/registration/correspondence_rejection_distance.h
index f02721a7..05357964 100644
--- a/registration/include/pcl/registration/correspondence_rejection_distance.h
+++ b/registration/include/pcl/registration/correspondence_rejection_distance.h
@@ -41,170 +41,165 @@
 #pragma once
 
 #include <pcl/registration/correspondence_rejection.h>
-#include <pcl/memory.h>  // for static_pointer_cast
+#include <pcl/conversions.h> // for fromPCLPointCloud2
+#include <pcl/memory.h>      // for static_pointer_cast
+
+namespace pcl {
+namespace registration {
+/**
+ * @b CorrespondenceRejectorDistance implements a simple correspondence
+ * rejection method based on thresholding the distances between the
+ * correspondences.
+ *
+ * \note If \ref setInputCloud and \ref setInputTarget are given, then the
+ * distances between correspondences will be estimated using the given XYZ
+ * data, and not read from the set of input correspondences.
+ *
+ * \author Dirk Holz, Radu B. Rusu
+ * \ingroup registration
+ */
+class PCL_EXPORTS CorrespondenceRejectorDistance : public CorrespondenceRejector {
+  using CorrespondenceRejector::getClassName;
+  using CorrespondenceRejector::input_correspondences_;
+  using CorrespondenceRejector::rejection_name_;
+
+public:
+  using Ptr = shared_ptr<CorrespondenceRejectorDistance>;
+  using ConstPtr = shared_ptr<const CorrespondenceRejectorDistance>;
+
+  /** \brief Empty constructor. */
+  CorrespondenceRejectorDistance() : max_distance_(std::numeric_limits<float>::max())
+  {
+    rejection_name_ = "CorrespondenceRejectorDistance";
+  }
+
+  /** \brief Empty destructor */
+  ~CorrespondenceRejectorDistance() {}
+
+  /** \brief Get a list of valid correspondences after rejection from the original set
+   * of correspondences. \param[in] original_correspondences the set of initial
+   * correspondences given \param[out] remaining_correspondences the resultant filtered
+   * set of remaining correspondences
+   */
+  void
+  getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
+                              pcl::Correspondences& remaining_correspondences) override;
+
+  /** \brief Set the maximum distance used for thresholding in correspondence rejection.
+   * \param[in] distance Distance to be used as maximum distance between
+   * correspondences. Correspondences with larger distances are rejected. \note
+   * Internally, the distance will be stored squared.
+   */
+  virtual inline void
+  setMaximumDistance(float distance)
+  {
+    max_distance_ = distance * distance;
+  };
+
+  /** \brief Get the maximum distance used for thresholding in correspondence rejection.
+   */
+  inline float
+  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 <typename PointT>
+  inline void
+  setInputSource(const typename pcl::PointCloud<PointT>::ConstPtr& cloud)
+  {
+    if (!data_container_)
+      data_container_.reset(new DataContainer<PointT>);
+    static_pointer_cast<DataContainer<PointT>>(data_container_)->setInputSource(cloud);
+  }
+
+  /** \brief Provide a target point cloud dataset (must contain XYZ
+   * data!), used to compute the correspondence distance.
+   * \param[in] target a cloud containing XYZ data
+   */
+  template <typename PointT>
+  inline void
+  setInputTarget(const typename pcl::PointCloud<PointT>::ConstPtr& target)
+  {
+    if (!data_container_)
+      data_container_.reset(new DataContainer<PointT>);
+    static_pointer_cast<DataContainer<PointT>>(data_container_)->setInputTarget(target);
+  }
 
+  /** \brief See if this rejector requires source points */
+  bool
+  requiresSourcePoints() const override
+  {
+    return (true);
+  }
+
+  /** \brief Blob method for setting the source cloud */
+  void
+  setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
+  {
+    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
+    fromPCLPointCloud2(*cloud2, *cloud);
+    setInputSource<PointXYZ>(cloud);
+  }
+
+  /** \brief See if this rejector requires a target cloud */
+  bool
+  requiresTargetPoints() const override
+  {
+    return (true);
+  }
 
-namespace pcl
-{
-  namespace registration
+  /** \brief Method for setting the target cloud */
+  void
+  setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
   {
-    /**
-      * @b CorrespondenceRejectorDistance implements a simple correspondence
-      * rejection method based on thresholding the distances between the
-      * correspondences.
-      *
-      * \note If \ref setInputCloud and \ref setInputTarget are given, then the
-      * distances between correspondences will be estimated using the given XYZ
-      * data, and not read from the set of input correspondences.
-      *
-      * \author Dirk Holz, Radu B. Rusu
-      * \ingroup registration
-      */
-    class PCL_EXPORTS CorrespondenceRejectorDistance: public CorrespondenceRejector
-    {
-      using CorrespondenceRejector::input_correspondences_;
-      using CorrespondenceRejector::rejection_name_;
-      using CorrespondenceRejector::getClassName;
-
-      public:
-        using Ptr = shared_ptr<CorrespondenceRejectorDistance>;
-        using ConstPtr = shared_ptr<const CorrespondenceRejectorDistance>;
-
-        /** \brief Empty constructor. */
-        CorrespondenceRejectorDistance () : max_distance_(std::numeric_limits<float>::max ())
-        {
-          rejection_name_ = "CorrespondenceRejectorDistance";
-        }
-      
-        /** \brief Empty destructor */
-        ~CorrespondenceRejectorDistance () {}
-
-        /** \brief Get a list of valid correspondences after rejection from the original set of correspondences.
-          * \param[in] original_correspondences the set of initial correspondences given
-          * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences
-          */
-        void
-        getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, 
-                                     pcl::Correspondences& remaining_correspondences) override;
-
-        /** \brief Set the maximum distance used for thresholding in correspondence rejection.
-          * \param[in] distance Distance to be used as maximum distance between correspondences. 
-          * Correspondences with larger distances are rejected.
-          * \note Internally, the distance will be stored squared.
-          */
-        virtual inline void 
-        setMaximumDistance (float distance) { max_distance_ = distance * distance; };
-
-        /** \brief Get the maximum distance used for thresholding in correspondence rejection. */
-        inline float 
-        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 <typename PointT>
-        PCL_DEPRECATED(1, 12, "pcl::registration::CorrespondenceRejectorDistance::setInputCloud is deprecated. Please use setInputSource instead")
-        inline void 
-        setInputCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud)
-        {
-          if (!data_container_)
-            data_container_.reset (new DataContainer<PointT>);
-          static_pointer_cast<DataContainer<PointT> > (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 <typename PointT> inline void 
-        setInputSource (const typename pcl::PointCloud<PointT>::ConstPtr &cloud)
-        {
-          if (!data_container_)
-            data_container_.reset (new DataContainer<PointT>);
-          static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (cloud);
-        }
-
-        /** \brief Provide a target point cloud dataset (must contain XYZ
-          * data!), used to compute the correspondence distance.  
-          * \param[in] target a cloud containing XYZ data
-          */
-        template <typename PointT> inline void 
-        setInputTarget (const typename pcl::PointCloud<PointT>::ConstPtr &target)
-        {
-          if (!data_container_)
-            data_container_.reset (new DataContainer<PointT>);
-          static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputTarget (target);
-        }
-
-
-        /** \brief See if this rejector requires source points */
-        bool
-        requiresSourcePoints () const override
-        { return (true); }
-
-        /** \brief Blob method for setting the source cloud */
-        void
-        setSourcePoints (pcl::PCLPointCloud2::ConstPtr cloud2) override
-        { 
-          PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
-          fromPCLPointCloud2 (*cloud2, *cloud);
-          setInputSource<PointXYZ> (cloud);
-        }
-        
-        /** \brief See if this rejector requires a target cloud */
-        bool
-        requiresTargetPoints () const override
-        { return (true); }
-
-        /** \brief Method for setting the target cloud */
-        void
-        setTargetPoints (pcl::PCLPointCloud2::ConstPtr cloud2) override
-        { 
-          PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
-          fromPCLPointCloud2 (*cloud2, *cloud);
-          setInputTarget<PointXYZ> (cloud);
-        }
-
-        /** \brief Provide a pointer to the search object used to find correspondences in
-          * the target cloud.
-          * \param[in] tree a pointer to the spatial search object.
-          * \param[in] force_no_recompute If set to true, this tree will NEVER be 
-          * recomputed, regardless of calls to setInputTarget. Only use if you are 
-          * confident that the tree will be set correctly.
-          */
-        template <typename PointT> inline void
-        setSearchMethodTarget (const typename pcl::search::KdTree<PointT>::Ptr &tree,
-                               bool force_no_recompute = false)
-        { 
-          static_pointer_cast< DataContainer<PointT> > 
-            (data_container_)->setSearchMethodTarget (tree, force_no_recompute );
-        }
-
-
-      protected:
-
-        /** \brief Apply the rejection algorithm.
-          * \param[out] correspondences the set of resultant correspondences.
-          */
-        inline void 
-        applyRejection (pcl::Correspondences &correspondences) override
-        {
-          getRemainingCorrespondences (*input_correspondences_, correspondences);
-        }
-
-        /** \brief The maximum distance threshold between two correspondent points in source <-> target. If the
-          * distance is larger than this threshold, the points will not be ignored in the alignment process.
-          */
-        float max_distance_;
-
-        using DataContainerPtr = DataContainerInterface::Ptr;
-
-        /** \brief A pointer to the DataContainer object containing the input and target point clouds */
-        DataContainerPtr data_container_;
-    };
+    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
+    fromPCLPointCloud2(*cloud2, *cloud);
+    setInputTarget<PointXYZ>(cloud);
+  }
+
+  /** \brief Provide a pointer to the search object used to find correspondences in
+   * the target cloud.
+   * \param[in] tree a pointer to the spatial search object.
+   * \param[in] force_no_recompute If set to true, this tree will NEVER be
+   * recomputed, regardless of calls to setInputTarget. Only use if you are
+   * confident that the tree will be set correctly.
+   */
+  template <typename PointT>
+  inline void
+  setSearchMethodTarget(const typename pcl::search::KdTree<PointT>::Ptr& tree,
+                        bool force_no_recompute = false)
+  {
+    static_pointer_cast<DataContainer<PointT>>(data_container_)
+        ->setSearchMethodTarget(tree, force_no_recompute);
+  }
 
+protected:
+  /** \brief Apply the rejection algorithm.
+   * \param[out] correspondences the set of resultant correspondences.
+   */
+  inline void
+  applyRejection(pcl::Correspondences& correspondences) override
+  {
+    getRemainingCorrespondences(*input_correspondences_, correspondences);
   }
-}
 
-#include <pcl/registration/impl/correspondence_rejection_distance.hpp>
+  /** \brief The maximum distance threshold between two correspondent points in source
+   * <-> target. If the distance is larger than this threshold, the points will not be
+   * ignored in the alignment process.
+   */
+  float max_distance_;
+
+  using DataContainerPtr = DataContainerInterface::Ptr;
+
+  /** \brief A pointer to the DataContainer object containing the input and target point
+   * clouds */
+  DataContainerPtr data_container_;
+};
+
+} // namespace registration
+} // namespace pcl
diff --git a/registration/include/pcl/registration/correspondence_rejection_features.h b/registration/include/pcl/registration/correspondence_rejection_features.h
index f7342a02..cf09fbd3 100644
--- a/registration/include/pcl/registration/correspondence_rejection_features.h
+++ b/registration/include/pcl/registration/correspondence_rejection_features.h
@@ -46,254 +46,270 @@
 
 #include <unordered_map>
 
-namespace pcl
-{
-  namespace registration
+namespace pcl {
+namespace registration {
+/** \brief CorrespondenceRejectorFeatures implements a correspondence rejection method
+ * based on a set of feature descriptors. Given an input feature space, the method
+ * checks if each feature in the source cloud has a correspondence in the target cloud,
+ * either by checking the first K (given) point correspondences, or by defining a
+ * tolerance threshold via a radius in feature space. \todo explain this better. \author
+ * Radu B. Rusu \ingroup registration
+ */
+class PCL_EXPORTS CorrespondenceRejectorFeatures : public CorrespondenceRejector {
+  using CorrespondenceRejector::getClassName;
+  using CorrespondenceRejector::input_correspondences_;
+  using CorrespondenceRejector::rejection_name_;
+
+public:
+  using Ptr = shared_ptr<CorrespondenceRejectorFeatures>;
+  using ConstPtr = shared_ptr<const CorrespondenceRejectorFeatures>;
+
+  /** \brief Empty constructor. */
+  CorrespondenceRejectorFeatures() : max_distance_(std::numeric_limits<float>::max())
   {
-    /** \brief CorrespondenceRejectorFeatures implements a correspondence rejection method based on a set of feature
-      * descriptors. Given an input feature space, the method checks if each feature in the source cloud has a
-      * correspondence in the target cloud, either by checking the first K (given) point correspondences, or 
-      * by defining a tolerance threshold via a radius in feature space.
-      * \todo explain this better.
-      * \author Radu B. Rusu
-      * \ingroup registration
-      */
-    class PCL_EXPORTS CorrespondenceRejectorFeatures: public CorrespondenceRejector
-    {
-      using CorrespondenceRejector::input_correspondences_;
-      using CorrespondenceRejector::rejection_name_;
-      using CorrespondenceRejector::getClassName;
-
-      public:
-        using Ptr = shared_ptr<CorrespondenceRejectorFeatures>;
-        using ConstPtr = shared_ptr<const CorrespondenceRejectorFeatures>;
-
-        /** \brief Empty constructor. */
-        CorrespondenceRejectorFeatures () : max_distance_ (std::numeric_limits<float>::max ())
-        {
-          rejection_name_ = "CorrespondenceRejectorFeatures";
-        }
-
-        /** \brief Empty destructor. */
-        ~CorrespondenceRejectorFeatures () {}
-
-        /** \brief Get a list of valid correspondences after rejection from the original set of correspondences
-          * \param[in] original_correspondences the set of initial correspondences given
-          * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences
-          */
-        void 
-        getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, 
-                                     pcl::Correspondences& remaining_correspondences) override;
-
-        /** \brief Provide a pointer to a cloud of feature descriptors associated with the source point cloud
-          * \param[in] source_feature a cloud of feature descriptors associated with the source point cloud
-          * \param[in] key a string that uniquely identifies the feature
-          */
-        template <typename FeatureT> inline void 
-        setSourceFeature (const typename pcl::PointCloud<FeatureT>::ConstPtr &source_feature, 
-                          const std::string &key);
-
-        /** \brief Get a pointer to the source cloud's feature descriptors, specified by the given \a key
-          * \param[in] key a string that uniquely identifies the feature (must match the key provided by setSourceFeature)
-          */
-        template <typename FeatureT> inline typename pcl::PointCloud<FeatureT>::ConstPtr 
-        getSourceFeature (const std::string &key);
-
-        /** \brief Provide a pointer to a cloud of feature descriptors associated with the target point cloud
-          * \param[in] target_feature a cloud of feature descriptors associated with the target point cloud
-          * \param[in] key a string that uniquely identifies the feature
-          */
-        template <typename FeatureT> inline void 
-        setTargetFeature (const typename pcl::PointCloud<FeatureT>::ConstPtr &target_feature, 
-                          const std::string &key);
-
-        /** \brief Get a pointer to the source cloud's feature descriptors, specified by the given \a key
-          * \param[in] key a string that uniquely identifies the feature (must match the key provided by setTargetFeature)
-          */
-        template <typename FeatureT> inline typename pcl::PointCloud<FeatureT>::ConstPtr 
-        getTargetFeature (const std::string &key);
-
-        /** \brief Set a hard distance threshold in the feature \a FeatureT space, between source and target
-          * features. Any feature correspondence that is above this threshold will be considered bad and will be
-          * filtered out.
-          * \param[in] thresh the distance threshold
-          * \param[in] key a string that uniquely identifies the feature
-          */
-        template <typename FeatureT> inline void 
-        setDistanceThreshold (double thresh, const std::string &key);
-
-        /** \brief Test that all features are valid (i.e., does each key have a valid source cloud, target cloud, 
-          * and search method)
-          */
-        inline bool 
-        hasValidFeatures ();
-
-        /** \brief Provide a boost shared pointer to a PointRepresentation to be used when comparing features
-          * \param[in] key a string that uniquely identifies the feature
-          * \param[in] fr the point feature representation to be used 
-          */
-        template <typename FeatureT> inline void
-        setFeatureRepresentation (const typename pcl::PointRepresentation<FeatureT>::ConstPtr &fr,
-                                  const std::string &key);
-
-      protected:
-
-        /** \brief Apply the rejection algorithm.
-          * \param[out] correspondences the set of resultant correspondences.
-          */
-        inline void 
-        applyRejection (pcl::Correspondences &correspondences) override
-        {
-          getRemainingCorrespondences (*input_correspondences_, correspondences);
-        }
-
-        /** \brief The maximum distance threshold between two correspondent points in source <-> target. If the
-          * distance is larger than this threshold, the points will not be ignored in the alignment process.
-          */
-        float max_distance_;
-
-        class FeatureContainerInterface
-        {
-          public:
-            /** \brief Empty destructor */
-            virtual ~FeatureContainerInterface () = default;
-            virtual bool isValid () = 0;
-            virtual double getCorrespondenceScore (int index) = 0;
-            virtual bool isCorrespondenceValid (int index) = 0;
-
-            using Ptr = shared_ptr<FeatureContainerInterface>;
-        };
-
-        using FeaturesMap = std::unordered_map<std::string, FeatureContainerInterface::Ptr>;
-
-        /** \brief An STL map containing features to use when performing the correspondence search.*/
-        FeaturesMap features_map_;
-
-        /** \brief An inner class containing pointers to the source and target feature clouds 
-          * and the parameters needed to perform the correspondence search.  This class extends 
-          * FeatureContainerInterface, which contains abstract methods for any methods that do not depend on the 
-          * FeatureT --- these methods can thus be called from a pointer to FeatureContainerInterface without 
-          * casting to the derived class.
-          */
-        template <typename FeatureT>
-        class FeatureContainer : public pcl::registration::CorrespondenceRejectorFeatures::FeatureContainerInterface
-        {
-          public:
-            using FeatureCloudConstPtr = typename pcl::PointCloud<FeatureT>::ConstPtr;
-            using SearchMethod = std::function<int (const pcl::PointCloud<FeatureT> &, int, std::vector<int> &,  std::vector<float> &)>;
-            
-            using PointRepresentationConstPtr = typename pcl::PointRepresentation<FeatureT>::ConstPtr;
-
-            FeatureContainer () : thresh_(std::numeric_limits<double>::max ()), feature_representation_()
-            {
-            }
-      
-            /** \brief Empty destructor */
-            ~FeatureContainer () {}
-
-            inline void 
-            setSourceFeature (const FeatureCloudConstPtr &source_features)
-            {
-              source_features_ = source_features;
-            }
-            
-            inline FeatureCloudConstPtr 
-            getSourceFeature ()
-            {
-              return (source_features_);
-            }
-            
-            inline void 
-            setTargetFeature (const FeatureCloudConstPtr &target_features)
-            {
-              target_features_ = target_features;
-            }
-            
-            inline FeatureCloudConstPtr 
-            getTargetFeature ()
-            {
-              return (target_features_);
-            }
-            
-            inline void 
-            setDistanceThreshold (double thresh)
-            {
-              thresh_ = thresh;
-            }
-
-            inline bool 
-            isValid () override
-            {
-              if (!source_features_ || !target_features_)
-                return (false);
-              return (source_features_->size () > 0 && 
-                      target_features_->size () > 0);
-            }
-
-            /** \brief Provide a boost shared pointer to a PointRepresentation to be used when comparing features
-              * \param[in] fr the point feature representation to be used
-              */
-            inline void
-            setFeatureRepresentation (const PointRepresentationConstPtr &fr)
-            {
-              feature_representation_ = fr;
-            }
-
-            /** \brief Obtain a score between a pair of correspondences.
-              * \param[in] index the index to check in the list of correspondences
-              * \return score the resultant computed score
-              */
-            inline double
-            getCorrespondenceScore (int index) override
-            {
-              // If no feature representation was given, reset to the default implementation for FeatureT
-              if (!feature_representation_)
-                feature_representation_.reset (new DefaultFeatureRepresentation<FeatureT>);
-
-              // Get the source and the target feature from the list
-              const FeatureT &feat_src = (*source_features_)[index];
-              const FeatureT &feat_tgt = (*target_features_)[index];
-
-              // Check if the representations are valid
-              if (!feature_representation_->isValid (feat_src) || !feature_representation_->isValid (feat_tgt))
-              {
-                PCL_ERROR ("[pcl::registration::%s::getCorrespondenceScore] Invalid feature representation given!\n", this->getClassName ().c_str ());
-                return (std::numeric_limits<double>::max ());
-              }
-
-              // Set the internal feature point representation of choice
-              Eigen::VectorXf feat_src_ptr = Eigen::VectorXf::Zero (feature_representation_->getNumberOfDimensions ());
-              feature_representation_->vectorize (FeatureT (feat_src), feat_src_ptr);
-              Eigen::VectorXf feat_tgt_ptr = Eigen::VectorXf::Zero (feature_representation_->getNumberOfDimensions ());
-              feature_representation_->vectorize (FeatureT (feat_tgt), feat_tgt_ptr);
-
-              // Compute the L2 norm
-              return ((feat_src_ptr - feat_tgt_ptr).squaredNorm ());
-            }
-
-            /** \brief Check whether the correspondence pair at the given index is valid
-              * by computing the score and testing it against the user given threshold 
-              * \param[in] index the index to check in the list of correspondences
-              * \return true if the correspondence is good, false otherwise
-              */
-            inline bool
-            isCorrespondenceValid (int index) override
-            {
-              return (getCorrespondenceScore (index) < thresh_ * thresh_);
-            }
-             
-          private:
-            FeatureCloudConstPtr source_features_, target_features_;
-            SearchMethod search_method_;
-
-            /** \brief The L2 squared Euclidean threshold. */
-            double thresh_;
-
-            /** \brief The internal point feature representation used. */
-            PointRepresentationConstPtr feature_representation_;
-        };
-    };
+    rejection_name_ = "CorrespondenceRejectorFeatures";
   }
-}
+
+  /** \brief Empty destructor. */
+  ~CorrespondenceRejectorFeatures() {}
+
+  /** \brief Get a list of valid correspondences after rejection from the original set
+   * of correspondences \param[in] original_correspondences the set of initial
+   * correspondences given \param[out] remaining_correspondences the resultant filtered
+   * set of remaining correspondences
+   */
+  void
+  getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
+                              pcl::Correspondences& remaining_correspondences) override;
+
+  /** \brief Provide a pointer to a cloud of feature descriptors associated with the
+   * source point cloud \param[in] source_feature a cloud of feature descriptors
+   * associated with the source point cloud \param[in] key a string that uniquely
+   * identifies the feature
+   */
+  template <typename FeatureT>
+  inline void
+  setSourceFeature(const typename pcl::PointCloud<FeatureT>::ConstPtr& source_feature,
+                   const std::string& key);
+
+  /** \brief Get a pointer to the source cloud's feature descriptors, specified by the
+   * given \a key \param[in] key a string that uniquely identifies the feature (must
+   * match the key provided by setSourceFeature)
+   */
+  template <typename FeatureT>
+  inline typename pcl::PointCloud<FeatureT>::ConstPtr
+  getSourceFeature(const std::string& key);
+
+  /** \brief Provide a pointer to a cloud of feature descriptors associated with the
+   * target point cloud \param[in] target_feature a cloud of feature descriptors
+   * associated with the target point cloud \param[in] key a string that uniquely
+   * identifies the feature
+   */
+  template <typename FeatureT>
+  inline void
+  setTargetFeature(const typename pcl::PointCloud<FeatureT>::ConstPtr& target_feature,
+                   const std::string& key);
+
+  /** \brief Get a pointer to the source cloud's feature descriptors, specified by the
+   * given \a key \param[in] key a string that uniquely identifies the feature (must
+   * match the key provided by setTargetFeature)
+   */
+  template <typename FeatureT>
+  inline typename pcl::PointCloud<FeatureT>::ConstPtr
+  getTargetFeature(const std::string& key);
+
+  /** \brief Set a hard distance threshold in the feature \a FeatureT space, between
+   * source and target features. Any feature correspondence that is above this threshold
+   * will be considered bad and will be filtered out. \param[in] thresh the distance
+   * threshold \param[in] key a string that uniquely identifies the feature
+   */
+  template <typename FeatureT>
+  inline void
+  setDistanceThreshold(double thresh, const std::string& key);
+
+  /** \brief Test that all features are valid (i.e., does each key have a valid source
+   * cloud, target cloud, and search method)
+   */
+  inline bool
+  hasValidFeatures();
+
+  /** \brief Provide a boost shared pointer to a PointRepresentation to be used when
+   * comparing features \param[in] key a string that uniquely identifies the feature
+   * \param[in] fr the point feature representation to be used
+   */
+  template <typename FeatureT>
+  inline void
+  setFeatureRepresentation(
+      const typename pcl::PointRepresentation<FeatureT>::ConstPtr& fr,
+      const std::string& key);
+
+protected:
+  /** \brief Apply the rejection algorithm.
+   * \param[out] correspondences the set of resultant correspondences.
+   */
+  inline void
+  applyRejection(pcl::Correspondences& correspondences) override
+  {
+    getRemainingCorrespondences(*input_correspondences_, correspondences);
+  }
+
+  /** \brief The maximum distance threshold between two correspondent points in source
+   * <-> target. If the distance is larger than this threshold, the points will not be
+   * ignored in the alignment process.
+   */
+  float max_distance_;
+
+  class FeatureContainerInterface {
+  public:
+    /** \brief Empty destructor */
+    virtual ~FeatureContainerInterface() = default;
+    virtual bool
+    isValid() = 0;
+    virtual double
+    getCorrespondenceScore(int index) = 0;
+    virtual bool
+    isCorrespondenceValid(int index) = 0;
+
+    using Ptr = shared_ptr<FeatureContainerInterface>;
+  };
+
+  using FeaturesMap = std::unordered_map<std::string, FeatureContainerInterface::Ptr>;
+
+  /** \brief An STL map containing features to use when performing the correspondence
+   * search.*/
+  FeaturesMap features_map_;
+
+  /** \brief An inner class containing pointers to the source and target feature clouds
+   * and the parameters needed to perform the correspondence search.  This class extends
+   * FeatureContainerInterface, which contains abstract methods for any methods that do
+   * not depend on the FeatureT --- these methods can thus be called from a pointer to
+   * FeatureContainerInterface without casting to the derived class.
+   */
+  template <typename FeatureT>
+  class FeatureContainer : public pcl::registration::CorrespondenceRejectorFeatures::
+                               FeatureContainerInterface {
+  public:
+    using FeatureCloudConstPtr = typename pcl::PointCloud<FeatureT>::ConstPtr;
+    using SearchMethod = std::function<int(
+        const pcl::PointCloud<FeatureT>&, int, pcl::Indices&, std::vector<float>&)>;
+
+    using PointRepresentationConstPtr =
+        typename pcl::PointRepresentation<FeatureT>::ConstPtr;
+
+    FeatureContainer()
+    : thresh_(std::numeric_limits<double>::max()), feature_representation_()
+    {}
+
+    /** \brief Empty destructor */
+    ~FeatureContainer() {}
+
+    inline void
+    setSourceFeature(const FeatureCloudConstPtr& source_features)
+    {
+      source_features_ = source_features;
+    }
+
+    inline FeatureCloudConstPtr
+    getSourceFeature()
+    {
+      return (source_features_);
+    }
+
+    inline void
+    setTargetFeature(const FeatureCloudConstPtr& target_features)
+    {
+      target_features_ = target_features;
+    }
+
+    inline FeatureCloudConstPtr
+    getTargetFeature()
+    {
+      return (target_features_);
+    }
+
+    inline void
+    setDistanceThreshold(double thresh)
+    {
+      thresh_ = thresh;
+    }
+
+    inline bool
+    isValid() override
+    {
+      if (!source_features_ || !target_features_)
+        return (false);
+      return (source_features_->size() > 0 && target_features_->size() > 0);
+    }
+
+    /** \brief Provide a boost shared pointer to a PointRepresentation to be used when
+     * comparing features \param[in] fr the point feature representation to be used
+     */
+    inline void
+    setFeatureRepresentation(const PointRepresentationConstPtr& fr)
+    {
+      feature_representation_ = fr;
+    }
+
+    /** \brief Obtain a score between a pair of correspondences.
+     * \param[in] index the index to check in the list of correspondences
+     * \return score the resultant computed score
+     */
+    inline double
+    getCorrespondenceScore(int index) override
+    {
+      // If no feature representation was given, reset to the default implementation for
+      // FeatureT
+      if (!feature_representation_)
+        feature_representation_.reset(new DefaultFeatureRepresentation<FeatureT>);
+
+      // Get the source and the target feature from the list
+      const FeatureT& feat_src = (*source_features_)[index];
+      const FeatureT& feat_tgt = (*target_features_)[index];
+
+      // Check if the representations are valid
+      if (!feature_representation_->isValid(feat_src) ||
+          !feature_representation_->isValid(feat_tgt)) {
+        PCL_ERROR("[pcl::registration::%s::getCorrespondenceScore] Invalid feature "
+                  "representation given!\n",
+                  this->getClassName().c_str());
+        return (std::numeric_limits<double>::max());
+      }
+
+      // Set the internal feature point representation of choice
+      Eigen::VectorXf feat_src_ptr =
+          Eigen::VectorXf::Zero(feature_representation_->getNumberOfDimensions());
+      feature_representation_->vectorize(FeatureT(feat_src), feat_src_ptr);
+      Eigen::VectorXf feat_tgt_ptr =
+          Eigen::VectorXf::Zero(feature_representation_->getNumberOfDimensions());
+      feature_representation_->vectorize(FeatureT(feat_tgt), feat_tgt_ptr);
+
+      // Compute the L2 norm
+      return ((feat_src_ptr - feat_tgt_ptr).squaredNorm());
+    }
+
+    /** \brief Check whether the correspondence pair at the given index is valid
+     * by computing the score and testing it against the user given threshold
+     * \param[in] index the index to check in the list of correspondences
+     * \return true if the correspondence is good, false otherwise
+     */
+    inline bool
+    isCorrespondenceValid(int index) override
+    {
+      return (getCorrespondenceScore(index) < thresh_ * thresh_);
+    }
+
+  private:
+    FeatureCloudConstPtr source_features_, target_features_;
+    SearchMethod search_method_;
+
+    /** \brief The L2 squared Euclidean threshold. */
+    double thresh_;
+
+    /** \brief The internal point feature representation used. */
+    PointRepresentationConstPtr feature_representation_;
+  };
+};
+} // namespace registration
+} // namespace pcl
 
 #include <pcl/registration/impl/correspondence_rejection_features.hpp>
diff --git a/registration/include/pcl/registration/correspondence_rejection_median_distance.h b/registration/include/pcl/registration/correspondence_rejection_median_distance.h
index 8cece9d2..0d5f00c6 100644
--- a/registration/include/pcl/registration/correspondence_rejection_median_distance.h
+++ b/registration/include/pcl/registration/correspondence_rejection_median_distance.h
@@ -41,173 +41,170 @@
 #pragma once
 
 #include <pcl/registration/correspondence_rejection.h>
-#include <pcl/memory.h>  // for static_pointer_cast
+#include <pcl/conversions.h> // for fromPCLPointCloud2
+#include <pcl/memory.h>      // for static_pointer_cast
 #include <pcl/point_cloud.h>
 
+namespace pcl {
+namespace registration {
+/** \brief CorrespondenceRejectorMedianDistance implements a simple correspondence
+ * rejection method based on thresholding based on the median distance between the
+ * correspondences.
+ *
+ * \note If \ref setInputCloud and \ref setInputTarget are given, then the
+ * distances between correspondences will be estimated using the given XYZ
+ * data, and not read from the set of input correspondences.
+ *
+ * \author Aravindhan K Krishnan. This code is ported from libpointmatcher
+ * (https://github.com/ethz-asl/libpointmatcher) \ingroup registration
+ */
+class PCL_EXPORTS CorrespondenceRejectorMedianDistance : public CorrespondenceRejector {
+  using CorrespondenceRejector::getClassName;
+  using CorrespondenceRejector::input_correspondences_;
+  using CorrespondenceRejector::rejection_name_;
+
+public:
+  using Ptr = shared_ptr<CorrespondenceRejectorMedianDistance>;
+  using ConstPtr = shared_ptr<const CorrespondenceRejectorMedianDistance>;
+
+  /** \brief Empty constructor. */
+  CorrespondenceRejectorMedianDistance() : median_distance_(0), factor_(1.0)
+  {
+    rejection_name_ = "CorrespondenceRejectorMedianDistance";
+  }
+
+  /** \brief Get a list of valid correspondences after rejection from the original set
+   * of correspondences. \param[in] original_correspondences the set of initial
+   * correspondences given \param[out] remaining_correspondences the resultant filtered
+   * set of remaining correspondences
+   */
+  void
+  getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
+                              pcl::Correspondences& remaining_correspondences) override;
+
+  /** \brief Get the median distance used for thresholding in correspondence rejection.
+   */
+  inline double
+  getMedianDistance() const
+  {
+    return (median_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 <typename PointT>
+  inline void
+  setInputSource(const typename pcl::PointCloud<PointT>::ConstPtr& cloud)
+  {
+    if (!data_container_)
+      data_container_.reset(new DataContainer<PointT>);
+    static_pointer_cast<DataContainer<PointT>>(data_container_)->setInputSource(cloud);
+  }
+
+  /** \brief Provide a target point cloud dataset (must contain XYZ
+   * data!), used to compute the correspondence distance.
+   * \param[in] target a cloud containing XYZ data
+   */
+  template <typename PointT>
+  inline void
+  setInputTarget(const typename pcl::PointCloud<PointT>::ConstPtr& target)
+  {
+    if (!data_container_)
+      data_container_.reset(new DataContainer<PointT>);
+    static_pointer_cast<DataContainer<PointT>>(data_container_)->setInputTarget(target);
+  }
 
-namespace pcl
-{
-  namespace registration
+  /** \brief See if this rejector requires source points */
+  bool
+  requiresSourcePoints() const override
   {
-    /** \brief CorrespondenceRejectorMedianDistance implements a simple correspondence
-      * rejection method based on thresholding based on the median distance between the
-      * correspondences.
-      *
-      * \note If \ref setInputCloud and \ref setInputTarget are given, then the
-      * distances between correspondences will be estimated using the given XYZ
-      * data, and not read from the set of input correspondences.
-      *
-      * \author Aravindhan K Krishnan. This code is ported from libpointmatcher (https://github.com/ethz-asl/libpointmatcher)
-      * \ingroup registration
-      */
-    class PCL_EXPORTS CorrespondenceRejectorMedianDistance: public CorrespondenceRejector
-    {
-      using CorrespondenceRejector::input_correspondences_;
-      using CorrespondenceRejector::rejection_name_;
-      using CorrespondenceRejector::getClassName;
-
-      public:
-        using Ptr = shared_ptr<CorrespondenceRejectorMedianDistance>;
-        using ConstPtr = shared_ptr<const CorrespondenceRejectorMedianDistance>;
-
-        /** \brief Empty constructor. */
-        CorrespondenceRejectorMedianDistance () 
-          : median_distance_ (0)
-          , factor_ (1.0)
-        {
-          rejection_name_ = "CorrespondenceRejectorMedianDistance";
-        }
-
-        /** \brief Get a list of valid correspondences after rejection from the original set of correspondences.
-          * \param[in] original_correspondences the set of initial correspondences given
-          * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences
-          */
-        void 
-        getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, 
-                                     pcl::Correspondences& remaining_correspondences) override;
-
-        /** \brief Get the median distance used for thresholding in correspondence rejection. */
-        inline double
-        getMedianDistance () const { return (median_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 <typename PointT> inline void 
-        setInputSource (const typename pcl::PointCloud<PointT>::ConstPtr &cloud)
-        {
-          if (!data_container_)
-            data_container_.reset (new DataContainer<PointT>);
-          static_pointer_cast<DataContainer<PointT> > (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 <typename PointT>
-        PCL_DEPRECATED(1, 12, "pcl::registration::CorrespondenceRejectorMedianDistance::setInputCloud is deprecated. Please use setInputSource instead")
-        inline void 
-        setInputCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud)
-        {
-          if (!data_container_)
-            data_container_.reset (new DataContainer<PointT>);
-          static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (cloud);
-        }
-
-        /** \brief Provide a target point cloud dataset (must contain XYZ
-          * data!), used to compute the correspondence distance.  
-          * \param[in] target a cloud containing XYZ data
-          */
-        template <typename PointT> inline void 
-        setInputTarget (const typename pcl::PointCloud<PointT>::ConstPtr &target)
-        {
-          if (!data_container_)
-            data_container_.reset (new DataContainer<PointT>);
-          static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputTarget (target);
-        }
-        
-        /** \brief See if this rejector requires source points */
-        bool
-        requiresSourcePoints () const override
-        { return (true); }
-
-        /** \brief Blob method for setting the source cloud */
-        void
-        setSourcePoints (pcl::PCLPointCloud2::ConstPtr cloud2) override
-        { 
-          PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
-          fromPCLPointCloud2 (*cloud2, *cloud);
-          setInputSource<PointXYZ> (cloud);
-        }
-        
-        /** \brief See if this rejector requires a target cloud */
-        bool
-        requiresTargetPoints () const override
-        { return (true); }
-
-        /** \brief Method for setting the target cloud */
-        void
-        setTargetPoints (pcl::PCLPointCloud2::ConstPtr cloud2) override
-        { 
-          PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
-          fromPCLPointCloud2 (*cloud2, *cloud);
-          setInputTarget<PointXYZ> (cloud);
-        }
-
-        /** \brief Provide a pointer to the search object used to find correspondences in
-          * the target cloud.
-          * \param[in] tree a pointer to the spatial search object.
-          * \param[in] force_no_recompute If set to true, this tree will NEVER be 
-          * recomputed, regardless of calls to setInputTarget. Only use if you are 
-          * confident that the tree will be set correctly.
-          */
-        template <typename PointT> inline void
-        setSearchMethodTarget (const typename pcl::search::KdTree<PointT>::Ptr &tree,
-                               bool force_no_recompute = false)
-        { 
-          static_pointer_cast< DataContainer<PointT> > 
-            (data_container_)->setSearchMethodTarget (tree, force_no_recompute );
-        }
-
-        /** \brief Set the factor for correspondence rejection. Points with distance greater than median times factor
-         *  will be rejected
-         *  \param[in] factor value
-         */
-        inline void
-        setMedianFactor (double factor) { factor_ = factor; };
-
-        /** \brief Get the factor used for thresholding in correspondence rejection. */
-        inline double
-        getMedianFactor () const { return factor_; };
-
-      protected:
-
-        /** \brief Apply the rejection algorithm.
-          * \param[out] correspondences the set of resultant correspondences.
-          */
-        inline void 
-        applyRejection (pcl::Correspondences &correspondences) override
-        {
-          getRemainingCorrespondences (*input_correspondences_, correspondences);
-        }
-
-        /** \brief The median distance threshold between two correspondent points in source <-> target.
-          */
-        double median_distance_;
-
-        /** \brief The factor for correspondence rejection. Points with distance greater than median times factor
-         *  will be rejected
-         */
-        double factor_;
-
-        using DataContainerPtr = DataContainerInterface::Ptr;
-
-        /** \brief A pointer to the DataContainer object containing the input and target point clouds */
-        DataContainerPtr data_container_;
-    };
+    return (true);
   }
-}
 
-#include <pcl/registration/impl/correspondence_rejection_median_distance.hpp>
+  /** \brief Blob method for setting the source cloud */
+  void
+  setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
+  {
+    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
+    fromPCLPointCloud2(*cloud2, *cloud);
+    setInputSource<PointXYZ>(cloud);
+  }
+
+  /** \brief See if this rejector requires a target cloud */
+  bool
+  requiresTargetPoints() const override
+  {
+    return (true);
+  }
+
+  /** \brief Method for setting the target cloud */
+  void
+  setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
+  {
+    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
+    fromPCLPointCloud2(*cloud2, *cloud);
+    setInputTarget<PointXYZ>(cloud);
+  }
+
+  /** \brief Provide a pointer to the search object used to find correspondences in
+   * the target cloud.
+   * \param[in] tree a pointer to the spatial search object.
+   * \param[in] force_no_recompute If set to true, this tree will NEVER be
+   * recomputed, regardless of calls to setInputTarget. Only use if you are
+   * confident that the tree will be set correctly.
+   */
+  template <typename PointT>
+  inline void
+  setSearchMethodTarget(const typename pcl::search::KdTree<PointT>::Ptr& tree,
+                        bool force_no_recompute = false)
+  {
+    static_pointer_cast<DataContainer<PointT>>(data_container_)
+        ->setSearchMethodTarget(tree, force_no_recompute);
+  }
+
+  /** \brief Set the factor for correspondence rejection. Points with distance greater
+   * than median times factor will be rejected \param[in] factor value
+   */
+  inline void
+  setMedianFactor(double factor)
+  {
+    factor_ = factor;
+  };
+
+  /** \brief Get the factor used for thresholding in correspondence rejection. */
+  inline double
+  getMedianFactor() const
+  {
+    return factor_;
+  };
+
+protected:
+  /** \brief Apply the rejection algorithm.
+   * \param[out] correspondences the set of resultant correspondences.
+   */
+  inline void
+  applyRejection(pcl::Correspondences& correspondences) override
+  {
+    getRemainingCorrespondences(*input_correspondences_, correspondences);
+  }
+
+  /** \brief The median distance threshold between two correspondent points in source
+   * <-> target.
+   */
+  double median_distance_;
+
+  /** \brief The factor for correspondence rejection. Points with distance greater than
+   * median times factor will be rejected
+   */
+  double factor_;
+
+  using DataContainerPtr = DataContainerInterface::Ptr;
+
+  /** \brief A pointer to the DataContainer object containing the input and target point
+   * clouds */
+  DataContainerPtr data_container_;
+};
+} // namespace registration
+} // namespace pcl
diff --git a/registration/include/pcl/registration/correspondence_rejection_one_to_one.h b/registration/include/pcl/registration/correspondence_rejection_one_to_one.h
index ea5e3822..d094c70e 100644
--- a/registration/include/pcl/registration/correspondence_rejection_one_to_one.h
+++ b/registration/include/pcl/registration/correspondence_rejection_one_to_one.h
@@ -42,55 +42,51 @@
 
 #include <pcl/registration/correspondence_rejection.h>
 
-namespace pcl
-{
-  namespace registration
-  {
-    /** \brief CorrespondenceRejectorOneToOne implements a correspondence
-      * rejection method based on eliminating duplicate match indices in
-      * the correspondences. Correspondences with the same match index are
-      * removed and only the one with smallest distance between query and
-      * match are kept. That is, considering match->query 1-m correspondences
-      * are removed leaving only 1-1 correspondences.
-      * \author Dirk Holz
-      * \ingroup registration
-      */
-    class PCL_EXPORTS CorrespondenceRejectorOneToOne: public CorrespondenceRejector
-    {
-      using CorrespondenceRejector::input_correspondences_;
-      using CorrespondenceRejector::rejection_name_;
-      using CorrespondenceRejector::getClassName;
-
-      public:
-        using Ptr = shared_ptr<CorrespondenceRejectorOneToOne>;
-        using ConstPtr = shared_ptr<const CorrespondenceRejectorOneToOne>;
+namespace pcl {
+namespace registration {
+/** \brief CorrespondenceRejectorOneToOne implements a correspondence
+ * rejection method based on eliminating duplicate match indices in
+ * the correspondences. Correspondences with the same match index are
+ * removed and only the one with smallest distance between query and
+ * match are kept. That is, considering match->query 1-m correspondences
+ * are removed leaving only 1-1 correspondences.
+ * \author Dirk Holz
+ * \ingroup registration
+ */
+class PCL_EXPORTS CorrespondenceRejectorOneToOne : public CorrespondenceRejector {
+  using CorrespondenceRejector::getClassName;
+  using CorrespondenceRejector::input_correspondences_;
+  using CorrespondenceRejector::rejection_name_;
 
-        /** \brief Empty constructor. */
-        CorrespondenceRejectorOneToOne ()
-        {
-          rejection_name_ = "CorrespondenceRejectorOneToOne";
-        }
+public:
+  using Ptr = shared_ptr<CorrespondenceRejectorOneToOne>;
+  using ConstPtr = shared_ptr<const CorrespondenceRejectorOneToOne>;
 
-        /** \brief Get a list of valid correspondences after rejection from the original set of correspondences.
-          * \param[in] original_correspondences the set of initial correspondences given
-          * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences
-          */
-        void 
-        getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, 
-                                     pcl::Correspondences& remaining_correspondences) override;
+  /** \brief Empty constructor. */
+  CorrespondenceRejectorOneToOne()
+  {
+    rejection_name_ = "CorrespondenceRejectorOneToOne";
+  }
 
-      protected:
-        /** \brief Apply the rejection algorithm.
-          * \param[out] correspondences the set of resultant correspondences.
-          */
-        inline void 
-        applyRejection (pcl::Correspondences &correspondences) override
-        {
-          getRemainingCorrespondences (*input_correspondences_, correspondences);
-        }
-    };
+  /** \brief Get a list of valid correspondences after rejection from the original set
+   * of correspondences. \param[in] original_correspondences the set of initial
+   * correspondences given \param[out] remaining_correspondences the resultant filtered
+   * set of remaining correspondences
+   */
+  void
+  getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
+                              pcl::Correspondences& remaining_correspondences) override;
 
+protected:
+  /** \brief Apply the rejection algorithm.
+   * \param[out] correspondences the set of resultant correspondences.
+   */
+  inline void
+  applyRejection(pcl::Correspondences& correspondences) override
+  {
+    getRemainingCorrespondences(*input_correspondences_, correspondences);
   }
-}
+};
 
-#include <pcl/registration/impl/correspondence_rejection_one_to_one.hpp>
+} // namespace registration
+} // namespace pcl
diff --git a/registration/include/pcl/registration/correspondence_rejection_organized_boundary.h b/registration/include/pcl/registration/correspondence_rejection_organized_boundary.h
index 49e6d263..25ec0d95 100644
--- a/registration/include/pcl/registration/correspondence_rejection_organized_boundary.h
+++ b/registration/include/pcl/registration/correspondence_rejection_organized_boundary.h
@@ -39,106 +39,114 @@
 #pragma once
 
 #include <pcl/registration/correspondence_rejection.h>
-#include <pcl/memory.h>  // for static_pointer_cast
-
-namespace pcl
-{
-  namespace registration
+#include <pcl/conversions.h> // for fromPCLPointCloud2
+#include <pcl/memory.h>      // for static_pointer_cast
+
+namespace pcl {
+namespace registration {
+/**
+ * @brief The CorrespondenceRejectionOrganizedBoundary class implements a simple
+ * correspondence rejection measure. For each pair of points in correspondence, it
+ * checks whether they are on the boundary of a silhouette. This is done by counting the
+ * number of NaN dexels in a window around the points (the threshold and window size can
+ * be set by the user). \note Both the source and the target clouds need to be
+ * organized, otherwise all the correspondences will be rejected.
+ *
+ * \author Alexandru E. Ichim
+ * \ingroup registration
+ */
+class PCL_EXPORTS CorrespondenceRejectionOrganizedBoundary
+: public CorrespondenceRejector {
+public:
+  /** @brief Empty constructor. */
+  CorrespondenceRejectionOrganizedBoundary()
+  : boundary_nans_threshold_(8), window_size_(5), depth_step_threshold_(0.025f)
+  {}
+
+  void
+  getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
+                              pcl::Correspondences& remaining_correspondences) override;
+
+  inline void
+  setNumberOfBoundaryNaNs(int val)
   {
-    /**
-      * @brief The CorrespondenceRejectionOrganizedBoundary class implements a simple correspondence rejection measure.
-      * For each pair of points in correspondence, it checks whether they are on the boundary of a silhouette. This is
-      * done by counting the number of NaN dexels in a window around the points (the threshold and window size can be set
-      * by the user).
-      * \note Both the source and the target clouds need to be organized, otherwise all the correspondences will be rejected.
-      *
-      * \author Alexandru E. Ichim
-      * \ingroup registration
-      */
-    class PCL_EXPORTS CorrespondenceRejectionOrganizedBoundary : public CorrespondenceRejector
-    {
-    public:
-      /** @brief Empty constructor. */
-      CorrespondenceRejectionOrganizedBoundary ()
-        : boundary_nans_threshold_ (8)
-        , window_size_ (5)
-        , depth_step_threshold_ (0.025f)
-      { }
-
-      void
-      getRemainingCorrespondences (const pcl::Correspondences& original_correspondences,
-                                   pcl::Correspondences& remaining_correspondences) override;
-
-      inline void
-      setNumberOfBoundaryNaNs (int val)
-      { boundary_nans_threshold_ = val; }
-
-
-      template <typename PointT> inline void
-      setInputSource (const typename pcl::PointCloud<PointT>::ConstPtr &cloud)
-      {
-        if (!data_container_)
-          data_container_.reset (new pcl::registration::DataContainer<PointT>);
-        static_pointer_cast<pcl::registration::DataContainer<PointT> > (data_container_)->setInputSource (cloud);
-      }
-
-      template <typename PointT> inline void
-      setInputTarget (const typename pcl::PointCloud<PointT>::ConstPtr &cloud)
-      {
-        if (!data_container_)
-          data_container_.reset (new pcl::registration::DataContainer<PointT>);
-        static_pointer_cast<pcl::registration::DataContainer<PointT> > (data_container_)->setInputTarget (cloud);
-      }
+    boundary_nans_threshold_ = val;
+  }
 
-      /** \brief See if this rejector requires source points */
-      bool
-      requiresSourcePoints () const override
-      { return (true); }
+  template <typename PointT>
+  inline void
+  setInputSource(const typename pcl::PointCloud<PointT>::ConstPtr& cloud)
+  {
+    if (!data_container_)
+      data_container_.reset(new pcl::registration::DataContainer<PointT>);
+    static_pointer_cast<pcl::registration::DataContainer<PointT>>(data_container_)
+        ->setInputSource(cloud);
+  }
 
-      /** \brief Blob method for setting the source cloud */
-      void
-      setSourcePoints (pcl::PCLPointCloud2::ConstPtr cloud2) override
-      { 
-        PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
-        fromPCLPointCloud2 (*cloud2, *cloud);
-        setInputSource<PointXYZ> (cloud);
-      }
-      
-      /** \brief See if this rejector requires a target cloud */
-      bool
-      requiresTargetPoints () const override
-      { return (true); }
+  template <typename PointT>
+  inline void
+  setInputTarget(const typename pcl::PointCloud<PointT>::ConstPtr& cloud)
+  {
+    if (!data_container_)
+      data_container_.reset(new pcl::registration::DataContainer<PointT>);
+    static_pointer_cast<pcl::registration::DataContainer<PointT>>(data_container_)
+        ->setInputTarget(cloud);
+  }
 
-      /** \brief Method for setting the target cloud */
-      void
-      setTargetPoints (pcl::PCLPointCloud2::ConstPtr cloud2) override
-      { 
-        PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
-        fromPCLPointCloud2 (*cloud2, *cloud);
-        setInputTarget<PointXYZ> (cloud);
-      }
+  /** \brief See if this rejector requires source points */
+  bool
+  requiresSourcePoints() const override
+  {
+    return (true);
+  }
 
-      virtual bool
-      updateSource (const Eigen::Matrix4d &)
-      { return (true); }
+  /** \brief Blob method for setting the source cloud */
+  void
+  setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
+  {
+    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
+    fromPCLPointCloud2(*cloud2, *cloud);
+    setInputSource<PointXYZ>(cloud);
+  }
 
-    protected:
+  /** \brief See if this rejector requires a target cloud */
+  bool
+  requiresTargetPoints() const override
+  {
+    return (true);
+  }
 
-      /** \brief Apply the rejection algorithm.
-        * \param[out] correspondences the set of resultant correspondences.
-        */
-      inline void
-      applyRejection (pcl::Correspondences &correspondences) override
-      { getRemainingCorrespondences (*input_correspondences_, correspondences); }
+  /** \brief Method for setting the target cloud */
+  void
+  setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
+  {
+    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
+    fromPCLPointCloud2(*cloud2, *cloud);
+    setInputTarget<PointXYZ>(cloud);
+  }
 
-      int boundary_nans_threshold_;
-      int window_size_;
-      float depth_step_threshold_;
+  virtual bool
+  updateSource(const Eigen::Matrix4d&)
+  {
+    return (true);
+  }
 
-      using DataContainerPtr = DataContainerInterface::Ptr;
-      DataContainerPtr data_container_;
-    };
+protected:
+  /** \brief Apply the rejection algorithm.
+   * \param[out] correspondences the set of resultant correspondences.
+   */
+  inline void
+  applyRejection(pcl::Correspondences& correspondences) override
+  {
+    getRemainingCorrespondences(*input_correspondences_, correspondences);
   }
-}
 
-#include <pcl/registration/impl/correspondence_rejection_organized_boundary.hpp>
+  int boundary_nans_threshold_;
+  int window_size_;
+  float depth_step_threshold_;
+
+  using DataContainerPtr = DataContainerInterface::Ptr;
+  DataContainerPtr data_container_;
+};
+} // namespace registration
+} // namespace pcl
diff --git a/registration/include/pcl/registration/correspondence_rejection_poly.h b/registration/include/pcl/registration/correspondence_rejection_poly.h
index 44fde05c..8f4f8007 100644
--- a/registration/include/pcl/registration/correspondence_rejection_poly.h
+++ b/registration/include/pcl/registration/correspondence_rejection_poly.h
@@ -41,343 +41,339 @@
 #include <pcl/registration/correspondence_rejection.h>
 #include <pcl/point_cloud.h>
 
-namespace pcl
-{
-  namespace registration
+namespace pcl {
+namespace registration {
+/** \brief CorrespondenceRejectorPoly implements a correspondence rejection method that
+ * exploits low-level and pose-invariant geometric constraints between two point sets by
+ * forming virtual polygons of a user-specifiable cardinality on each model using the
+ * input correspondences. These polygons are then checked in a pose-invariant manner
+ * (i.e. the side lengths must be approximately equal), and rejection is performed by
+ * thresholding these edge lengths.
+ *
+ * If you use this in academic work, please cite:
+ *
+ * A. G. Buch, D. Kraft, J.-K. Kämäräinen, H. G. Petersen and N. Krüger.
+ * Pose Estimation using Local Structure-Specific Shape and Appearance Context.
+ * International Conference on Robotics and Automation (ICRA), 2013.
+ *
+ * \author Anders Glent Buch
+ * \ingroup registration
+ */
+template <typename SourceT, typename TargetT>
+class PCL_EXPORTS CorrespondenceRejectorPoly : public CorrespondenceRejector {
+  using CorrespondenceRejector::getClassName;
+  using CorrespondenceRejector::input_correspondences_;
+  using CorrespondenceRejector::rejection_name_;
+
+public:
+  using Ptr = shared_ptr<CorrespondenceRejectorPoly<SourceT, TargetT>>;
+  using ConstPtr = shared_ptr<const CorrespondenceRejectorPoly<SourceT, TargetT>>;
+
+  using PointCloudSource = pcl::PointCloud<SourceT>;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+
+  using PointCloudTarget = pcl::PointCloud<TargetT>;
+  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
+  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
+
+  /** \brief Empty constructor */
+  CorrespondenceRejectorPoly()
+  : iterations_(10000)
+  , cardinality_(3)
+  , similarity_threshold_(0.75f)
+  , similarity_threshold_squared_(0.75f * 0.75f)
+  {
+    rejection_name_ = "CorrespondenceRejectorPoly";
+  }
+
+  /** \brief Get a list of valid correspondences after rejection from the original set
+   * of correspondences. \param[in] original_correspondences the set of initial
+   * correspondences given \param[out] remaining_correspondences the resultant filtered
+   * set of remaining correspondences
+   */
+  void
+  getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
+                              pcl::Correspondences& remaining_correspondences) override;
+
+  /** \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
+   */
+  inline void
+  setInputSource(const PointCloudSourceConstPtr& cloud)
+  {
+    input_ = cloud;
+  }
+
+  /** \brief Provide a target point cloud dataset (must contain XYZ data!), used to
+   * compute the correspondence distance. \param[in] target a cloud containing XYZ data
+   */
+  inline void
+  setInputTarget(const PointCloudTargetConstPtr& target)
+  {
+    target_ = target;
+  }
+
+  /** \brief See if this rejector requires source points */
+  bool
+  requiresSourcePoints() const override
+  {
+    return (true);
+  }
+
+  /** \brief Blob method for setting the source cloud */
+  void
+  setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
+  {
+    PointCloudSourcePtr cloud(new PointCloudSource);
+    fromPCLPointCloud2(*cloud2, *cloud);
+    setInputSource(cloud);
+  }
+
+  /** \brief See if this rejector requires a target cloud */
+  bool
+  requiresTargetPoints() const override
   {
-    /** \brief CorrespondenceRejectorPoly implements a correspondence rejection method that exploits low-level and
-      * pose-invariant geometric constraints between two point sets by forming virtual polygons of a user-specifiable
-      * cardinality on each model using the input correspondences.
-      * These polygons are then checked in a pose-invariant manner (i.e. the side lengths must be approximately equal),
-      * and rejection is performed by thresholding these edge lengths.
-      * 
-      * If you use this in academic work, please cite:
-      * 
-      * A. G. Buch, D. Kraft, J.-K. Kämäräinen, H. G. Petersen and N. Krüger.
-      * Pose Estimation using Local Structure-Specific Shape and Appearance Context.
-      * International Conference on Robotics and Automation (ICRA), 2013. 
-      *
-      * \author Anders Glent Buch
-      * \ingroup registration
-      */
-    template <typename SourceT, typename TargetT>
-    class PCL_EXPORTS CorrespondenceRejectorPoly: public CorrespondenceRejector
+    return (true);
+  }
+
+  /** \brief Method for setting the target cloud */
+  void
+  setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
+  {
+    PointCloudTargetPtr cloud(new PointCloudTarget);
+    fromPCLPointCloud2(*cloud2, *cloud);
+    setInputTarget(cloud);
+  }
+
+  /** \brief Set the polygon cardinality
+   * \param cardinality polygon cardinality
+   */
+  inline void
+  setCardinality(int cardinality)
+  {
+    cardinality_ = cardinality;
+  }
+
+  /** \brief Get the polygon cardinality
+   * \return polygon cardinality
+   */
+  inline int
+  getCardinality()
+  {
+    return (cardinality_);
+  }
+
+  /** \brief Set the similarity threshold in [0,1[ between edge lengths,
+   * where 1 is a perfect match
+   * \param similarity_threshold similarity threshold
+   */
+  inline void
+  setSimilarityThreshold(float similarity_threshold)
+  {
+    similarity_threshold_ = similarity_threshold;
+    similarity_threshold_squared_ = similarity_threshold * similarity_threshold;
+  }
+
+  /** \brief Get the similarity threshold between edge lengths
+   * \return similarity threshold
+   */
+  inline float
+  getSimilarityThreshold()
+  {
+    return (similarity_threshold_);
+  }
+
+  /** \brief Set the number of iterations
+   * \param iterations number of iterations
+   */
+  inline void
+  setIterations(int iterations)
+  {
+    iterations_ = iterations;
+  }
+
+  /** \brief Get the number of iterations
+   * \return number of iterations
+   */
+  inline int
+  getIterations()
+  {
+    return (iterations_);
+  }
+
+  /** \brief Polygonal rejection of a single polygon, indexed by a subset of
+   * correspondences \param corr all correspondences into \ref input_ and \ref target_
+   * \param idx sampled indices into \b correspondences, must have a size equal to \ref
+   * cardinality_ \return true if all edge length ratios are larger than or equal to
+   * \ref similarity_threshold_
+   */
+  inline bool
+  thresholdPolygon(const pcl::Correspondences& corr, const std::vector<int>& idx)
+  {
+    if (cardinality_ ==
+        2) // Special case: when two points are considered, we only have one edge
     {
-      using CorrespondenceRejector::input_correspondences_;
-      using CorrespondenceRejector::rejection_name_;
-      using CorrespondenceRejector::getClassName;
-
-      public:
-        using Ptr = shared_ptr<CorrespondenceRejectorPoly<SourceT, TargetT> >;
-        using ConstPtr = shared_ptr<const CorrespondenceRejectorPoly<SourceT, TargetT> >;
-        
-        using PointCloudSource = pcl::PointCloud<SourceT>;
-        using PointCloudSourcePtr = typename PointCloudSource::Ptr;
-        using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
-        
-        using PointCloudTarget = pcl::PointCloud<TargetT>;
-        using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
-        using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
-
-        /** \brief Empty constructor */
-        CorrespondenceRejectorPoly ()
-          : iterations_ (10000)
-          , cardinality_ (3)
-          , similarity_threshold_ (0.75f)
-          , similarity_threshold_squared_ (0.75f * 0.75f)
-        {
-          rejection_name_ = "CorrespondenceRejectorPoly";
-        }
-
-        /** \brief Get a list of valid correspondences after rejection from the original set of correspondences.
-          * \param[in] original_correspondences the set of initial correspondences given
-          * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences
-          */
-        void 
-        getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, 
-                                     pcl::Correspondences& remaining_correspondences) override;
-
-        /** \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
-          */
-        inline void 
-        setInputSource (const PointCloudSourceConstPtr &cloud)
-        {
-          input_ = 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
-          */
-        PCL_DEPRECATED(1, 12, "pcl::registration::CorrespondenceRejectorPoly::setInputCloud is deprecated. Please use setInputSource instead")
-        inline void 
-        setInputCloud (const PointCloudSourceConstPtr &cloud)
-        {
-          input_ = cloud;
-        }
-
-        /** \brief Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence distance.
-          * \param[in] target a cloud containing XYZ data
-          */
-        inline void 
-        setInputTarget (const PointCloudTargetConstPtr &target)
-        {
-          target_ = target;
-        }
-        
-        /** \brief See if this rejector requires source points */
-        bool
-        requiresSourcePoints () const override
-        { return (true); }
-
-        /** \brief Blob method for setting the source cloud */
-        void
-        setSourcePoints (pcl::PCLPointCloud2::ConstPtr cloud2) override
-        { 
-          PointCloudSourcePtr cloud (new PointCloudSource);
-          fromPCLPointCloud2 (*cloud2, *cloud);
-          setInputSource (cloud);
-        }
-        
-        /** \brief See if this rejector requires a target cloud */
-        bool
-        requiresTargetPoints () const override
-        { return (true); }
-
-        /** \brief Method for setting the target cloud */
-        void
-        setTargetPoints (pcl::PCLPointCloud2::ConstPtr cloud2) override
-        { 
-          PointCloudTargetPtr cloud (new PointCloudTarget);
-          fromPCLPointCloud2 (*cloud2, *cloud);
-          setInputTarget (cloud);
-        }
-        
-        /** \brief Set the polygon cardinality
-          * \param cardinality polygon cardinality
-          */
-        inline void 
-        setCardinality (int cardinality)
-        {
-          cardinality_ = cardinality;
-        }
-        
-        /** \brief Get the polygon cardinality
-          * \return polygon cardinality
-          */
-        inline int 
-        getCardinality ()
-        {
-          return (cardinality_);
-        }
-        
-        /** \brief Set the similarity threshold in [0,1[ between edge lengths,
-          * where 1 is a perfect match
-          * \param similarity_threshold similarity threshold
-          */
-        inline void 
-        setSimilarityThreshold (float similarity_threshold)
-        {
-          similarity_threshold_ = similarity_threshold;
-          similarity_threshold_squared_ = similarity_threshold * similarity_threshold;
-        }
-        
-        /** \brief Get the similarity threshold between edge lengths
-          * \return similarity threshold
-          */
-        inline float 
-        getSimilarityThreshold ()
-        {
-          return (similarity_threshold_);
-        }
-        
-        /** \brief Set the number of iterations
-          * \param iterations number of iterations
-          */
-        inline void 
-        setIterations (int iterations)
-        {
-          iterations_ = iterations;
-        }
-        
-        /** \brief Get the number of iterations
-          * \return number of iterations
-          */
-        inline int 
-        getIterations ()
-        {
-          return (iterations_);
-        }
-        
-        /** \brief Polygonal rejection of a single polygon, indexed by a subset of correspondences
-          * \param corr all correspondences into \ref input_ and \ref target_
-          * \param idx sampled indices into \b correspondences, must have a size equal to \ref cardinality_
-          * \return true if all edge length ratios are larger than or equal to \ref similarity_threshold_
-          */
-        inline bool 
-        thresholdPolygon (const pcl::Correspondences& corr, const std::vector<int>& idx)
-        {
-          if (cardinality_ == 2) // Special case: when two points are considered, we only have one edge
-          {
-            return (thresholdEdgeLength (corr[ idx[0] ].index_query, corr[ idx[1] ].index_query,
-                                         corr[ idx[0] ].index_match, corr[ idx[1] ].index_match,
-                                         cardinality_));
-          }
-          // Otherwise check all edges
-          for (int i = 0; i < cardinality_; ++i)
-          {
-            if (!thresholdEdgeLength (corr[ idx[i] ].index_query, corr[ idx[(i+1)%cardinality_] ].index_query,
-                                      corr[ idx[i] ].index_match, corr[ idx[(i+1)%cardinality_] ].index_match,
-                                      similarity_threshold_squared_))
-            {
-              return (false);
-            }
-          }
-          return (true);
-        }
-        
-        /** \brief Polygonal rejection of a single polygon, indexed by two point index vectors
-          * \param source_indices indices of polygon points in \ref input_, must have a size equal to \ref cardinality_
-          * \param target_indices corresponding indices of polygon points in \ref target_, must have a size equal to \ref cardinality_
-          * \return true if all edge length ratios are larger than or equal to \ref similarity_threshold_
-          */
-        inline bool 
-        thresholdPolygon (const std::vector<int>& source_indices, const std::vector<int>& target_indices)
-        {
-          // Convert indices to correspondences and an index vector pointing to each element
-          pcl::Correspondences corr (cardinality_);
-          std::vector<int> idx (cardinality_);
-          for (int i = 0; i < cardinality_; ++i)
-          {
-            corr[i].index_query = source_indices[i];
-            corr[i].index_match = target_indices[i];
-            idx[i] = i;
-          }
-          
-          return (thresholdPolygon (corr, idx));
-        }
-
-      protected:
-        /** \brief Apply the rejection algorithm.
-          * \param[out] correspondences the set of resultant correspondences.
-          */
-        inline void 
-        applyRejection (pcl::Correspondences &correspondences) override
-        {
-          getRemainingCorrespondences (*input_correspondences_, correspondences);
-        }
-        
-        /** \brief Get k unique random indices in range {0,...,n-1} (sampling without replacement)
-          * \note No check is made to ensure that k <= n.
-          * \param n upper index range, exclusive
-          * \param k number of unique indices to sample
-          * \return k unique random indices in range {0,...,n-1}
-          */
-        inline std::vector<int> 
-        getUniqueRandomIndices (int n, int k)
-        {
-          // Marked sampled indices and sample counter
-          std::vector<bool> sampled (n, false);
-          int samples = 0;
-          // Resulting unique indices
-          std::vector<int> result;
-          result.reserve (k);
-          do
-          {
-            // Pick a random index in the range
-            const int idx = (std::rand () % n);
-            // If unique
-            if (!sampled[idx])
-            {
-              // Mark as sampled and increment result counter
-              sampled[idx] = true;
-              ++samples;
-              // Store
-              result.push_back (idx);
-            }
-          }
-          while (samples < k);
-          
-          return (result);
-        }
-        
-        /** \brief Squared Euclidean distance between two points using the members x, y and z
-          * \param p1 first point
-          * \param p2 second point
-          * \return squared Euclidean distance
-          */
-        inline float 
-        computeSquaredDistance (const SourceT& p1, const TargetT& p2)
-        {
-          const float dx = p2.x - p1.x;
-          const float dy = p2.y - p1.y;
-          const float dz = p2.z - p1.z;
-          
-          return (dx*dx + dy*dy + dz*dz);
-        }
-        
-        /** \brief Edge length similarity thresholding
-          * \param index_query_1 index of first source vertex
-          * \param index_query_2 index of second source vertex
-          * \param index_match_1 index of first target vertex
-          * \param index_match_2 index of second target vertex
-          * \param simsq squared similarity threshold in [0,1]
-          * \return true if edge length ratio is larger than or equal to threshold
-          */
-        inline bool 
-        thresholdEdgeLength (int index_query_1,
-                             int index_query_2,
-                             int index_match_1,
-                             int index_match_2,
-                             float simsq)
-        {
-          // Distance between source points
-          const float dist_src = computeSquaredDistance ((*input_)[index_query_1], (*input_)[index_query_2]);
-          // Distance between target points
-          const float dist_tgt = computeSquaredDistance ((*target_)[index_match_1], (*target_)[index_match_2]);
-          // Edge length similarity [0,1] where 1 is a perfect match
-          const float edge_sim = (dist_src < dist_tgt ? dist_src / dist_tgt : dist_tgt / dist_src);
-          
-          return (edge_sim >= simsq);
-        }
-        
-        /** \brief Compute a linear histogram. This function is equivalent to the MATLAB function \b histc, with the
-          * edges set as follows: <b> lower:(upper-lower)/bins:upper </b>
-          * \param data input samples
-          * \param lower lower bound of input samples
-          * \param upper upper bound of input samples
-          * \param bins number of bins in output
-          * \return linear histogram
-          */
-        std::vector<int> 
-        computeHistogram (const std::vector<float>& data, float lower, float upper, int bins);
-        
-        /** \brief Find the optimal value for binary histogram thresholding using Otsu's method
-          * \param histogram input histogram
-          * \return threshold value according to Otsu's criterion
-          */
-        int 
-        findThresholdOtsu (const std::vector<int>& histogram);
-
-        /** \brief The input point cloud dataset */
-        PointCloudSourceConstPtr input_;
-
-        /** \brief The input point cloud dataset target */
-        PointCloudTargetConstPtr target_;
-        
-        /** \brief Number of iterations to run */
-        int iterations_;
-        
-        /** \brief The polygon cardinality used during rejection */
-        int cardinality_;
-        
-        /** \brief Lower edge length threshold in [0,1] used for verifying polygon similarities, where 1 is a perfect match */
-        float similarity_threshold_;
-        
-        /** \brief Squared value if \ref similarity_threshold_, only for internal use */
-        float similarity_threshold_squared_;
-    };
+      return (thresholdEdgeLength(corr[idx[0]].index_query,
+                                  corr[idx[1]].index_query,
+                                  corr[idx[0]].index_match,
+                                  corr[idx[1]].index_match,
+                                  cardinality_));
+    }
+    // Otherwise check all edges
+    for (int i = 0; i < cardinality_; ++i) {
+      if (!thresholdEdgeLength(corr[idx[i]].index_query,
+                               corr[idx[(i + 1) % cardinality_]].index_query,
+                               corr[idx[i]].index_match,
+                               corr[idx[(i + 1) % cardinality_]].index_match,
+                               similarity_threshold_squared_)) {
+        return (false);
+      }
+    }
+    return (true);
   }
-}
+
+  /** \brief Polygonal rejection of a single polygon, indexed by two point index vectors
+   * \param source_indices indices of polygon points in \ref input_, must have a size
+   * equal to \ref cardinality_ \param target_indices corresponding indices of polygon
+   * points in \ref target_, must have a size equal to \ref cardinality_ \return true if
+   * all edge length ratios are larger than or equal to \ref similarity_threshold_
+   */
+  inline bool
+  thresholdPolygon(const pcl::Indices& source_indices,
+                   const pcl::Indices& target_indices)
+  {
+    // Convert indices to correspondences and an index vector pointing to each element
+    pcl::Correspondences corr(cardinality_);
+    std::vector<int> idx(cardinality_);
+    for (int i = 0; i < cardinality_; ++i) {
+      corr[i].index_query = source_indices[i];
+      corr[i].index_match = target_indices[i];
+      idx[i] = i;
+    }
+
+    return (thresholdPolygon(corr, idx));
+  }
+
+protected:
+  /** \brief Apply the rejection algorithm.
+   * \param[out] correspondences the set of resultant correspondences.
+   */
+  inline void
+  applyRejection(pcl::Correspondences& correspondences) override
+  {
+    getRemainingCorrespondences(*input_correspondences_, correspondences);
+  }
+
+  /** \brief Get k unique random indices in range {0,...,n-1} (sampling without
+   * replacement) \note No check is made to ensure that k <= n. \param n upper index
+   * range, exclusive \param k number of unique indices to sample \return k unique
+   * random indices in range {0,...,n-1}
+   */
+  inline std::vector<int>
+  getUniqueRandomIndices(int n, int k)
+  {
+    // Marked sampled indices and sample counter
+    std::vector<bool> sampled(n, false);
+    int samples = 0;
+    // Resulting unique indices
+    std::vector<int> result;
+    result.reserve(k);
+    do {
+      // Pick a random index in the range
+      const int idx = (std::rand() % n);
+      // If unique
+      if (!sampled[idx]) {
+        // Mark as sampled and increment result counter
+        sampled[idx] = true;
+        ++samples;
+        // Store
+        result.push_back(idx);
+      }
+    } while (samples < k);
+
+    return (result);
+  }
+
+  /** \brief Squared Euclidean distance between two points using the members x, y and z
+   * \param p1 first point
+   * \param p2 second point
+   * \return squared Euclidean distance
+   */
+  inline float
+  computeSquaredDistance(const SourceT& p1, const TargetT& p2)
+  {
+    const float dx = p2.x - p1.x;
+    const float dy = p2.y - p1.y;
+    const float dz = p2.z - p1.z;
+
+    return (dx * dx + dy * dy + dz * dz);
+  }
+
+  /** \brief Edge length similarity thresholding
+   * \param index_query_1 index of first source vertex
+   * \param index_query_2 index of second source vertex
+   * \param index_match_1 index of first target vertex
+   * \param index_match_2 index of second target vertex
+   * \param simsq squared similarity threshold in [0,1]
+   * \return true if edge length ratio is larger than or equal to threshold
+   */
+  inline bool
+  thresholdEdgeLength(int index_query_1,
+                      int index_query_2,
+                      int index_match_1,
+                      int index_match_2,
+                      float simsq)
+  {
+    // Distance between source points
+    const float dist_src =
+        computeSquaredDistance((*input_)[index_query_1], (*input_)[index_query_2]);
+    // Distance between target points
+    const float dist_tgt =
+        computeSquaredDistance((*target_)[index_match_1], (*target_)[index_match_2]);
+    // Edge length similarity [0,1] where 1 is a perfect match
+    const float edge_sim =
+        (dist_src < dist_tgt ? dist_src / dist_tgt : dist_tgt / dist_src);
+
+    return (edge_sim >= simsq);
+  }
+
+  /** \brief Compute a linear histogram. This function is equivalent to the MATLAB
+   * function \b histc, with the edges set as follows: <b>
+   * lower:(upper-lower)/bins:upper </b> \param data input samples \param lower lower
+   * bound of input samples \param upper upper bound of input samples \param bins number
+   * of bins in output \return linear histogram
+   */
+  std::vector<int>
+  computeHistogram(const std::vector<float>& data, float lower, float upper, int bins);
+
+  /** \brief Find the optimal value for binary histogram thresholding using Otsu's
+   * method \param histogram input histogram \return threshold value according to Otsu's
+   * criterion
+   */
+  int
+  findThresholdOtsu(const std::vector<int>& histogram);
+
+  /** \brief The input point cloud dataset */
+  PointCloudSourceConstPtr input_;
+
+  /** \brief The input point cloud dataset target */
+  PointCloudTargetConstPtr target_;
+
+  /** \brief Number of iterations to run */
+  int iterations_;
+
+  /** \brief The polygon cardinality used during rejection */
+  int cardinality_;
+
+  /** \brief Lower edge length threshold in [0,1] used for verifying polygon
+   * similarities, where 1 is a perfect match */
+  float similarity_threshold_;
+
+  /** \brief Squared value if \ref similarity_threshold_, only for internal use */
+  float similarity_threshold_squared_;
+};
+} // namespace registration
+} // namespace pcl
 
 #include <pcl/registration/impl/correspondence_rejection_poly.hpp>
diff --git a/registration/include/pcl/registration/correspondence_rejection_sample_consensus.h b/registration/include/pcl/registration/correspondence_rejection_sample_consensus.h
index b165ec5f..7df68977 100644
--- a/registration/include/pcl/registration/correspondence_rejection_sample_consensus.h
+++ b/registration/include/pcl/registration/correspondence_rejection_sample_consensus.h
@@ -40,207 +40,240 @@
 
 #pragma once
 
-
-#include <pcl/memory.h>
 #include <pcl/registration/correspondence_rejection.h>
+#include <pcl/memory.h>
+
+namespace pcl {
+namespace registration {
+/** \brief CorrespondenceRejectorSampleConsensus implements a correspondence rejection
+ * using Random Sample Consensus to identify inliers (and reject outliers)
+ * \author Dirk Holz
+ * \ingroup registration
+ */
+template <typename PointT>
+class CorrespondenceRejectorSampleConsensus : public CorrespondenceRejector {
+  using PointCloud = pcl::PointCloud<PointT>;
+  using PointCloudPtr = typename PointCloud::Ptr;
+  using PointCloudConstPtr = typename PointCloud::ConstPtr;
+
+public:
+  using CorrespondenceRejector::getClassName;
+  using CorrespondenceRejector::input_correspondences_;
+  using CorrespondenceRejector::rejection_name_;
+
+  using Ptr = shared_ptr<CorrespondenceRejectorSampleConsensus<PointT>>;
+  using ConstPtr = shared_ptr<const CorrespondenceRejectorSampleConsensus<PointT>>;
+
+  /** \brief Empty constructor. Sets the inlier threshold to 5cm (0.05m),
+   * and the maximum number of iterations to 1000.
+   */
+  CorrespondenceRejectorSampleConsensus()
+  : inlier_threshold_(0.05)
+  , max_iterations_(1000) // std::numeric_limits<int>::max ()
+  , input_()
+  , input_transformed_()
+  , target_()
+  , refine_(false)
+  , save_inliers_(false)
+  {
+    rejection_name_ = "CorrespondenceRejectorSampleConsensus";
+  }
+
+  /** \brief Empty destructor. */
+  ~CorrespondenceRejectorSampleConsensus() {}
+
+  /** \brief Get a list of valid correspondences after rejection from the original set
+   * of correspondences. \param[in] original_correspondences the set of initial
+   * correspondences given \param[out] remaining_correspondences the resultant filtered
+   * set of remaining correspondences
+   */
+  inline void
+  getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
+                              pcl::Correspondences& remaining_correspondences) override;
+
+  /** \brief Provide a source point cloud dataset (must contain XYZ data!)
+   * \param[in] cloud a cloud containing XYZ data
+   */
+  virtual inline void
+  setInputSource(const PointCloudConstPtr& cloud)
+  {
+    input_ = cloud;
+  }
+
+  /** \brief Get a pointer to the input point cloud dataset target. */
+  inline PointCloudConstPtr const
+  getInputSource()
+  {
+    return (input_);
+  }
+
+  /** \brief Provide a target point cloud dataset (must contain XYZ data!)
+   * \param[in] cloud a cloud containing XYZ data
+   */
+  virtual inline void
+  setInputTarget(const PointCloudConstPtr& cloud)
+  {
+    target_ = cloud;
+  }
+
+  /** \brief Get a pointer to the input point cloud dataset target. */
+  inline PointCloudConstPtr const
+  getInputTarget()
+  {
+    return (target_);
+  }
+
+  /** \brief See if this rejector requires source points */
+  bool
+  requiresSourcePoints() const override
+  {
+    return (true);
+  }
+
+  /** \brief Blob method for setting the source cloud */
+  void
+  setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
+  {
+    PointCloudPtr cloud(new PointCloud);
+    fromPCLPointCloud2(*cloud2, *cloud);
+    setInputSource(cloud);
+  }
+
+  /** \brief See if this rejector requires a target cloud */
+  bool
+  requiresTargetPoints() const override
+  {
+    return (true);
+  }
+
+  /** \brief Method for setting the target cloud */
+  void
+  setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
+  {
+    PointCloudPtr cloud(new PointCloud);
+    fromPCLPointCloud2(*cloud2, *cloud);
+    setInputTarget(cloud);
+  }
+
+  /** \brief Set the maximum distance between corresponding points.
+   * Correspondences with distances below the threshold are considered as inliers.
+   * \param[in] threshold Distance threshold in the same dimension as source and target
+   * data sets.
+   */
+  inline void
+  setInlierThreshold(double threshold)
+  {
+    inlier_threshold_ = threshold;
+  };
+
+  /** \brief Get the maximum distance between corresponding points.
+   * \return Distance threshold in the same dimension as source and target data sets.
+   */
+  inline double
+  getInlierThreshold()
+  {
+    return inlier_threshold_;
+  };
+
+  /** \brief Set the maximum number of iterations.
+   * \param[in] max_iterations Maximum number if iterations to run
+   */
+  inline void
+  setMaximumIterations(int max_iterations)
+  {
+    max_iterations_ = std::max(max_iterations, 0);
+  }
+
+  /** \brief Get the maximum number of iterations.
+   * \return max_iterations Maximum number if iterations to run
+   */
+  inline int
+  getMaximumIterations()
+  {
+    return (max_iterations_);
+  }
+
+  /** \brief Get the best transformation after RANSAC rejection.
+   * \return The homogeneous 4x4 transformation yielding the largest number of inliers.
+   */
+  inline Eigen::Matrix4f
+  getBestTransformation()
+  {
+    return best_transformation_;
+  };
+
+  /** \brief Specify whether the model should be refined internally using the variance
+   * of the inliers \param[in] refine true if the model should be refined, false
+   * otherwise
+   */
+  inline void
+  setRefineModel(const bool refine)
+  {
+    refine_ = refine;
+  }
+
+  /** \brief Get the internal refine parameter value as set by the user using
+   * setRefineModel */
+  inline bool
+  getRefineModel() const
+  {
+    return (refine_);
+  }
+
+  /** \brief Get the inlier indices found by the correspondence rejector. This
+   * information is only saved if setSaveInliers(true) was called in advance.
+   * \param[out] inlier_indices Indices for the inliers
+   */
+  inline void
+  getInliersIndices(pcl::Indices& inlier_indices)
+  {
+    inlier_indices = inlier_indices_;
+  }
+
+  /** \brief Set whether to save inliers or not
+   * \param[in] s True to save inliers / False otherwise
+   */
+  inline void
+  setSaveInliers(bool s)
+  {
+    save_inliers_ = s;
+  }
+
+  /** \brief Get whether the rejector is configured to save inliers */
+  inline bool
+  getSaveInliers()
+  {
+    return save_inliers_;
+  }
+
+protected:
+  /** \brief Apply the rejection algorithm.
+   * \param[out] correspondences the set of resultant correspondences.
+   */
+  inline void
+  applyRejection(pcl::Correspondences& correspondences) override
+  {
+    getRemainingCorrespondences(*input_correspondences_, correspondences);
+  }
+
+  double inlier_threshold_;
+
+  int max_iterations_;
+
+  PointCloudConstPtr input_;
+  PointCloudPtr input_transformed_;
+  PointCloudConstPtr target_;
+
+  Eigen::Matrix4f best_transformation_;
+
+  bool refine_;
+  pcl::Indices inlier_indices_;
+  bool save_inliers_;
 
-#include <pcl/common/transforms.h>
-
-namespace pcl
-{
-  namespace registration
-  {
-    /** \brief CorrespondenceRejectorSampleConsensus implements a correspondence rejection
-      * using Random Sample Consensus to identify inliers (and reject outliers)
-      * \author Dirk Holz
-      * \ingroup registration
-      */
-    template <typename PointT>
-    class CorrespondenceRejectorSampleConsensus: public CorrespondenceRejector
-    {
-      using PointCloud = pcl::PointCloud<PointT>;
-      using PointCloudPtr = typename PointCloud::Ptr;
-      using PointCloudConstPtr = typename PointCloud::ConstPtr;
-
-      public:
-        using CorrespondenceRejector::input_correspondences_;
-        using CorrespondenceRejector::rejection_name_;
-        using CorrespondenceRejector::getClassName;
-
-        using Ptr = shared_ptr<CorrespondenceRejectorSampleConsensus<PointT> >;
-        using ConstPtr = shared_ptr<const CorrespondenceRejectorSampleConsensus<PointT> >;
-
-        /** \brief Empty constructor. Sets the inlier threshold to 5cm (0.05m), 
-          * and the maximum number of iterations to 1000. 
-          */
-        CorrespondenceRejectorSampleConsensus () 
-          : inlier_threshold_ (0.05)
-          , max_iterations_ (1000) // std::numeric_limits<int>::max ()
-          , input_ ()
-          , input_transformed_ ()
-          , target_ ()
-          , refine_ (false)
-          , save_inliers_ (false)
-        {
-          rejection_name_ = "CorrespondenceRejectorSampleConsensus";
-        }
-
-        /** \brief Empty destructor. */
-        ~CorrespondenceRejectorSampleConsensus () {}
-
-        /** \brief Get a list of valid correspondences after rejection from the original set of correspondences.
-          * \param[in] original_correspondences the set of initial correspondences given
-          * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences
-          */
-        inline void 
-        getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, 
-                                     pcl::Correspondences& remaining_correspondences) override;
-
-        /** \brief Provide a source point cloud dataset (must contain XYZ data!)
-          * \param[in] cloud a cloud containing XYZ data
-          */
-        virtual inline void 
-        setInputSource (const PointCloudConstPtr &cloud) 
-        { 
-          input_ = cloud; 
-        }
-
-        /** \brief Get a pointer to the input point cloud dataset target. */
-        inline PointCloudConstPtr const 
-        getInputSource () { return (input_); }
-
-        /** \brief Provide a target point cloud dataset (must contain XYZ data!)
-          * \param[in] cloud a cloud containing XYZ data
-          */
-        virtual inline void 
-        setInputTarget (const PointCloudConstPtr &cloud) { target_ = cloud; }
-
-        /** \brief Get a pointer to the input point cloud dataset target. */
-        inline PointCloudConstPtr const 
-        getInputTarget () { return (target_ ); }
-
-
-        /** \brief See if this rejector requires source points */
-        bool
-        requiresSourcePoints () const override
-        { return (true); }
-
-        /** \brief Blob method for setting the source cloud */
-        void
-        setSourcePoints (pcl::PCLPointCloud2::ConstPtr cloud2) override
-        { 
-          PointCloudPtr cloud (new PointCloud);
-          fromPCLPointCloud2 (*cloud2, *cloud);
-          setInputSource (cloud);
-        }
-        
-        /** \brief See if this rejector requires a target cloud */
-        bool
-        requiresTargetPoints () const override
-        { return (true); }
-
-        /** \brief Method for setting the target cloud */
-        void
-        setTargetPoints (pcl::PCLPointCloud2::ConstPtr cloud2) override
-        { 
-          PointCloudPtr cloud (new PointCloud);
-          fromPCLPointCloud2 (*cloud2, *cloud);
-          setInputTarget (cloud);
-        }
-
-        /** \brief Set the maximum distance between corresponding points.
-          * Correspondences with distances below the threshold are considered as inliers.
-          * \param[in] threshold Distance threshold in the same dimension as source and target data sets.
-          */
-        inline void 
-        setInlierThreshold (double threshold) { inlier_threshold_ = threshold; };
-
-        /** \brief Get the maximum distance between corresponding points.
-          * \return Distance threshold in the same dimension as source and target data sets.
-          */
-        inline double 
-        getInlierThreshold () { return inlier_threshold_; };
-
-        /** \brief Set the maximum number of iterations.
-          * \param[in] max_iterations Maximum number if iterations to run
-          */
-        inline void 
-        setMaximumIterations (int max_iterations) { max_iterations_ = std::max (max_iterations, 0); }
-
-        /** \brief Get the maximum number of iterations.
-          * \return max_iterations Maximum number if iterations to run
-          */
-        inline int 
-        getMaximumIterations () { return (max_iterations_); }
-
-        /** \brief Get the best transformation after RANSAC rejection.
-          * \return The homogeneous 4x4 transformation yielding the largest number of inliers.
-          */
-        inline Eigen::Matrix4f 
-        getBestTransformation () { return best_transformation_; };
-
-        /** \brief Specify whether the model should be refined internally using the variance of the inliers
-          * \param[in] refine true if the model should be refined, false otherwise
-          */
-        inline void
-        setRefineModel (const bool refine)
-        {
-          refine_ = refine;
-        }
-
-        /** \brief Get the internal refine parameter value as set by the user using setRefineModel */
-        inline bool
-        getRefineModel () const
-        {
-          return (refine_);
-        }
-
-        /** \brief Get the inlier indices found by the correspondence rejector. This information is only saved if setSaveInliers(true) was called in advance.
-          * \param[out] inlier_indices Indices for the inliers
-          */
-        inline void
-        getInliersIndices (std::vector<int> &inlier_indices) { inlier_indices = inlier_indices_; }
-
-        /** \brief Set whether to save inliers or not
-          * \param[in] s True to save inliers / False otherwise
-          */
-        inline void
-        setSaveInliers (bool s) { save_inliers_ = s; }
-
-        /** \brief Get whether the rejector is configured to save inliers */
-        inline bool
-        getSaveInliers () { return save_inliers_; }
-
-
-      protected:
-
-        /** \brief Apply the rejection algorithm.
-          * \param[out] correspondences the set of resultant correspondences.
-          */
-        inline void 
-        applyRejection (pcl::Correspondences &correspondences) override
-        {
-          getRemainingCorrespondences (*input_correspondences_, correspondences);
-        }
-
-        double inlier_threshold_;
-
-        int max_iterations_;
-
-        PointCloudConstPtr input_;
-        PointCloudPtr input_transformed_;
-        PointCloudConstPtr target_;
-
-        Eigen::Matrix4f best_transformation_;
-
-        bool refine_;
-        std::vector<int> inlier_indices_;
-        bool save_inliers_;
-
-      public:
-        PCL_MAKE_ALIGNED_OPERATOR_NEW
-    };
-  }
-}
+public:
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+};
+} // namespace registration
+} // namespace pcl
 
 #include <pcl/registration/impl/correspondence_rejection_sample_consensus.hpp>
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 db359a48..04db4340 100644
--- a/registration/include/pcl/registration/correspondence_rejection_sample_consensus_2d.h
+++ b/registration/include/pcl/registration/correspondence_rejection_sample_consensus_2d.h
@@ -38,125 +38,122 @@
 
 #pragma once
 
-#include <pcl/memory.h>
 #include <pcl/registration/correspondence_rejection_sample_consensus.h>
+#include <pcl/memory.h>
 
-namespace pcl
-{
-  namespace registration
+namespace pcl {
+namespace registration {
+/** \brief CorrespondenceRejectorSampleConsensus2D implements a pixel-based
+ * correspondence rejection using Random Sample Consensus to identify inliers
+ * (and reject outliers)
+ * \author Radu B. Rusu
+ * \ingroup registration
+ */
+template <typename PointT>
+class CorrespondenceRejectorSampleConsensus2D
+: public CorrespondenceRejectorSampleConsensus<PointT> {
+  using PointCloud = pcl::PointCloud<PointT>;
+  using PointCloudPtr = typename PointCloud::Ptr;
+  using PointCloudConstPtr = typename PointCloud::ConstPtr;
+
+public:
+  using CorrespondenceRejectorSampleConsensus<PointT>::refine_;
+  using CorrespondenceRejectorSampleConsensus<PointT>::input_;
+  using CorrespondenceRejectorSampleConsensus<PointT>::target_;
+  using CorrespondenceRejectorSampleConsensus<PointT>::input_correspondences_;
+  using CorrespondenceRejectorSampleConsensus<PointT>::rejection_name_;
+  using CorrespondenceRejectorSampleConsensus<PointT>::getClassName;
+  using CorrespondenceRejectorSampleConsensus<PointT>::inlier_threshold_;
+  using CorrespondenceRejectorSampleConsensus<PointT>::max_iterations_;
+  using CorrespondenceRejectorSampleConsensus<PointT>::best_transformation_;
+
+  using Ptr = shared_ptr<CorrespondenceRejectorSampleConsensus2D<PointT>>;
+  using ConstPtr = shared_ptr<const CorrespondenceRejectorSampleConsensus2D<PointT>>;
+
+  /** \brief Empty constructor. Sets the inlier threshold to 5cm (0.05m),
+   * and the maximum number of iterations to 1000.
+   */
+  CorrespondenceRejectorSampleConsensus2D()
+  : projection_matrix_(Eigen::Matrix3f::Identity())
   {
-    /** \brief CorrespondenceRejectorSampleConsensus2D implements a pixel-based 
-      * correspondence rejection using Random Sample Consensus to identify inliers 
-      * (and reject outliers)
-      * \author Radu B. Rusu
-      * \ingroup registration
-      */
-    template <typename PointT>
-    class CorrespondenceRejectorSampleConsensus2D: public CorrespondenceRejectorSampleConsensus<PointT>
-    {
-      using PointCloud = pcl::PointCloud<PointT>;
-      using PointCloudPtr = typename PointCloud::Ptr;
-      using PointCloudConstPtr = typename PointCloud::ConstPtr;
-
-      public:
-        using CorrespondenceRejectorSampleConsensus<PointT>::refine_;
-        using CorrespondenceRejectorSampleConsensus<PointT>::input_;
-        using CorrespondenceRejectorSampleConsensus<PointT>::target_;
-        using CorrespondenceRejectorSampleConsensus<PointT>::input_correspondences_;
-        using CorrespondenceRejectorSampleConsensus<PointT>::rejection_name_;
-        using CorrespondenceRejectorSampleConsensus<PointT>::getClassName;
-        using CorrespondenceRejectorSampleConsensus<PointT>::inlier_threshold_;
-        using CorrespondenceRejectorSampleConsensus<PointT>::max_iterations_;
-        using CorrespondenceRejectorSampleConsensus<PointT>::best_transformation_;
-
-        using Ptr = shared_ptr<CorrespondenceRejectorSampleConsensus2D<PointT> >;
-        using ConstPtr = shared_ptr<const CorrespondenceRejectorSampleConsensus2D<PointT> >;
-
-        /** \brief Empty constructor. Sets the inlier threshold to 5cm (0.05m), 
-          * and the maximum number of iterations to 1000. 
-          */
-        CorrespondenceRejectorSampleConsensus2D ()
-          : projection_matrix_ (Eigen::Matrix3f::Identity ())
-        {
-          rejection_name_ = "CorrespondenceRejectorSampleConsensus2D";
-          // Put the projection matrix together
-          //projection_matrix_ (0, 0) = 525.f;
-          //projection_matrix_ (1, 1) = 525.f;
-          //projection_matrix_ (0, 2) = 320.f;
-          //projection_matrix_ (1, 2) = 240.f;
-        }
-
-        /** \brief Get a list of valid correspondences after rejection from the original set of correspondences.
-          * \param[in] original_correspondences the set of initial correspondences given
-          * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences
-          */
-        inline void 
-        getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, 
-                                     pcl::Correspondences& remaining_correspondences);
-
-        /** \brief Sets the focal length parameters of the target camera.
-          * \param[in] fx the focal length in pixels along the x-axis of the image
-          * \param[in] fy the focal length in pixels along the y-axis of the image
-          */
-        inline void
-        setFocalLengths (const float fx, const float fy)
-        { 
-          projection_matrix_ (0, 0) = fx;
-          projection_matrix_ (1, 1) = fy;
-        }
-
-        /** \brief Reads back the focal length parameters of the target camera.
-          * \param[out] fx the focal length in pixels along the x-axis of the image
-          * \param[out] fy the focal length in pixels along the y-axis of the image
-          */
-        inline void
-        getFocalLengths (float &fx, float &fy) const
-        { 
-          fx = projection_matrix_ (0, 0); 
-          fy = projection_matrix_ (1, 1); 
-        }
+    rejection_name_ = "CorrespondenceRejectorSampleConsensus2D";
+    // Put the projection matrix together
+    // projection_matrix_ (0, 0) = 525.f;
+    // projection_matrix_ (1, 1) = 525.f;
+    // projection_matrix_ (0, 2) = 320.f;
+    // projection_matrix_ (1, 2) = 240.f;
+  }
 
+  /** \brief Get a list of valid correspondences after rejection from the original set
+   * of correspondences. \param[in] original_correspondences the set of initial
+   * correspondences given \param[out] remaining_correspondences the resultant filtered
+   * set of remaining correspondences
+   */
+  inline void
+  getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
+                              pcl::Correspondences& remaining_correspondences);
+
+  /** \brief Sets the focal length parameters of the target camera.
+   * \param[in] fx the focal length in pixels along the x-axis of the image
+   * \param[in] fy the focal length in pixels along the y-axis of the image
+   */
+  inline void
+  setFocalLengths(const float fx, const float fy)
+  {
+    projection_matrix_(0, 0) = fx;
+    projection_matrix_(1, 1) = fy;
+  }
 
-        /** \brief Sets the camera center parameters of the target camera.
-          * \param[in] cx the x-coordinate of the camera center
-          * \param[in] cy the y-coordinate of the camera center
-          */
-        inline void
-        setCameraCenters (const float cx, const float cy)
-        { 
-          projection_matrix_ (0, 2) = cx;
-          projection_matrix_ (1, 2) = cy;
-        }
+  /** \brief Reads back the focal length parameters of the target camera.
+   * \param[out] fx the focal length in pixels along the x-axis of the image
+   * \param[out] fy the focal length in pixels along the y-axis of the image
+   */
+  inline void
+  getFocalLengths(float& fx, float& fy) const
+  {
+    fx = projection_matrix_(0, 0);
+    fy = projection_matrix_(1, 1);
+  }
 
-        /** \brief Reads back the camera center parameters of the target camera.
-          * \param[out] cx the x-coordinate of the camera center
-          * \param[out] cy the y-coordinate of the camera center
-          */
-        inline void
-        getCameraCenters (float &cx, float &cy) const
-        {
-          cx = projection_matrix_ (0, 2);
-          cy = projection_matrix_ (1, 2);
-        }
+  /** \brief Sets the camera center parameters of the target camera.
+   * \param[in] cx the x-coordinate of the camera center
+   * \param[in] cy the y-coordinate of the camera center
+   */
+  inline void
+  setCameraCenters(const float cx, const float cy)
+  {
+    projection_matrix_(0, 2) = cx;
+    projection_matrix_(1, 2) = cy;
+  }
 
-      protected:
+  /** \brief Reads back the camera center parameters of the target camera.
+   * \param[out] cx the x-coordinate of the camera center
+   * \param[out] cy the y-coordinate of the camera center
+   */
+  inline void
+  getCameraCenters(float& cx, float& cy) const
+  {
+    cx = projection_matrix_(0, 2);
+    cy = projection_matrix_(1, 2);
+  }
 
-        /** \brief Apply the rejection algorithm.
-          * \param[out] correspondences the set of resultant correspondences.
-          */
-        inline void 
-        applyRejection (pcl::Correspondences &correspondences)
-        {
-          getRemainingCorrespondences (*input_correspondences_, correspondences);
-        }
+protected:
+  /** \brief Apply the rejection algorithm.
+   * \param[out] correspondences the set of resultant correspondences.
+   */
+  inline void
+  applyRejection(pcl::Correspondences& correspondences)
+  {
+    getRemainingCorrespondences(*input_correspondences_, correspondences);
+  }
 
-        /** \brief Camera projection matrix. */
-        Eigen::Matrix3f projection_matrix_;
+  /** \brief Camera projection matrix. */
+  Eigen::Matrix3f projection_matrix_;
 
-      public:
-        PCL_MAKE_ALIGNED_OPERATOR_NEW
-    };
-  }
-}
+public:
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+};
+} // namespace registration
+} // namespace pcl
 
 #include <pcl/registration/impl/correspondence_rejection_sample_consensus_2d.hpp>
diff --git a/registration/include/pcl/registration/correspondence_rejection_surface_normal.h b/registration/include/pcl/registration/correspondence_rejection_surface_normal.h
index 510b8a64..52027610 100644
--- a/registration/include/pcl/registration/correspondence_rejection_surface_normal.h
+++ b/registration/include/pcl/registration/correspondence_rejection_surface_normal.h
@@ -40,286 +40,314 @@
 #pragma once
 
 #include <pcl/registration/correspondence_rejection.h>
-#include <pcl/memory.h>  // for static_pointer_cast
+#include <pcl/conversions.h> // for fromPCLPointCloud2
+#include <pcl/memory.h>      // for static_pointer_cast
 #include <pcl/point_cloud.h>
 
+namespace pcl {
+namespace registration {
+/**
+ * @b CorrespondenceRejectorSurfaceNormal implements a simple correspondence
+ * rejection method based on the angle between the normals at correspondent points.
+ *
+ * \note If \ref setInputCloud and \ref setInputTarget are given, then the
+ * distances between correspondences will be estimated using the given XYZ
+ * data, and not read from the set of input correspondences.
+ *
+ * \author Aravindhan K Krishnan (original code from libpointmatcher:
+ * https://github.com/ethz-asl/libpointmatcher) \ingroup registration
+ */
+class PCL_EXPORTS CorrespondenceRejectorSurfaceNormal : public CorrespondenceRejector {
+  using CorrespondenceRejector::getClassName;
+  using CorrespondenceRejector::input_correspondences_;
+  using CorrespondenceRejector::rejection_name_;
 
-namespace pcl
-{
-  namespace registration
-  {
-    /**
-      * @b CorrespondenceRejectorSurfaceNormal implements a simple correspondence
-      * rejection method based on the angle between the normals at correspondent points.
-      *
-      * \note If \ref setInputCloud and \ref setInputTarget are given, then the
-      * distances between correspondences will be estimated using the given XYZ
-      * data, and not read from the set of input correspondences.
-      *
-      * \author Aravindhan K Krishnan (original code from libpointmatcher: https://github.com/ethz-asl/libpointmatcher)
-      * \ingroup registration
-      */
-    class PCL_EXPORTS CorrespondenceRejectorSurfaceNormal: public CorrespondenceRejector
-    {
-      using CorrespondenceRejector::input_correspondences_;
-      using CorrespondenceRejector::rejection_name_;
-      using CorrespondenceRejector::getClassName;
-
-      public:
-        using Ptr = shared_ptr<CorrespondenceRejectorSurfaceNormal>;
-        using ConstPtr = shared_ptr<const CorrespondenceRejectorSurfaceNormal>;
-
-        /** \brief Empty constructor. Sets the threshold to 1.0. */
-        CorrespondenceRejectorSurfaceNormal () 
-          : threshold_ (1.0)
-        {
-          rejection_name_ = "CorrespondenceRejectorSurfaceNormal";
-        }
-
-        /** \brief Get a list of valid correspondences after rejection from the original set of correspondences.
-          * \param[in] original_correspondences the set of initial correspondences given
-          * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences
-          */
-        void 
-        getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, 
-                                     pcl::Correspondences& remaining_correspondences) override;
+public:
+  using Ptr = shared_ptr<CorrespondenceRejectorSurfaceNormal>;
+  using ConstPtr = shared_ptr<const CorrespondenceRejectorSurfaceNormal>;
 
-        /** \brief Set the thresholding angle between the normals for correspondence rejection. 
-          * \param[in] threshold cosine of the thresholding angle between the normals for rejection
-          */
-        inline void
-        setThreshold (double threshold) { threshold_ = threshold; };
+  /** \brief Empty constructor. Sets the threshold to 1.0. */
+  CorrespondenceRejectorSurfaceNormal() : threshold_(1.0)
+  {
+    rejection_name_ = "CorrespondenceRejectorSurfaceNormal";
+  }
 
-        /** \brief Get the thresholding angle between the normals for correspondence rejection. */
-        inline double
-        getThreshold () const { return threshold_; };
+  /** \brief Get a list of valid correspondences after rejection from the original set
+   * of correspondences. \param[in] original_correspondences the set of initial
+   * correspondences given \param[out] remaining_correspondences the resultant filtered
+   * set of remaining correspondences
+   */
+  void
+  getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
+                              pcl::Correspondences& remaining_correspondences) override;
 
-        /** \brief Initialize the data container object for the point type and the normal type. */
-        template <typename PointT, typename NormalT> inline void 
-        initializeDataContainer ()
-        {
-          data_container_.reset (new DataContainer<PointT, NormalT>);
-        }
+  /** \brief Set the thresholding angle between the normals for correspondence
+   * rejection. \param[in] threshold cosine of the thresholding angle between the
+   * normals for rejection
+   */
+  inline void
+  setThreshold(double threshold)
+  {
+    threshold_ = threshold;
+  };
 
-        /** \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 <typename PointT>
-        PCL_DEPRECATED(1, 12, "pcl::registration::CorrespondenceRejectorSurfaceNormal::setInputCloud is deprecated. Please use setInputSource instead")
-        inline void 
-        setInputCloud (const typename pcl::PointCloud<PointT>::ConstPtr &input)
-        {
-          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;
-          }
-          static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (input);
-        }
+  /** \brief Get the thresholding angle between the normals for correspondence
+   * rejection. */
+  inline double
+  getThreshold() const
+  {
+    return threshold_;
+  };
 
-        /** \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 <typename PointT> inline void 
-        setInputSource (const typename pcl::PointCloud<PointT>::ConstPtr &input)
-        {
-          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;
-          }
-          static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (input);
-        }
+  /** \brief Initialize the data container object for the point type and the normal
+   * type. */
+  template <typename PointT, typename NormalT>
+  inline void
+  initializeDataContainer()
+  {
+    data_container_.reset(new DataContainer<PointT, NormalT>);
+  }
 
-        /** \brief Get the target input point cloud */
-        template <typename PointT> inline typename pcl::PointCloud<PointT>::ConstPtr
-        getInputSource () const 
-        { 
-          if (!data_container_)
-          {
-            PCL_ERROR ("[pcl::registration::%s::getInputSource] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
-            return;
-          }
-          return (static_pointer_cast<DataContainer<PointT> > (data_container_)->getInputSource ());
-        }
+  /** \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 <typename PointT>
+  inline void
+  setInputSource(const typename pcl::PointCloud<PointT>::ConstPtr& input)
+  {
+    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;
+    }
+    static_pointer_cast<DataContainer<PointT>>(data_container_)->setInputSource(input);
+  }
 
-        /** \brief Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence distance.  
-          * \param[in] target a cloud containing XYZ data
-          */
-        template <typename PointT> inline void 
-        setInputTarget (const typename pcl::PointCloud<PointT>::ConstPtr &target)
-        {
-          if (!data_container_)
-          {
-            PCL_ERROR ("[pcl::registration::%s::setInputTarget] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
-            return;
-          }
-          static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputTarget (target);
-        }
+  /** \brief Get the target input point cloud */
+  template <typename PointT>
+  inline typename pcl::PointCloud<PointT>::ConstPtr
+  getInputSource() const
+  {
+    if (!data_container_) {
+      PCL_ERROR(
+          "[pcl::registration::%s::getInputSource] Initialize the data container "
+          "object by calling intializeDataContainer () before using this function.\n",
+          getClassName().c_str());
+      return;
+    }
+    return (
+        static_pointer_cast<DataContainer<PointT>>(data_container_)->getInputSource());
+  }
 
-        /** \brief Provide a pointer to the search object used to find correspondences in
-          * the target cloud.
-          * \param[in] tree a pointer to the spatial search object.
-          * \param[in] force_no_recompute If set to true, this tree will NEVER be 
-          * recomputed, regardless of calls to setInputTarget. Only use if you are 
-          * confident that the tree will be set correctly.
-          */
-        template <typename PointT> inline void
-        setSearchMethodTarget (const typename pcl::search::KdTree<PointT>::Ptr &tree,
-                               bool force_no_recompute = false)
-        { 
-          static_pointer_cast< DataContainer<PointT> > 
-            (data_container_)->setSearchMethodTarget (tree, force_no_recompute );
-        }
+  /** \brief Provide a target point cloud dataset (must contain XYZ data!), used to
+   * compute the correspondence distance. \param[in] target a cloud containing XYZ data
+   */
+  template <typename PointT>
+  inline void
+  setInputTarget(const typename pcl::PointCloud<PointT>::ConstPtr& target)
+  {
+    if (!data_container_) {
+      PCL_ERROR(
+          "[pcl::registration::%s::setInputTarget] Initialize the data container "
+          "object by calling intializeDataContainer () before using this function.\n",
+          getClassName().c_str());
+      return;
+    }
+    static_pointer_cast<DataContainer<PointT>>(data_container_)->setInputTarget(target);
+  }
 
-        /** \brief Get the target input point cloud */
-        template <typename PointT> inline typename pcl::PointCloud<PointT>::ConstPtr
-        getInputTarget () const 
-        { 
-          if (!data_container_)
-          {
-            PCL_ERROR ("[pcl::registration::%s::getInputTarget] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
-            return;
-          }
-          return (static_pointer_cast<DataContainer<PointT> > (data_container_)->getInputTarget ());
-        }
+  /** \brief Provide a pointer to the search object used to find correspondences in
+   * the target cloud.
+   * \param[in] tree a pointer to the spatial search object.
+   * \param[in] force_no_recompute If set to true, this tree will NEVER be
+   * recomputed, regardless of calls to setInputTarget. Only use if you are
+   * confident that the tree will be set correctly.
+   */
+  template <typename PointT>
+  inline void
+  setSearchMethodTarget(const typename pcl::search::KdTree<PointT>::Ptr& tree,
+                        bool force_no_recompute = false)
+  {
+    static_pointer_cast<DataContainer<PointT>>(data_container_)
+        ->setSearchMethodTarget(tree, force_no_recompute);
+  }
 
-        /** \brief Set the normals computed on the input point cloud
-          * \param[in] normals the normals computed for the input cloud
-          */
-        template <typename PointT, typename NormalT> inline void 
-        setInputNormals (const typename pcl::PointCloud<NormalT>::ConstPtr &normals)
-        {
-          if (!data_container_)
-          {
-            PCL_ERROR ("[pcl::registration::%s::setInputNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
-            return;
-          }
-          static_pointer_cast<DataContainer<PointT, NormalT> > (data_container_)->setInputNormals (normals);
-        }
+  /** \brief Get the target input point cloud */
+  template <typename PointT>
+  inline typename pcl::PointCloud<PointT>::ConstPtr
+  getInputTarget() const
+  {
+    if (!data_container_) {
+      PCL_ERROR(
+          "[pcl::registration::%s::getInputTarget] Initialize the data container "
+          "object by calling intializeDataContainer () before using this function.\n",
+          getClassName().c_str());
+      return;
+    }
+    return (
+        static_pointer_cast<DataContainer<PointT>>(data_container_)->getInputTarget());
+  }
 
-        /** \brief Get the normals computed on the input point cloud */
-        template <typename NormalT> inline typename pcl::PointCloud<NormalT>::Ptr
-        getInputNormals () const 
-        { 
-          if (!data_container_)
-          {
-            PCL_ERROR ("[pcl::registration::%s::getInputNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
-            return;
-          }
-          return (static_pointer_cast<DataContainer<pcl::PointXYZ, NormalT> > (data_container_)->getInputNormals ());
-        }
+  /** \brief Set the normals computed on the input point cloud
+   * \param[in] normals the normals computed for the input cloud
+   */
+  template <typename PointT, typename NormalT>
+  inline void
+  setInputNormals(const typename pcl::PointCloud<NormalT>::ConstPtr& normals)
+  {
+    if (!data_container_) {
+      PCL_ERROR(
+          "[pcl::registration::%s::setInputNormals] Initialize the data container "
+          "object by calling intializeDataContainer () before using this function.\n",
+          getClassName().c_str());
+      return;
+    }
+    static_pointer_cast<DataContainer<PointT, NormalT>>(data_container_)
+        ->setInputNormals(normals);
+  }
 
-        /** \brief Set the normals computed on the target point cloud
-          * \param[in] normals the normals computed for the input cloud
-          */
-        template <typename PointT, typename NormalT> inline void 
-        setTargetNormals (const typename pcl::PointCloud<NormalT>::ConstPtr &normals)
-        {
-          if (!data_container_)
-          {
-            PCL_ERROR ("[pcl::registration::%s::setTargetNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
-            return;
-          }
-          static_pointer_cast<DataContainer<PointT, NormalT> > (data_container_)->setTargetNormals (normals);
-        }
+  /** \brief Get the normals computed on the input point cloud */
+  template <typename NormalT>
+  inline typename pcl::PointCloud<NormalT>::Ptr
+  getInputNormals() const
+  {
+    if (!data_container_) {
+      PCL_ERROR(
+          "[pcl::registration::%s::getInputNormals] Initialize the data container "
+          "object by calling intializeDataContainer () before using this function.\n",
+          getClassName().c_str());
+      return;
+    }
+    return (static_pointer_cast<DataContainer<pcl::PointXYZ, NormalT>>(data_container_)
+                ->getInputNormals());
+  }
 
-        /** \brief Get the normals computed on the target point cloud */
-        template <typename NormalT> inline typename pcl::PointCloud<NormalT>::Ptr
-        getTargetNormals () const 
-        { 
-          if (!data_container_)
-          {
-            PCL_ERROR ("[pcl::registration::%s::getTargetNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
-            return;
-          }
-          return (static_pointer_cast<DataContainer<pcl::PointXYZ, NormalT> > (data_container_)->getTargetNormals ());
-        }
+  /** \brief Set the normals computed on the target point cloud
+   * \param[in] normals the normals computed for the input cloud
+   */
+  template <typename PointT, typename NormalT>
+  inline void
+  setTargetNormals(const typename pcl::PointCloud<NormalT>::ConstPtr& normals)
+  {
+    if (!data_container_) {
+      PCL_ERROR(
+          "[pcl::registration::%s::setTargetNormals] Initialize the data container "
+          "object by calling intializeDataContainer () before using this function.\n",
+          getClassName().c_str());
+      return;
+    }
+    static_pointer_cast<DataContainer<PointT, NormalT>>(data_container_)
+        ->setTargetNormals(normals);
+  }
 
+  /** \brief Get the normals computed on the target point cloud */
+  template <typename NormalT>
+  inline typename pcl::PointCloud<NormalT>::Ptr
+  getTargetNormals() const
+  {
+    if (!data_container_) {
+      PCL_ERROR(
+          "[pcl::registration::%s::getTargetNormals] Initialize the data container "
+          "object by calling intializeDataContainer () before using this function.\n",
+          getClassName().c_str());
+      return;
+    }
+    return (static_pointer_cast<DataContainer<pcl::PointXYZ, NormalT>>(data_container_)
+                ->getTargetNormals());
+  }
 
-        /** \brief See if this rejector requires source points */
-        bool
-        requiresSourcePoints () const override
-        { return (true); }
+  /** \brief See if this rejector requires source points */
+  bool
+  requiresSourcePoints() const override
+  {
+    return (true);
+  }
 
-        /** \brief Blob method for setting the source cloud */
-        void
-        setSourcePoints (pcl::PCLPointCloud2::ConstPtr cloud2) override
-        { 
-          if (!data_container_)
-            initializeDataContainer<PointXYZ, Normal> ();
-          PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
-          fromPCLPointCloud2 (*cloud2, *cloud);
-          setInputSource<PointXYZ> (cloud);
-        }
-        
-        /** \brief See if this rejector requires a target cloud */
-        bool
-        requiresTargetPoints () const override
-        { return (true); }
+  /** \brief Blob method for setting the source cloud */
+  void
+  setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
+  {
+    if (!data_container_)
+      initializeDataContainer<PointXYZ, Normal>();
+    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
+    fromPCLPointCloud2(*cloud2, *cloud);
+    setInputSource<PointXYZ>(cloud);
+  }
 
-        /** \brief Method for setting the target cloud */
-        void
-        setTargetPoints (pcl::PCLPointCloud2::ConstPtr cloud2) override
-        { 
-          if (!data_container_)
-            initializeDataContainer<PointXYZ, Normal> ();
-          PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
-          fromPCLPointCloud2 (*cloud2, *cloud);
-          setInputTarget<PointXYZ> (cloud);
-        }
-        
-        /** \brief See if this rejector requires source normals */
-        bool
-        requiresSourceNormals () const override
-        { return (true); }
+  /** \brief See if this rejector requires a target cloud */
+  bool
+  requiresTargetPoints() const override
+  {
+    return (true);
+  }
 
-        /** \brief Blob method for setting the source normals */
-        void
-        setSourceNormals (pcl::PCLPointCloud2::ConstPtr cloud2) override
-        { 
-          if (!data_container_)
-            initializeDataContainer<PointXYZ, Normal> ();
-          PointCloud<Normal>::Ptr cloud (new PointCloud<Normal>);
-          fromPCLPointCloud2 (*cloud2, *cloud);
-          setInputNormals<PointXYZ, Normal> (cloud);
-        }
-        
-        /** \brief See if this rejector requires target normals*/
-        bool
-        requiresTargetNormals () const override
-        { return (true); }
+  /** \brief Method for setting the target cloud */
+  void
+  setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
+  {
+    if (!data_container_)
+      initializeDataContainer<PointXYZ, Normal>();
+    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
+    fromPCLPointCloud2(*cloud2, *cloud);
+    setInputTarget<PointXYZ>(cloud);
+  }
 
-        /** \brief Method for setting the target normals */
-        void
-        setTargetNormals (pcl::PCLPointCloud2::ConstPtr cloud2) override
-        { 
-          if (!data_container_)
-            initializeDataContainer<PointXYZ, Normal> ();
-          PointCloud<Normal>::Ptr cloud (new PointCloud<Normal>);
-          fromPCLPointCloud2 (*cloud2, *cloud);
-          setTargetNormals<PointXYZ, Normal> (cloud);
-        }
+  /** \brief See if this rejector requires source normals */
+  bool
+  requiresSourceNormals() const override
+  {
+    return (true);
+  }
 
-      protected:
+  /** \brief Blob method for setting the source normals */
+  void
+  setSourceNormals(pcl::PCLPointCloud2::ConstPtr cloud2) override
+  {
+    if (!data_container_)
+      initializeDataContainer<PointXYZ, Normal>();
+    PointCloud<Normal>::Ptr cloud(new PointCloud<Normal>);
+    fromPCLPointCloud2(*cloud2, *cloud);
+    setInputNormals<PointXYZ, Normal>(cloud);
+  }
 
-        /** \brief Apply the rejection algorithm.
-          * \param[out] correspondences the set of resultant correspondences.
-          */
-        inline void 
-        applyRejection (pcl::Correspondences &correspondences) override
-        {
-          getRemainingCorrespondences (*input_correspondences_, correspondences);
-        }
+  /** \brief See if this rejector requires target normals*/
+  bool
+  requiresTargetNormals() const override
+  {
+    return (true);
+  }
 
-        /** \brief The median distance threshold between two correspondent points in source <-> target. */
-        double threshold_;
+  /** \brief Method for setting the target normals */
+  void
+  setTargetNormals(pcl::PCLPointCloud2::ConstPtr cloud2) override
+  {
+    if (!data_container_)
+      initializeDataContainer<PointXYZ, Normal>();
+    PointCloud<Normal>::Ptr cloud(new PointCloud<Normal>);
+    fromPCLPointCloud2(*cloud2, *cloud);
+    setTargetNormals<PointXYZ, Normal>(cloud);
+  }
 
-        using DataContainerPtr = DataContainerInterface::Ptr;
-        /** \brief A pointer to the DataContainer object containing the input and target point clouds */
-        DataContainerPtr data_container_;
-    };
+protected:
+  /** \brief Apply the rejection algorithm.
+   * \param[out] correspondences the set of resultant correspondences.
+   */
+  inline void
+  applyRejection(pcl::Correspondences& correspondences) override
+  {
+    getRemainingCorrespondences(*input_correspondences_, correspondences);
   }
-}
 
-#include <pcl/registration/impl/correspondence_rejection_surface_normal.hpp>
+  /** \brief The median distance threshold between two correspondent points in source
+   * <-> target. */
+  double threshold_;
+
+  using DataContainerPtr = DataContainerInterface::Ptr;
+  /** \brief A pointer to the DataContainer object containing the input and target point
+   * clouds */
+  DataContainerPtr data_container_;
+};
+} // namespace registration
+} // namespace pcl
diff --git a/registration/include/pcl/registration/correspondence_rejection_trimmed.h b/registration/include/pcl/registration/correspondence_rejection_trimmed.h
index e645161c..66e0def0 100644
--- a/registration/include/pcl/registration/correspondence_rejection_trimmed.h
+++ b/registration/include/pcl/registration/correspondence_rejection_trimmed.h
@@ -42,98 +42,104 @@
 
 #include <pcl/registration/correspondence_rejection.h>
 
-namespace pcl
-{
-  namespace registration
-  {
-    /** \brief CorrespondenceRejectorTrimmed implements a correspondence
-      * rejection for ICP-like registration algorithms that uses only the best
-      * 'k' correspondences where 'k' is some estimate of the overlap between
-      * the two point clouds being registered.
-      *
-      * Reference:
-      * 'The Trimmed Iterative Closest Point Algorithm' by
-      * D. Chetverikov, D. Svirko, D. Stepanov, and Pavel Krsek.
-      * In Proceedings of the 16th International Conference on Pattern
-      * Recognition (ICPR 2002).
-      *
-      * \author Dirk Holz
-      * \ingroup registration
-      */
-    class PCL_EXPORTS CorrespondenceRejectorTrimmed: public CorrespondenceRejector
-    {
-      using CorrespondenceRejector::input_correspondences_;
-      using CorrespondenceRejector::rejection_name_;
-      using CorrespondenceRejector::getClassName;
-
-      public:
-        using Ptr = shared_ptr<CorrespondenceRejectorTrimmed>;
-        using ConstPtr = shared_ptr<const CorrespondenceRejectorTrimmed>;
-
-        /** \brief Empty constructor. */
-        CorrespondenceRejectorTrimmed () : 
-          overlap_ratio_ (0.5f),
-          nr_min_correspondences_ (0)
-        {
-          rejection_name_ = "CorrespondenceRejectorTrimmed";
-        }
-
-        /** \brief Destructor. */
-        ~CorrespondenceRejectorTrimmed () {}
-
-        /** \brief Set the expected ratio of overlap between point clouds (in
-          * terms of correspondences).
-          * \param[in] ratio ratio of overlap between 0 (no overlap, no
-          * correspondences) and 1 (full overlap, all correspondences)
-          */
-        virtual inline void 
-        setOverlapRatio (float ratio) { overlap_ratio_ = std::min (1.0f, std::max (0.0f, ratio)); };
-
-        /** \brief Get the maximum distance used for thresholding in correspondence rejection. */
-        inline float 
-        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
-          * \a nr_min_correspondences_ correspondences (or all correspondences in case \a nr_min_correspondences_
-          * is less than the number of given correspondences). 
-          * \param[in] min_correspondences the minimum number of correspondences
-          */
-        inline void 
-        setMinCorrespondences (unsigned int min_correspondences) { nr_min_correspondences_ = min_correspondences; };
+namespace pcl {
+namespace registration {
+/** \brief CorrespondenceRejectorTrimmed implements a correspondence
+ * rejection for ICP-like registration algorithms that uses only the best
+ * 'k' correspondences where 'k' is some estimate of the overlap between
+ * the two point clouds being registered.
+ *
+ * Reference:
+ * 'The Trimmed Iterative Closest Point Algorithm' by
+ * D. Chetverikov, D. Svirko, D. Stepanov, and Pavel Krsek.
+ * In Proceedings of the 16th International Conference on Pattern
+ * Recognition (ICPR 2002).
+ *
+ * \author Dirk Holz
+ * \ingroup registration
+ */
+class PCL_EXPORTS CorrespondenceRejectorTrimmed : public CorrespondenceRejector {
+  using CorrespondenceRejector::getClassName;
+  using CorrespondenceRejector::input_correspondences_;
+  using CorrespondenceRejector::rejection_name_;
 
-        /** \brief Get the minimum number of correspondences. */
-        inline unsigned int 
-        getMinCorrespondences () const { return nr_min_correspondences_; };
+public:
+  using Ptr = shared_ptr<CorrespondenceRejectorTrimmed>;
+  using ConstPtr = shared_ptr<const CorrespondenceRejectorTrimmed>;
 
+  /** \brief Empty constructor. */
+  CorrespondenceRejectorTrimmed() : overlap_ratio_(0.5f), nr_min_correspondences_(0)
+  {
+    rejection_name_ = "CorrespondenceRejectorTrimmed";
+  }
 
-        /** \brief Get a list of valid correspondences after rejection from the original set of correspondences.
-          * \param[in] original_correspondences the set of initial correspondences given
-          * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences
-          */
-        void
-        getRemainingCorrespondences (const pcl::Correspondences& original_correspondences,
-                                     pcl::Correspondences& remaining_correspondences) override;
+  /** \brief Destructor. */
+  ~CorrespondenceRejectorTrimmed() {}
 
-      protected:
+  /** \brief Set the expected ratio of overlap between point clouds (in
+   * terms of correspondences).
+   * \param[in] ratio ratio of overlap between 0 (no overlap, no
+   * correspondences) and 1 (full overlap, all correspondences)
+   */
+  virtual inline void
+  setOverlapRatio(float ratio)
+  {
+    overlap_ratio_ = std::min(1.0f, std::max(0.0f, ratio));
+  };
 
-        /** \brief Apply the rejection algorithm.
-          * \param[out] correspondences the set of resultant correspondences.
-          */
-        inline void 
-        applyRejection (pcl::Correspondences &correspondences) override
-        {
-          getRemainingCorrespondences (*input_correspondences_, correspondences);
-        }
+  /** \brief Get the maximum distance used for thresholding in correspondence rejection.
+   */
+  inline float
+  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 \a nr_min_correspondences_ correspondences (or all correspondences
+   * in case \a nr_min_correspondences_ is less than the number of given
+   * correspondences). \param[in] min_correspondences the minimum number of
+   * correspondences
+   */
+  inline void
+  setMinCorrespondences(unsigned int min_correspondences)
+  {
+    nr_min_correspondences_ = min_correspondences;
+  };
 
-        /** Overlap Ratio in [0..1] */
-        float overlap_ratio_;
+  /** \brief Get the minimum number of correspondences. */
+  inline unsigned int
+  getMinCorrespondences() const
+  {
+    return nr_min_correspondences_;
+  };
+
+  /** \brief Get a list of valid correspondences after rejection from the original set
+   * of correspondences. \param[in] original_correspondences the set of initial
+   * correspondences given \param[out] remaining_correspondences the resultant filtered
+   * set of remaining correspondences
+   */
+  void
+  getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
+                              pcl::Correspondences& remaining_correspondences) override;
+
+protected:
+  /** \brief Apply the rejection algorithm.
+   * \param[out] correspondences the set of resultant correspondences.
+   */
+  inline void
+  applyRejection(pcl::Correspondences& correspondences) override
+  {
+    getRemainingCorrespondences(*input_correspondences_, correspondences);
+  }
 
-        /** Minimum number of correspondences. */
-        unsigned int nr_min_correspondences_;
-    };
+  /** Overlap Ratio in [0..1] */
+  float overlap_ratio_;
 
-  }
-}
+  /** Minimum number of correspondences. */
+  unsigned int nr_min_correspondences_;
+};
 
-#include <pcl/registration/impl/correspondence_rejection_trimmed.hpp>
+} // namespace registration
+} // namespace pcl
diff --git a/registration/include/pcl/registration/correspondence_rejection_var_trimmed.h b/registration/include/pcl/registration/correspondence_rejection_var_trimmed.h
index 93753a48..e5b5c7a7 100644
--- a/registration/include/pcl/registration/correspondence_rejection_var_trimmed.h
+++ b/registration/include/pcl/registration/correspondence_rejection_var_trimmed.h
@@ -40,215 +40,223 @@
 #pragma once
 
 #include <pcl/registration/correspondence_rejection.h>
-#include <pcl/memory.h>  // for static_pointer_cast
+#include <pcl/conversions.h> // for fromPCLPointCloud2
+#include <pcl/memory.h>      // for static_pointer_cast
 #include <pcl/point_cloud.h>
 
 #include <vector>
 
-namespace pcl
-{
-  namespace registration
+namespace pcl {
+namespace registration {
+/**
+ * @b CorrespondenceRejectoVarTrimmed implements a simple correspondence
+ * rejection method by considering as inliers a certain percentage of correspondences
+ * with the least distances. The percentage of inliers is computed internally as
+ * mentioned in the paper 'Outlier Robust ICP for minimizing Fractional RMSD, J. M.
+ * Philips et al'
+ *
+ * \note If \ref setInputCloud and \ref setInputTarget are given, then the
+ * distances between correspondences will be estimated using the given XYZ
+ * data, and not read from the set of input correspondences.
+ *
+ * \author Aravindhan K Krishnan. This code is ported from libpointmatcher
+ * (https://github.com/ethz-asl/libpointmatcher) \ingroup registration
+ */
+class PCL_EXPORTS CorrespondenceRejectorVarTrimmed : public CorrespondenceRejector {
+  using CorrespondenceRejector::getClassName;
+  using CorrespondenceRejector::input_correspondences_;
+  using CorrespondenceRejector::rejection_name_;
+
+public:
+  using Ptr = shared_ptr<CorrespondenceRejectorVarTrimmed>;
+  using ConstPtr = shared_ptr<const CorrespondenceRejectorVarTrimmed>;
+
+  /** \brief Empty constructor. */
+  CorrespondenceRejectorVarTrimmed()
+  : trimmed_distance_(0), factor_(), min_ratio_(0.05), max_ratio_(0.95), lambda_(0.95)
+  {
+    rejection_name_ = "CorrespondenceRejectorVarTrimmed";
+  }
+
+  /** \brief Get a list of valid correspondences after rejection from the original set
+   * of correspondences. \param[in] original_correspondences the set of initial
+   * correspondences given \param[out] remaining_correspondences the resultant filtered
+   * set of remaining correspondences
+   */
+  void
+  getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
+                              pcl::Correspondences& remaining_correspondences) override;
+
+  /** \brief Get the trimmed distance used for thresholding in correspondence rejection.
+   */
+  inline double
+  getTrimmedDistance() const
+  {
+    return trimmed_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 <typename PointT>
+  inline void
+  setInputSource(const typename pcl::PointCloud<PointT>::ConstPtr& cloud)
+  {
+    if (!data_container_)
+      data_container_.reset(new DataContainer<PointT>);
+    static_pointer_cast<DataContainer<PointT>>(data_container_)->setInputSource(cloud);
+  }
+
+  /** \brief Provide a target point cloud dataset (must contain XYZ
+   * data!), used to compute the correspondence distance.
+   * \param[in] target a cloud containing XYZ data
+   */
+  template <typename PointT>
+  inline void
+  setInputTarget(const typename pcl::PointCloud<PointT>::ConstPtr& target)
+  {
+    if (!data_container_)
+      data_container_.reset(new DataContainer<PointT>);
+    static_pointer_cast<DataContainer<PointT>>(data_container_)->setInputTarget(target);
+  }
+
+  /** \brief See if this rejector requires source points */
+  bool
+  requiresSourcePoints() const override
+  {
+    return (true);
+  }
+
+  /** \brief Blob method for setting the source cloud */
+  void
+  setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
+  {
+    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
+    fromPCLPointCloud2(*cloud2, *cloud);
+    setInputSource<PointXYZ>(cloud);
+  }
+
+  /** \brief See if this rejector requires a target cloud */
+  bool
+  requiresTargetPoints() const override
+  {
+    return (true);
+  }
+
+  /** \brief Method for setting the target cloud */
+  void
+  setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
+  {
+    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
+    fromPCLPointCloud2(*cloud2, *cloud);
+    setInputTarget<PointXYZ>(cloud);
+  }
+
+  /** \brief Provide a pointer to the search object used to find correspondences in
+   * the target cloud.
+   * \param[in] tree a pointer to the spatial search object.
+   * \param[in] force_no_recompute If set to true, this tree will NEVER be
+   * recomputed, regardless of calls to setInputTarget. Only use if you are
+   * confident that the tree will be set correctly.
+   */
+  template <typename PointT>
+  inline void
+  setSearchMethodTarget(const typename pcl::search::KdTree<PointT>::Ptr& tree,
+                        bool force_no_recompute = false)
+  {
+    static_pointer_cast<DataContainer<PointT>>(data_container_)
+        ->setSearchMethodTarget(tree, force_no_recompute);
+  }
+
+  /** \brief Get the computed inlier ratio used for thresholding in correspondence
+   * rejection. */
+  inline double
+  getTrimFactor() const
+  {
+    return factor_;
+  }
+
+  /** brief set the minimum overlap ratio
+   * \param[in] ratio the overlap ratio [0..1]
+   */
+  inline void
+  setMinRatio(double ratio)
+  {
+    min_ratio_ = ratio;
+  }
+
+  /** brief get the minimum overlap ratio
+   */
+  inline double
+  getMinRatio() const
+  {
+    return min_ratio_;
+  }
+
+  /** brief set the maximum overlap ratio
+   * \param[in] ratio the overlap ratio [0..1]
+   */
+  inline void
+  setMaxRatio(double ratio)
+  {
+    max_ratio_ = ratio;
+  }
+
+  /** brief get the maximum overlap ratio
+   */
+  inline double
+  getMaxRatio() const
+  {
+    return max_ratio_;
+  }
+
+protected:
+  /** \brief Apply the rejection algorithm.
+   * \param[out] correspondences the set of resultant correspondences.
+   */
+  inline void
+  applyRejection(pcl::Correspondences& correspondences) override
   {
-    /**
-      * @b CorrespondenceRejectoVarTrimmed implements a simple correspondence
-      * rejection method by considering as inliers a certain percentage of correspondences 
-      * with the least distances. The percentage of inliers is computed internally as mentioned
-      * in the paper 'Outlier Robust ICP for minimizing Fractional RMSD, J. M. Philips et al'
-      *
-      * \note If \ref setInputCloud and \ref setInputTarget are given, then the
-      * distances between correspondences will be estimated using the given XYZ
-      * data, and not read from the set of input correspondences.
-      *
-      * \author Aravindhan K Krishnan. This code is ported from libpointmatcher (https://github.com/ethz-asl/libpointmatcher)
-      * \ingroup registration
-      */
-    class PCL_EXPORTS CorrespondenceRejectorVarTrimmed: public CorrespondenceRejector
-    {
-      using CorrespondenceRejector::input_correspondences_;
-      using CorrespondenceRejector::rejection_name_;
-      using CorrespondenceRejector::getClassName;
-
-      public:
-        using Ptr = shared_ptr<CorrespondenceRejectorVarTrimmed>;
-        using ConstPtr = shared_ptr<const CorrespondenceRejectorVarTrimmed>;
-
-        /** \brief Empty constructor. */
-        CorrespondenceRejectorVarTrimmed () : 
-          trimmed_distance_ (0), 
-          factor_ (),
-          min_ratio_ (0.05),
-          max_ratio_ (0.95),
-          lambda_ (0.95)
-        {
-          rejection_name_ = "CorrespondenceRejectorVarTrimmed";
-        }
-
-        /** \brief Get a list of valid correspondences after rejection from the original set of correspondences.
-          * \param[in] original_correspondences the set of initial correspondences given
-          * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences
-          */
-        void 
-        getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, 
-                                     pcl::Correspondences& remaining_correspondences) override;
-
-        /** \brief Get the trimmed distance used for thresholding in correspondence rejection. */
-        inline double
-        getTrimmedDistance () const { return trimmed_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 <typename PointT> inline void 
-        setInputSource (const typename pcl::PointCloud<PointT>::ConstPtr &cloud)
-        {
-          if (!data_container_)
-            data_container_.reset (new DataContainer<PointT>);
-          static_pointer_cast<DataContainer<PointT> > (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 <typename PointT>
-        PCL_DEPRECATED(1, 12, "pcl::registration::CorrespondenceRejectorVarTrimmed::setInputCloud is deprecated. Please use setInputSource instead")
-        inline void 
-        setInputCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud)
-        {
-          if (!data_container_)
-            data_container_.reset (new DataContainer<PointT>);
-          static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (cloud);
-        }
-
-        /** \brief Provide a target point cloud dataset (must contain XYZ
-          * data!), used to compute the correspondence distance.  
-          * \param[in] target a cloud containing XYZ data
-          */
-        template <typename PointT> inline void 
-        setInputTarget (const typename pcl::PointCloud<PointT>::ConstPtr &target)
-        {
-          if (!data_container_)
-            data_container_.reset (new DataContainer<PointT>);
-          static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputTarget (target);
-        }
-
-
-        
-        /** \brief See if this rejector requires source points */
-        bool
-        requiresSourcePoints () const override
-        { return (true); }
-
-        /** \brief Blob method for setting the source cloud */
-        void
-        setSourcePoints (pcl::PCLPointCloud2::ConstPtr cloud2) override
-        { 
-          PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
-          fromPCLPointCloud2 (*cloud2, *cloud);
-          setInputSource<PointXYZ> (cloud);
-        }
-        
-        /** \brief See if this rejector requires a target cloud */
-        bool
-        requiresTargetPoints () const override
-        { return (true); }
-
-        /** \brief Method for setting the target cloud */
-        void
-        setTargetPoints (pcl::PCLPointCloud2::ConstPtr cloud2) override
-        { 
-          PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
-          fromPCLPointCloud2 (*cloud2, *cloud);
-          setInputTarget<PointXYZ> (cloud);
-        }
-
-        /** \brief Provide a pointer to the search object used to find correspondences in
-          * the target cloud.
-          * \param[in] tree a pointer to the spatial search object.
-          * \param[in] force_no_recompute If set to true, this tree will NEVER be 
-          * recomputed, regardless of calls to setInputTarget. Only use if you are 
-          * confident that the tree will be set correctly.
-          */
-        template <typename PointT> inline void
-        setSearchMethodTarget (const typename pcl::search::KdTree<PointT>::Ptr &tree,
-                               bool force_no_recompute = false)
-        { 
-          static_pointer_cast< DataContainer<PointT> > 
-            (data_container_)->setSearchMethodTarget (tree, force_no_recompute );
-        }
-
-        /** \brief Get the computed inlier ratio used for thresholding in correspondence rejection. */
-        inline double
-        getTrimFactor () const { return factor_; }
-
-        /** brief set the minimum overlap ratio
-          * \param[in] ratio the overlap ratio [0..1]
-          */
-        inline void
-        setMinRatio (double ratio) { min_ratio_ = ratio; }
-
-        /** brief get the minimum overlap ratio
-          */
-        inline double
-        getMinRatio () const { return min_ratio_; }
-
-        /** brief set the maximum overlap ratio
-          * \param[in] ratio the overlap ratio [0..1]
-          */
-        inline void
-        setMaxRatio (double ratio) { max_ratio_ = ratio; }
-
-        /** brief get the maximum overlap ratio
-          */
-        inline double
-        getMaxRatio () const { return max_ratio_; }
-
-      protected:
-
-        /** \brief Apply the rejection algorithm.
-          * \param[out] correspondences the set of resultant correspondences.
-          */
-        inline void 
-        applyRejection (pcl::Correspondences &correspondences) override
-        {
-          getRemainingCorrespondences (*input_correspondences_, correspondences);
-        }
-
-        /** \brief The inlier distance threshold (based on the computed trim factor) between two correspondent points in source <-> target.
-          */
-        double trimmed_distance_;
-
-        /** \brief The factor for correspondence rejection. Only factor times the total points sorted based on 
-         *  the correspondence distances will be considered as inliers. Remaining points are rejected. This factor is
-         *  computed internally 
-         */
-        double factor_;
-
-        /** \brief The minimum overlap ratio between the input and target clouds
-         */
-        double min_ratio_;
-
-        /** \brief The maximum overlap ratio between the input and target clouds
-         */
-        double max_ratio_;
-
-       /** \brief part of the term that balances the root mean square difference. This is an internal parameter
-         */
-        double lambda_;
-
-        using DataContainerPtr = DataContainerInterface::Ptr;
-
-        /** \brief A pointer to the DataContainer object containing the input and target point clouds */
-        DataContainerPtr data_container_;
-
-      private:
-
-        /** \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 <double> &dists) const;
-    };
+    getRemainingCorrespondences(*input_correspondences_, correspondences);
   }
-}
 
-#include <pcl/registration/impl/correspondence_rejection_var_trimmed.hpp>
+  /** \brief The inlier distance threshold (based on the computed trim factor) between
+   * two correspondent points in source <-> target.
+   */
+  double trimmed_distance_;
+
+  /** \brief The factor for correspondence rejection. Only factor times the total points
+   * sorted based on the correspondence distances will be considered as inliers.
+   * Remaining points are rejected. This factor is computed internally
+   */
+  double factor_;
+
+  /** \brief The minimum overlap ratio between the input and target clouds
+   */
+  double min_ratio_;
+
+  /** \brief The maximum overlap ratio between the input and target clouds
+   */
+  double max_ratio_;
+
+  /** \brief part of the term that balances the root mean square difference. This is an
+   * internal parameter
+   */
+  double lambda_;
+
+  using DataContainerPtr = DataContainerInterface::Ptr;
+
+  /** \brief A pointer to the DataContainer object containing the input and target point
+   * clouds */
+  DataContainerPtr data_container_;
+
+private:
+  /** \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<double>& dists) const;
+};
+} // namespace registration
+} // namespace pcl
diff --git a/registration/include/pcl/registration/correspondence_sorting.h b/registration/include/pcl/registration/correspondence_sorting.h
index fcb724fa..1044fbfa 100644
--- a/registration/include/pcl/registration/correspondence_sorting.h
+++ b/registration/include/pcl/registration/correspondence_sorting.h
@@ -41,102 +41,91 @@
 #pragma once
 
 #if defined __GNUC__
-#  pragma GCC system_header
+#pragma GCC system_header
 #endif
 
 #include <pcl/registration/correspondence_types.h>
 
-namespace pcl
-{
-  namespace registration
+namespace pcl {
+namespace registration {
+/** @b sortCorrespondencesByQueryIndex : a functor for sorting correspondences by query
+ * index \author Dirk Holz \ingroup registration
+ */
+struct sortCorrespondencesByQueryIndex {
+  using first_argument_type = pcl::Correspondence;
+  using second_argument_type = pcl::Correspondence;
+  using result_type = bool;
+  bool
+  operator()(pcl::Correspondence a, pcl::Correspondence b)
   {
-    /** @b sortCorrespondencesByQueryIndex : a functor for sorting correspondences by query index
-      * \author Dirk Holz
-      * \ingroup registration
-      */
-    struct sortCorrespondencesByQueryIndex
-    {
-      using first_argument_type = pcl::Correspondence;
-      using second_argument_type = pcl::Correspondence;
-      using result_type = bool;
-      bool
-      operator()( pcl::Correspondence a, pcl::Correspondence b)
-      {
-        return (a.index_query < b.index_query);
-      }
-    };
-
-    /** @b sortCorrespondencesByMatchIndex : a functor for sorting correspondences by match index
-      * \author Dirk Holz
-      * \ingroup registration
-      */
-    struct sortCorrespondencesByMatchIndex
-    {
-      using first_argument_type = pcl::Correspondence;
-      using second_argument_type = pcl::Correspondence;
-      using result_type = bool;
-      bool 
-      operator()( pcl::Correspondence a, pcl::Correspondence b)
-      {
-        return (a.index_match < b.index_match);
-      }
-    };
+    return (a.index_query < b.index_query);
+  }
+};
 
-    /** @b sortCorrespondencesByDistance : a functor for sorting correspondences by distance
-      * \author Dirk Holz
-      * \ingroup registration
-      */
-    struct sortCorrespondencesByDistance
-    {
-      using first_argument_type = pcl::Correspondence;
-      using second_argument_type = pcl::Correspondence;
-      using result_type = bool;
-      bool 
-      operator()( pcl::Correspondence a, pcl::Correspondence b)
-      {
-        return (a.distance < b.distance);
-      }
-    };
+/** @b sortCorrespondencesByMatchIndex : a functor for sorting correspondences by match
+ * index \author Dirk Holz \ingroup registration
+ */
+struct sortCorrespondencesByMatchIndex {
+  using first_argument_type = pcl::Correspondence;
+  using second_argument_type = pcl::Correspondence;
+  using result_type = bool;
+  bool
+  operator()(pcl::Correspondence a, pcl::Correspondence b)
+  {
+    return (a.index_match < b.index_match);
+  }
+};
 
-    /** @b sortCorrespondencesByQueryIndexAndDistance : a functor for sorting correspondences by query index _and_ distance
-      * \author Dirk Holz
-      * \ingroup registration
-      */
-    struct sortCorrespondencesByQueryIndexAndDistance
-    {
-      using first_argument_type = pcl::Correspondence;
-      using second_argument_type = pcl::Correspondence;
-      using result_type = bool;
-      inline bool 
-      operator()( pcl::Correspondence a, pcl::Correspondence b)
-      {
-        if (a.index_query < b.index_query)
-          return (true);
-        else if ( (a.index_query == b.index_query) && (a.distance < b.distance) )
-          return (true);
-        return (false);
-      }
-    };
+/** @b sortCorrespondencesByDistance : a functor for sorting correspondences by distance
+ * \author Dirk Holz
+ * \ingroup registration
+ */
+struct sortCorrespondencesByDistance {
+  using first_argument_type = pcl::Correspondence;
+  using second_argument_type = pcl::Correspondence;
+  using result_type = bool;
+  bool
+  operator()(pcl::Correspondence a, pcl::Correspondence b)
+  {
+    return (a.distance < b.distance);
+  }
+};
 
-    /** @b sortCorrespondencesByMatchIndexAndDistance : a functor for sorting correspondences by match index _and_ distance
-      * \author Dirk Holz
-      * \ingroup registration
-      */
-    struct sortCorrespondencesByMatchIndexAndDistance
-    {
-      using first_argument_type = pcl::Correspondence;
-      using second_argument_type = pcl::Correspondence;
-      using result_type = bool;
-      inline bool 
-      operator()( pcl::Correspondence a, pcl::Correspondence b)
-      {
-        if (a.index_match < b.index_match)
-          return (true);
-        else if ( (a.index_match == b.index_match) && (a.distance < b.distance) )
-          return (true);
-        return (false);
-      }
-    };
+/** @b sortCorrespondencesByQueryIndexAndDistance : a functor for sorting
+ * correspondences by query index _and_ distance \author Dirk Holz \ingroup registration
+ */
+struct sortCorrespondencesByQueryIndexAndDistance {
+  using first_argument_type = pcl::Correspondence;
+  using second_argument_type = pcl::Correspondence;
+  using result_type = bool;
+  inline bool
+  operator()(pcl::Correspondence a, pcl::Correspondence b)
+  {
+    if (a.index_query < b.index_query)
+      return (true);
+    else if ((a.index_query == b.index_query) && (a.distance < b.distance))
+      return (true);
+    return (false);
+  }
+};
 
+/** @b sortCorrespondencesByMatchIndexAndDistance : a functor for sorting
+ * correspondences by match index _and_ distance \author Dirk Holz \ingroup registration
+ */
+struct sortCorrespondencesByMatchIndexAndDistance {
+  using first_argument_type = pcl::Correspondence;
+  using second_argument_type = pcl::Correspondence;
+  using result_type = bool;
+  inline bool
+  operator()(pcl::Correspondence a, pcl::Correspondence b)
+  {
+    if (a.index_match < b.index_match)
+      return (true);
+    else if ((a.index_match == b.index_match) && (a.distance < b.distance))
+      return (true);
+    return (false);
   }
-}
+};
+
+} // namespace registration
+} // namespace pcl
diff --git a/registration/include/pcl/registration/correspondence_types.h b/registration/include/pcl/registration/correspondence_types.h
index 7a3ad204..895a23e3 100644
--- a/registration/include/pcl/registration/correspondence_types.h
+++ b/registration/include/pcl/registration/correspondence_types.h
@@ -42,36 +42,36 @@
 
 #include <pcl/correspondence.h>
 
-namespace pcl
-{
-  namespace registration
-  {
-    /** \brief calculates the mean and standard deviation of descriptor distances from correspondences
-      * \param[in] correspondences list of correspondences
-      * \param[out] mean the mean descriptor distance of correspondences
-      * \param[out] stddev the standard deviation of descriptor distances.
-      * \note The sample variance is used to determine the standard deviation
-      */
-    inline void 
-    getCorDistMeanStd (const pcl::Correspondences& correspondences, double &mean, double &stddev);
+namespace pcl {
+namespace registration {
+/** \brief calculates the mean and standard deviation of descriptor distances from
+ * correspondences \param[in] correspondences list of correspondences \param[out] mean
+ * the mean descriptor distance of correspondences \param[out] stddev the standard
+ * deviation of descriptor distances. \note The sample variance is used to determine the
+ * standard deviation
+ */
+inline void
+getCorDistMeanStd(const pcl::Correspondences& correspondences,
+                  double& mean,
+                  double& stddev);
 
-    /** \brief extracts the query indices
-      * \param[in] correspondences list of correspondences
-      * \param[out] indices array of extracted indices.
-      * \note order of indices corresponds to input list of descriptor correspondences
-      */
-    inline void 
-    getQueryIndices (const pcl::Correspondences& correspondences, std::vector<int>& indices);
+/** \brief extracts the query indices
+ * \param[in] correspondences list of correspondences
+ * \param[out] indices array of extracted indices.
+ * \note order of indices corresponds to input list of descriptor correspondences
+ */
+inline void
+getQueryIndices(const pcl::Correspondences& correspondences, pcl::Indices& indices);
 
-    /** \brief extracts the match indices
-      * \param[in] correspondences list of correspondences
-      * \param[out] indices array of extracted indices.
-      * \note order of indices corresponds to input list of descriptor correspondences
-      */
-    inline void 
-    getMatchIndices (const pcl::Correspondences& correspondences, std::vector<int>& indices);
+/** \brief extracts the match indices
+ * \param[in] correspondences list of correspondences
+ * \param[out] indices array of extracted indices.
+ * \note order of indices corresponds to input list of descriptor correspondences
+ */
+inline void
+getMatchIndices(const pcl::Correspondences& correspondences, pcl::Indices& indices);
 
-  }
-}
+} // namespace registration
+} // namespace pcl
 
 #include <pcl/registration/impl/correspondence_types.hpp>
diff --git a/registration/include/pcl/registration/default_convergence_criteria.h b/registration/include/pcl/registration/default_convergence_criteria.h
index aa02d272..ab9baea2 100644
--- a/registration/include/pcl/registration/default_convergence_criteria.h
+++ b/registration/include/pcl/registration/default_convergence_criteria.h
@@ -39,242 +39,302 @@
 
 #pragma once
 
+#include <pcl/registration/convergence_criteria.h>
+#include <pcl/correspondence.h>
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
-#include <pcl/registration/eigen.h>
-#include <pcl/correspondence.h>
-#include <pcl/registration/convergence_criteria.h>
 
-namespace pcl
-{
-  namespace registration
+namespace pcl {
+namespace registration {
+/** \brief @b DefaultConvergenceCriteria represents an instantiation of
+ * ConvergenceCriteria, and implements the following criteria for registration loop
+ * evaluation:
+ *
+ *  * a maximum number of iterations has been reached
+ *  * the transformation (R, t) cannot be further updated (the difference between
+ * current and previous is smaller than a threshold)
+ *  * the Mean Squared Error (MSE) between the current set of correspondences and the
+ * previous one is smaller than some threshold (both relative and absolute tests)
+ *
+ * \note Convergence is considered reached if ANY of the above criteria are met.
+ *
+ * \author Radu B. Rusu
+ * \ingroup registration
+ */
+template <typename Scalar = float>
+class DefaultConvergenceCriteria : public ConvergenceCriteria {
+public:
+  using Ptr = shared_ptr<DefaultConvergenceCriteria<Scalar>>;
+  using ConstPtr = shared_ptr<const DefaultConvergenceCriteria<Scalar>>;
+
+  using Matrix4 = Eigen::Matrix<Scalar, 4, 4>;
+
+  enum ConvergenceState {
+    CONVERGENCE_CRITERIA_NOT_CONVERGED,
+    CONVERGENCE_CRITERIA_ITERATIONS,
+    CONVERGENCE_CRITERIA_TRANSFORM,
+    CONVERGENCE_CRITERIA_ABS_MSE,
+    CONVERGENCE_CRITERIA_REL_MSE,
+    CONVERGENCE_CRITERIA_NO_CORRESPONDENCES,
+    CONVERGENCE_CRITERIA_FAILURE_AFTER_MAX_ITERATIONS
+  };
+
+  /** \brief Empty constructor.
+   * Sets:
+   *  * the maximum number of iterations to 1000
+   *  * the rotation threshold to 0.256 degrees (0.99999)
+   *  * the translation threshold to 0.0003 meters (3e-4^2)
+   *  * the MSE relative / absolute thresholds to 0.001% and 1e-12
+   *
+   * \param[in] iterations a reference to the number of iterations the loop has ran so
+   * far \param[in] transform a reference to the current transformation obtained by the
+   * transformation evaluation \param[in] correspondences a reference to the current set
+   * of point correspondences between source and target
+   */
+  DefaultConvergenceCriteria(const int& iterations,
+                             const Matrix4& transform,
+                             const pcl::Correspondences& correspondences)
+  : iterations_(iterations)
+  , transformation_(transform)
+  , correspondences_(correspondences)
+  , correspondences_prev_mse_(std::numeric_limits<double>::max())
+  , correspondences_cur_mse_(std::numeric_limits<double>::max())
+  , max_iterations_(100) // 100 iterations
+  , failure_after_max_iter_(false)
+  , rotation_threshold_(0.99999)        // 0.256 degrees
+  , translation_threshold_(3e-4 * 3e-4) // 0.0003 meters
+  , mse_threshold_relative_(0.00001)    // 0.001% of the previous MSE (relative error)
+  , mse_threshold_absolute_(1e-12)      // MSE (absolute error)
+  , iterations_similar_transforms_(0)
+  , max_iterations_similar_transforms_(0)
+  , convergence_state_(CONVERGENCE_CRITERIA_NOT_CONVERGED)
+  {}
+
+  /** \brief Empty destructor */
+  ~DefaultConvergenceCriteria() {}
+
+  /** \brief Set the maximum number of consecutive iterations that the internal
+   * rotation, translation, and MSE differences are allowed to be similar. \param[in]
+   * nr_iterations the maximum number of iterations
+   */
+  inline void
+  setMaximumIterationsSimilarTransforms(const int nr_iterations)
+  {
+    max_iterations_similar_transforms_ = nr_iterations;
+  }
+
+  /** \brief Get the maximum number of consecutive iterations that the internal
+   * rotation, translation, and MSE differences are allowed to be similar, as set by the
+   * user.
+   */
+  inline int
+  getMaximumIterationsSimilarTransforms() const
+  {
+    return (max_iterations_similar_transforms_);
+  }
+
+  /** \brief Set the maximum number of iterations the internal optimization should run
+   * for. \param[in] nr_iterations the maximum number of iterations the internal
+   * optimization should run for
+   */
+  inline void
+  setMaximumIterations(const int nr_iterations)
+  {
+    max_iterations_ = nr_iterations;
+  }
+
+  /** \brief Get the maximum number of iterations the internal optimization should run
+   * for, as set by the user. */
+  inline int
+  getMaximumIterations() const
+  {
+    return (max_iterations_);
+  }
+
+  /** \brief Specifies if the registration fails or converges when the maximum number of
+   * iterations is reached. \param[in] failure_after_max_iter If true, the registration
+   * fails. If false, the registration is assumed to have converged.
+   */
+  inline void
+  setFailureAfterMaximumIterations(const bool failure_after_max_iter)
+  {
+    failure_after_max_iter_ = failure_after_max_iter;
+  }
+
+  /** \brief Get whether the registration will fail or converge when the maximum number
+   * of iterations is reached. */
+  inline bool
+  getFailureAfterMaximumIterations() const
+  {
+    return (failure_after_max_iter_);
+  }
+
+  /** \brief Set the rotation threshold cosine angle (maximum allowable difference
+   * between two consecutive transformations) in order for an optimization to be
+   * considered as having converged to the final solution. \param[in] threshold the
+   * rotation threshold in order for an optimization to be considered as having
+   * converged to the final solution.
+   */
+  inline void
+  setRotationThreshold(const double threshold)
+  {
+    rotation_threshold_ = threshold;
+  }
+
+  /** \brief Get the rotation threshold cosine angle (maximum allowable difference
+   * between two consecutive transformations) as set by the user.
+   */
+  inline double
+  getRotationThreshold() const
+  {
+    return (rotation_threshold_);
+  }
+
+  /** \brief Set the translation threshold (maximum allowable difference between two
+   * consecutive transformations) in order for an optimization to be considered as
+   * having converged to the final solution. \param[in] threshold the translation
+   * threshold in order for an optimization to be considered as having converged to the
+   * final solution.
+   */
+  inline void
+  setTranslationThreshold(const double threshold)
+  {
+    translation_threshold_ = threshold;
+  }
+
+  /** \brief Get the rotation threshold cosine angle (maximum allowable difference
+   * between two consecutive transformations) as set by the user.
+   */
+  inline double
+  getTranslationThreshold() const
+  {
+    return (translation_threshold_);
+  }
+
+  /** \brief Set the relative MSE between two consecutive sets of correspondences.
+   * \param[in] mse_relative the relative MSE threshold
+   */
+  inline void
+  setRelativeMSE(const double mse_relative)
+  {
+    mse_threshold_relative_ = mse_relative;
+  }
+
+  /** \brief Get the relative MSE between two consecutive sets of correspondences. */
+  inline double
+  getRelativeMSE() const
+  {
+    return (mse_threshold_relative_);
+  }
+
+  /** \brief Set the absolute MSE between two consecutive sets of correspondences.
+   * \param[in] mse_absolute the relative MSE threshold
+   */
+  inline void
+  setAbsoluteMSE(const double mse_absolute)
+  {
+    mse_threshold_absolute_ = mse_absolute;
+  }
+
+  /** \brief Get the absolute MSE between two consecutive sets of correspondences. */
+  inline double
+  getAbsoluteMSE() const
+  {
+    return (mse_threshold_absolute_);
+  }
+
+  /** \brief Check if convergence has been reached. */
+  bool
+  hasConverged() override;
+
+  /** \brief Return the convergence state after hasConverged () */
+  ConvergenceState
+  getConvergenceState()
+  {
+    return (convergence_state_);
+  }
+
+  /** \brief Sets the convergence state externally (for example, when ICP does not find
+   * enough correspondences to estimate a transformation, the function is called setting
+   * the convergence state to ConvergenceState::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES)
+   * \param[in] c the convergence state
+   */
+  inline void
+  setConvergenceState(ConvergenceState c)
   {
-    /** \brief @b DefaultConvergenceCriteria represents an instantiation of
-      * ConvergenceCriteria, and implements the following criteria for registration loop
-      * evaluation:
-      *
-      *  * a maximum number of iterations has been reached
-      *  * the transformation (R, t) cannot be further updated (the difference between current and previous is smaller than a threshold)
-      *  * the Mean Squared Error (MSE) between the current set of correspondences and the previous one is smaller than some threshold (both relative and absolute tests)
-      *
-      * \note Convergence is considered reached if ANY of the above criteria are met.
-      *
-      * \author Radu B. Rusu
-      * \ingroup registration
-      */
-    template <typename Scalar = float>
-    class DefaultConvergenceCriteria : public ConvergenceCriteria
-    {
-      public:
-        using Ptr = shared_ptr<DefaultConvergenceCriteria<Scalar> >;
-        using ConstPtr = shared_ptr<const DefaultConvergenceCriteria<Scalar> >;
-
-        using Matrix4 = Eigen::Matrix<Scalar, 4, 4>;
-
-        enum ConvergenceState
-        {
-          CONVERGENCE_CRITERIA_NOT_CONVERGED,
-          CONVERGENCE_CRITERIA_ITERATIONS,
-          CONVERGENCE_CRITERIA_TRANSFORM,
-          CONVERGENCE_CRITERIA_ABS_MSE,
-          CONVERGENCE_CRITERIA_REL_MSE,
-          CONVERGENCE_CRITERIA_NO_CORRESPONDENCES,
-          CONVERGENCE_CRITERIA_FAILURE_AFTER_MAX_ITERATIONS
-        };
-
-        /** \brief Empty constructor.
-          * Sets:
-          *  * the maximum number of iterations to 1000
-          *  * the rotation threshold to 0.256 degrees (0.99999)
-          *  * the translation threshold to 0.0003 meters (3e-4^2)
-          *  * the MSE relative / absolute thresholds to 0.001% and 1e-12
-          *
-          * \param[in] iterations a reference to the number of iterations the loop has ran so far
-          * \param[in] transform a reference to the current transformation obtained by the transformation evaluation
-          * \param[in] correspondences a reference to the current set of point correspondences between source and target
-          */
-        DefaultConvergenceCriteria (const int &iterations, const Matrix4 &transform, const pcl::Correspondences &correspondences)
-          : iterations_ (iterations)
-          , transformation_ (transform)
-          , correspondences_ (correspondences)
-          , correspondences_prev_mse_ (std::numeric_limits<double>::max ())
-          , correspondences_cur_mse_ (std::numeric_limits<double>::max ())
-          , max_iterations_ (100)                 // 100 iterations
-          , failure_after_max_iter_ (false)
-          , rotation_threshold_ (0.99999)         // 0.256 degrees
-          , translation_threshold_ (3e-4 * 3e-4)  // 0.0003 meters
-          , mse_threshold_relative_ (0.00001)     // 0.001% of the previous MSE (relative error)
-          , mse_threshold_absolute_ (1e-12)       // MSE (absolute error)
-          , iterations_similar_transforms_ (0)
-          , max_iterations_similar_transforms_ (0)
-          , convergence_state_ (CONVERGENCE_CRITERIA_NOT_CONVERGED)
-        {
-        }
-      
-        /** \brief Empty destructor */
-        ~DefaultConvergenceCriteria () {}
-
-        /** \brief Set the maximum number of consecutive iterations that the internal rotation, 
-          * translation, and MSE differences are allowed to be similar. 
-          * \param[in] nr_iterations the maximum number of iterations 
-          */
-        inline void
-        setMaximumIterationsSimilarTransforms (const int nr_iterations) { max_iterations_similar_transforms_ = nr_iterations; }
-
-        /** \brief Get the maximum number of consecutive iterations that the internal rotation, 
-          * translation, and MSE differences are allowed to be similar, as set by the user.
-          */
-        inline int
-        getMaximumIterationsSimilarTransforms () const { return (max_iterations_similar_transforms_); }
-
-        /** \brief Set the maximum number of iterations the internal optimization should run for.
-          * \param[in] nr_iterations the maximum number of iterations the internal optimization should run for
-          */
-        inline void
-        setMaximumIterations (const int nr_iterations) { max_iterations_ = nr_iterations; }
-
-        /** \brief Get the maximum number of iterations the internal optimization should run for, as set by the user. */
-        inline int
-        getMaximumIterations () const { return (max_iterations_); }
-
-        /** \brief Specifies if the registration fails or converges when the maximum number of iterations is reached.
-          * \param[in] failure_after_max_iter If true, the registration fails. If false, the registration is assumed to have converged.
-          */
-        inline void
-        setFailureAfterMaximumIterations (const bool failure_after_max_iter) { failure_after_max_iter_ = failure_after_max_iter; }
-
-        /** \brief Get whether the registration will fail or converge when the maximum number of iterations is reached. */
-        inline bool
-        getFailureAfterMaximumIterations () const { return (failure_after_max_iter_); }
-
-        /** \brief Set the rotation threshold cosine angle (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.
-          * \param[in] threshold the rotation threshold in order for an optimization to be considered as having converged to the final solution.
-          */
-        inline void
-        setRotationThreshold (const double threshold) { rotation_threshold_ = threshold; }
-
-        /** \brief Get the rotation threshold cosine angle (maximum allowable difference between two consecutive transformations) as set by the user.
-          */
-        inline double
-        getRotationThreshold () const { return (rotation_threshold_); }
-
-        /** \brief Set the translation threshold (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.
-          * \param[in] threshold the translation threshold in order for an optimization to be considered as having converged to the final solution.
-          */
-        inline void
-        setTranslationThreshold (const double threshold) { translation_threshold_ = threshold; }
-
-        /** \brief Get the rotation threshold cosine angle (maximum allowable difference between two consecutive transformations) as set by the user.
-          */
-        inline double
-        getTranslationThreshold () const { return (translation_threshold_); }
-
-        /** \brief Set the relative MSE between two consecutive sets of correspondences.
-          * \param[in] mse_relative the relative MSE threshold
-          */
-        inline void
-        setRelativeMSE (const double mse_relative) { mse_threshold_relative_ = mse_relative; }
-
-        /** \brief Get the relative MSE between two consecutive sets of correspondences. */
-        inline double
-        getRelativeMSE () const { return (mse_threshold_relative_); }
-
-        /** \brief Set the absolute MSE between two consecutive sets of correspondences.
-          * \param[in] mse_absolute the relative MSE threshold
-          */
-        inline void
-        setAbsoluteMSE (const double mse_absolute) { mse_threshold_absolute_ = mse_absolute; }
-
-        /** \brief Get the absolute MSE between two consecutive sets of correspondences. */
-        inline double
-        getAbsoluteMSE () const { return (mse_threshold_absolute_); }
-
-
-        /** \brief Check if convergence has been reached. */
-        bool
-        hasConverged () override;
-
-        /** \brief Return the convergence state after hasConverged () */
-        ConvergenceState
-        getConvergenceState ()
-        {
-          return (convergence_state_);
-        }
-
-        /** \brief Sets the convergence state externally (for example, when ICP does not find
-         * enough correspondences to estimate a transformation, the function is called setting
-         * the convergence state to ConvergenceState::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES)
-         * \param[in] c the convergence state
-         */
-        inline void
-        setConvergenceState(ConvergenceState c)
-        {
-          convergence_state_ = c;
-        }
-
-      protected:
-
-        /** \brief Calculate the mean squared error (MSE) of the distance for a given set of correspondences.
-          * \param[in] correspondences the given set of correspondences
-          */
-        inline double
-        calculateMSE (const pcl::Correspondences &correspondences) const
-        {
-          double mse = 0;
-          for (const auto &correspondence : correspondences)
-            mse += correspondence.distance;
-          mse /= double (correspondences.size ());
-          return (mse);
-        }
-
-        /** \brief The number of iterations done by the registration loop so far. */
-        const int &iterations_;
-
-        /** \brief The current transformation obtained by the transformation estimation method. */
-        const Matrix4 &transformation_;
-
-        /** \brief The current set of point correspondences between the source and the target. */
-        const pcl::Correspondences &correspondences_;
-
-        /** \brief The MSE for the previous set of correspondences. */
-        double correspondences_prev_mse_;
-
-        /** \brief The MSE for the current set of correspondences. */
-        double correspondences_cur_mse_;
-
-        /** \brief The maximum nuyyGmber of iterations that the registration loop is to be executed. */
-        int max_iterations_;
-
-        /** \brief Specifys if the registration fails or converges when the maximum number of iterations is reached. */
-        bool failure_after_max_iter_;
-
-        /** \brief The rotation threshold is the relative rotation between two iterations (as angle cosine). */
-        double rotation_threshold_;
-
-        /** \brief The translation threshold is the relative translation between two iterations (0 if no translation). */
-        double translation_threshold_;
-
-        /** \brief The relative change from the previous MSE for the current set of correspondences, e.g. .1 means 10% change. */
-        double mse_threshold_relative_;
-
-        /** \brief The absolute change from the previous MSE for the current set of correspondences. */
-        double mse_threshold_absolute_;
-
-        /** \brief Internal counter for the number of iterations that the internal 
-          * rotation, translation, and MSE differences are allowed to be similar. */
-        int iterations_similar_transforms_;
-
-        /** \brief The maximum number of iterations that the internal rotation, 
-          * translation, and MSE differences are allowed to be similar. */
-        int max_iterations_similar_transforms_;
-
-        /** \brief The state of the convergence (e.g., why did the registration converge). */
-        ConvergenceState convergence_state_;
-
-      public:
-        PCL_MAKE_ALIGNED_OPERATOR_NEW
-     };
+    convergence_state_ = c;
   }
-}
+
+protected:
+  /** \brief Calculate the mean squared error (MSE) of the distance for a given set of
+   * correspondences. \param[in] correspondences the given set of correspondences
+   */
+  inline double
+  calculateMSE(const pcl::Correspondences& correspondences) const
+  {
+    double mse = 0;
+    for (const auto& correspondence : correspondences)
+      mse += correspondence.distance;
+    mse /= double(correspondences.size());
+    return (mse);
+  }
+
+  /** \brief The number of iterations done by the registration loop so far. */
+  const int& iterations_;
+
+  /** \brief The current transformation obtained by the transformation estimation
+   * method. */
+  const Matrix4& transformation_;
+
+  /** \brief The current set of point correspondences between the source and the target.
+   */
+  const pcl::Correspondences& correspondences_;
+
+  /** \brief The MSE for the previous set of correspondences. */
+  double correspondences_prev_mse_;
+
+  /** \brief The MSE for the current set of correspondences. */
+  double correspondences_cur_mse_;
+
+  /** \brief The maximum nuyyGmber of iterations that the registration loop is to be
+   * executed. */
+  int max_iterations_;
+
+  /** \brief Specifys if the registration fails or converges when the maximum number of
+   * iterations is reached. */
+  bool failure_after_max_iter_;
+
+  /** \brief The rotation threshold is the relative rotation between two iterations (as
+   * angle cosine). */
+  double rotation_threshold_;
+
+  /** \brief The translation threshold is the relative translation between two
+   * iterations (0 if no translation). */
+  double translation_threshold_;
+
+  /** \brief The relative change from the previous MSE for the current set of
+   * correspondences, e.g. .1 means 10% change. */
+  double mse_threshold_relative_;
+
+  /** \brief The absolute change from the previous MSE for the current set of
+   * correspondences. */
+  double mse_threshold_absolute_;
+
+  /** \brief Internal counter for the number of iterations that the internal
+   * rotation, translation, and MSE differences are allowed to be similar. */
+  int iterations_similar_transforms_;
+
+  /** \brief The maximum number of iterations that the internal rotation,
+   * translation, and MSE differences are allowed to be similar. */
+  int max_iterations_similar_transforms_;
+
+  /** \brief The state of the convergence (e.g., why did the registration converge). */
+  ConvergenceState convergence_state_;
+
+public:
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+};
+} // namespace registration
+} // namespace pcl
 
 #include <pcl/registration/impl/default_convergence_criteria.hpp>
diff --git a/registration/include/pcl/registration/distances.h b/registration/include/pcl/registration/distances.h
index b367c6b1..caa261a3 100644
--- a/registration/include/pcl/registration/distances.h
+++ b/registration/include/pcl/registration/distances.h
@@ -40,104 +40,105 @@
 
 #pragma once
 
-#include <pcl/registration/eigen.h>
+#include <Eigen/Core>
+
+#include <string.h>
+
+#include <algorithm>
 #include <vector>
 
-namespace pcl
+namespace pcl {
+namespace distances {
+
+/** \brief Compute the median value from a set of doubles
+ * \param[in] fvec the set of doubles
+ * \param[in] m the number of doubles in the set
+ */
+inline double
+computeMedian(double* fvec, int m)
 {
-  namespace distances
-  {
+  // Copy the values to vectors for faster sorting
+  std::vector<double> data(m);
+  memcpy(&data[0], fvec, sizeof(double) * m);
 
-    /** \brief Compute the median value from a set of doubles
-      * \param[in] fvec the set of doubles
-      * \param[in] m the number of doubles in the set
-      */
-    inline double 
-    computeMedian (double *fvec, int m)
-    {
-      // Copy the values to vectors for faster sorting
-      std::vector<double> data (m);
-      memcpy (&data[0], fvec, sizeof (double) * m);
-      
-      std::nth_element(data.begin(), data.begin() + (data.size () >> 1), data.end());
-      return (data[data.size () >> 1]);
-    }
+  std::nth_element(data.begin(), data.begin() + (data.size() >> 1), data.end());
+  return (data[data.size() >> 1]);
+}
 
-    /** \brief Use a Huber kernel to estimate the distance between two vectors
-      * \param[in] p_src the first eigen vector
-      * \param[in] p_tgt the second eigen vector
-      * \param[in] sigma the sigma value
-      */
-    inline double
-    huber (const Eigen::Vector4f &p_src, const Eigen::Vector4f &p_tgt, double sigma) 
-    {
-      Eigen::Array4f diff = (p_tgt.array () - p_src.array ()).abs ();
-      double norm = 0.0;
-      for (int i = 0; i < 3; ++i)
-      {
-        if (diff[i] < sigma)
-          norm += diff[i] * diff[i];
-        else
-          norm += 2.0 * sigma * diff[i] - sigma * sigma;
-      }
-      return (norm);
-    }
+/** \brief Use a Huber kernel to estimate the distance between two vectors
+ * \param[in] p_src the first eigen vector
+ * \param[in] p_tgt the second eigen vector
+ * \param[in] sigma the sigma value
+ */
+inline double
+huber(const Eigen::Vector4f& p_src, const Eigen::Vector4f& p_tgt, double sigma)
+{
+  Eigen::Array4f diff = (p_tgt.array() - p_src.array()).abs();
+  double norm = 0.0;
+  for (int i = 0; i < 3; ++i) {
+    if (diff[i] < sigma)
+      norm += diff[i] * diff[i];
+    else
+      norm += 2.0 * sigma * diff[i] - sigma * sigma;
+  }
+  return (norm);
+}
 
-    /** \brief Use a Huber kernel to estimate the distance between two vectors
-      * \param[in] diff the norm difference between two vectors
-      * \param[in] sigma the sigma value
-      */
-    inline double
-    huber (double diff, double sigma) 
-    {
-      double norm = 0.0;
-      if (diff < sigma)
-        norm += diff * diff;
-      else
-        norm += 2.0 * sigma * diff - sigma * sigma;
-      return (norm);
-    }
+/** \brief Use a Huber kernel to estimate the distance between two vectors
+ * \param[in] diff the norm difference between two vectors
+ * \param[in] sigma the sigma value
+ */
+inline double
+huber(double diff, double sigma)
+{
+  double norm = 0.0;
+  if (diff < sigma)
+    norm += diff * diff;
+  else
+    norm += 2.0 * sigma * diff - sigma * sigma;
+  return (norm);
+}
 
-    /** \brief Use a Gedikli kernel to estimate the distance between two vectors
-      * (for more information, see 
-      * \param[in] val the norm difference between two vectors
-      * \param[in] clipping the clipping value
-      * \param[in] slope the slope. Default: 4
-      */
-    inline double
-    gedikli (double val, double clipping, double slope = 4) 
-    {
-      return (1.0 / (1.0 + pow (std::abs(val) / clipping, slope)));
-    }
+/** \brief Use a Gedikli kernel to estimate the distance between two vectors
+ * (for more information, see
+ * \param[in] val the norm difference between two vectors
+ * \param[in] clipping the clipping value
+ * \param[in] slope the slope. Default: 4
+ */
+inline double
+gedikli(double val, double clipping, double slope = 4)
+{
+  return (1.0 / (1.0 + pow(std::abs(val) / clipping, slope)));
+}
 
-    /** \brief Compute the Manhattan distance between two eigen vectors.
-      * \param[in] p_src the first eigen vector
-      * \param[in] p_tgt the second eigen vector
-      */
-    inline double
-    l1 (const Eigen::Vector4f &p_src, const Eigen::Vector4f &p_tgt) 
-    {
-      return ((p_src.array () - p_tgt.array ()).abs ().sum ());
-    }
+/** \brief Compute the Manhattan distance between two eigen vectors.
+ * \param[in] p_src the first eigen vector
+ * \param[in] p_tgt the second eigen vector
+ */
+inline double
+l1(const Eigen::Vector4f& p_src, const Eigen::Vector4f& p_tgt)
+{
+  return ((p_src.array() - p_tgt.array()).abs().sum());
+}
 
-    /** \brief Compute the Euclidean distance between two eigen vectors.
-      * \param[in] p_src the first eigen vector
-      * \param[in] p_tgt the second eigen vector
-      */
-    inline double
-    l2 (const Eigen::Vector4f &p_src, const Eigen::Vector4f &p_tgt) 
-    {
-      return ((p_src - p_tgt).norm ());
-    }
+/** \brief Compute the Euclidean distance between two eigen vectors.
+ * \param[in] p_src the first eigen vector
+ * \param[in] p_tgt the second eigen vector
+ */
+inline double
+l2(const Eigen::Vector4f& p_src, const Eigen::Vector4f& p_tgt)
+{
+  return ((p_src - p_tgt).norm());
+}
 
-    /** \brief Compute the squared Euclidean distance between two eigen vectors.
-      * \param[in] p_src the first eigen vector
-      * \param[in] p_tgt the second eigen vector
-      */
-    inline double
-    l2Sqr (const Eigen::Vector4f &p_src, const Eigen::Vector4f &p_tgt) 
-    {
-      return ((p_src - p_tgt).squaredNorm ());
-    }
-  }
+/** \brief Compute the squared Euclidean distance between two eigen vectors.
+ * \param[in] p_src the first eigen vector
+ * \param[in] p_tgt the second eigen vector
+ */
+inline double
+l2Sqr(const Eigen::Vector4f& p_src, const Eigen::Vector4f& p_tgt)
+{
+  return ((p_src - p_tgt).squaredNorm());
 }
+} // namespace distances
+} // namespace pcl
diff --git a/registration/include/pcl/registration/edge_measurements.h b/registration/include/pcl/registration/edge_measurements.h
index d3667bc7..a14aabe7 100644
--- a/registration/include/pcl/registration/edge_measurements.h
+++ b/registration/include/pcl/registration/edge_measurements.h
@@ -43,31 +43,31 @@
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
 
-namespace pcl
-{
-  namespace registration
-  {
-    /** \brief @b NullMeasurement struct
-      * \author Nicola Fioraio
-      * \ingroup registration
-      */
-    struct NullMeasurement
-    {};
+namespace pcl {
+namespace registration {
+/** \brief @b NullMeasurement struct
+ * \author Nicola Fioraio
+ * \ingroup registration
+ */
+struct NullMeasurement {};
 
-    /** \brief @b PoseMeasurement struct
-      * \author Nicola Fioraio
-      * \ingroup registration
-      */
-    template <typename VertexT, typename InformationT>
-    struct PoseMeasurement
-    {
-      VertexT v_start, v_end;
-      Eigen::Matrix4f relative_transformation;
-      InformationT information_matrix;
-      PCL_MAKE_ALIGNED_OPERATOR_NEW
+/** \brief @b PoseMeasurement struct
+ * \author Nicola Fioraio
+ * \ingroup registration
+ */
+template <typename VertexT, typename InformationT>
+struct PoseMeasurement {
+  VertexT v_start, v_end;
+  Eigen::Matrix4f relative_transformation;
+  InformationT information_matrix;
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
 
-      PoseMeasurement (const VertexT& v_s, const VertexT& v_e, const Eigen::Matrix4f& tr, const InformationT& mtx)
-        : v_start (v_s), v_end (v_e), relative_transformation (tr), information_matrix (mtx) {}
-    };
-  }
-}
+  PoseMeasurement(const VertexT& v_s,
+                  const VertexT& v_e,
+                  const Eigen::Matrix4f& tr,
+                  const InformationT& mtx)
+  : v_start(v_s), v_end(v_e), relative_transformation(tr), information_matrix(mtx)
+  {}
+};
+} // namespace registration
+} // namespace pcl
diff --git a/registration/include/pcl/registration/eigen.h b/registration/include/pcl/registration/eigen.h
index 3f1c831c..5712e8fc 100644
--- a/registration/include/pcl/registration/eigen.h
+++ b/registration/include/pcl/registration/eigen.h
@@ -40,10 +40,11 @@
 #pragma once
 
 #if defined __GNUC__
-#  pragma GCC system_header 
+#pragma GCC system_header
 #endif
+PCL_DEPRECATED_HEADER(1, 15, "Please include the needed eigen headers directly.")
 
 #include <Eigen/Core>
+#include <Eigen/Dense>
 #include <Eigen/Geometry>
 #include <unsupported/Eigen/Polynomials>
-#include <Eigen/Dense>
diff --git a/registration/include/pcl/registration/elch.h b/registration/include/pcl/registration/elch.h
index 08d9ba2d..981b153c 100644
--- a/registration/include/pcl/registration/elch.h
+++ b/registration/include/pcl/registration/elch.h
@@ -40,216 +40,214 @@
 
 #pragma once
 
+#include <pcl/registration/boost_graph.h>
+#include <pcl/registration/icp.h>
+#include <pcl/registration/registration.h>
 #include <pcl/memory.h>
-#include <pcl/pcl_macros.h>
 #include <pcl/pcl_base.h>
-#include <pcl/point_types.h>
+#include <pcl/pcl_macros.h>
 #include <pcl/point_cloud.h>
-#include <pcl/registration/registration.h>
-#include <pcl/registration/boost.h>
-#include <pcl/registration/eigen.h>
-#include <pcl/registration/icp.h>
-#include <pcl/registration/boost_graph.h>
+#include <pcl/point_types.h>
+
+namespace pcl {
+namespace registration {
+/** \brief @b ELCH (Explicit Loop Closing Heuristic) class
+ * \author Jochen Sprickerhof
+ * \ingroup registration
+ */
+template <typename PointT>
+class ELCH : public PCLBase<PointT> {
+public:
+  using Ptr = shared_ptr<ELCH<PointT>>;
+  using ConstPtr = shared_ptr<const ELCH<PointT>>;
+
+  using PointCloud = pcl::PointCloud<PointT>;
+  using PointCloudPtr = typename PointCloud::Ptr;
+  using PointCloudConstPtr = typename PointCloud::ConstPtr;
+
+  struct Vertex {
+    Vertex() : cloud() {}
+    PointCloudPtr cloud;
+    Eigen::Affine3f transform;
+  };
+
+  /** \brief graph structure to hold the SLAM graph */
+  using LoopGraph = boost::adjacency_list<boost::listS,
+                                          boost::eigen_vecS,
+                                          boost::undirectedS,
+                                          Vertex,
+                                          boost::no_property>;
+
+  using LoopGraphPtr = shared_ptr<LoopGraph>;
+
+  using Registration = pcl::Registration<PointT, PointT>;
+  using RegistrationPtr = typename Registration::Ptr;
+  using RegistrationConstPtr = typename Registration::ConstPtr;
+
+  /** \brief Empty constructor. */
+  ELCH()
+  : loop_graph_(new LoopGraph)
+  , loop_start_(0)
+  , loop_end_(0)
+  , reg_(new pcl::IterativeClosestPoint<PointT, PointT>)
+  , compute_loop_(true)
+  , vd_(){};
+
+  /** \brief Empty destructor */
+  ~ELCH() {}
+
+  /** \brief Add a new point cloud to the internal graph.
+   * \param[in] cloud the new point cloud
+   */
+  inline void
+  addPointCloud(PointCloudPtr cloud)
+  {
+    typename boost::graph_traits<LoopGraph>::vertex_descriptor vd =
+        add_vertex(*loop_graph_);
+    (*loop_graph_)[vd].cloud = cloud;
+    if (num_vertices(*loop_graph_) > 1)
+      add_edge(vd_, vd, *loop_graph_);
+    vd_ = vd;
+  }
+
+  /** \brief Getter for the internal graph. */
+  inline LoopGraphPtr
+  getLoopGraph()
+  {
+    return (loop_graph_);
+  }
+
+  /** \brief Setter for a new internal graph.
+   * \param[in] loop_graph the new graph
+   */
+  inline void
+  setLoopGraph(LoopGraphPtr loop_graph)
+  {
+    loop_graph_ = loop_graph;
+  }
+
+  /** \brief Getter for the first scan of a loop. */
+  inline typename boost::graph_traits<LoopGraph>::vertex_descriptor
+  getLoopStart()
+  {
+    return (loop_start_);
+  }
+
+  /** \brief Setter for the first scan of a loop.
+   * \param[in] loop_start the scan that starts the loop
+   */
+  inline void
+  setLoopStart(
+      const typename boost::graph_traits<LoopGraph>::vertex_descriptor& loop_start)
+  {
+    loop_start_ = loop_start;
+  }
+
+  /** \brief Getter for the last scan of a loop. */
+  inline typename boost::graph_traits<LoopGraph>::vertex_descriptor
+  getLoopEnd()
+  {
+    return (loop_end_);
+  }
+
+  /** \brief Setter for the last scan of a loop.
+   * \param[in] loop_end the scan that ends the loop
+   */
+  inline void
+  setLoopEnd(const typename boost::graph_traits<LoopGraph>::vertex_descriptor& loop_end)
+  {
+    loop_end_ = loop_end;
+  }
 
-namespace pcl
-{
-  namespace registration
+  /** \brief Getter for the registration algorithm. */
+  inline RegistrationPtr
+  getReg()
   {
-    /** \brief @b ELCH (Explicit Loop Closing Heuristic) class
-      * \author Jochen Sprickerhof
-      * \ingroup registration
-      */
-    template <typename PointT>
-    class ELCH : public PCLBase<PointT>
-    {
-      public:
-        using Ptr = shared_ptr<ELCH<PointT> >;
-        using ConstPtr = shared_ptr<const ELCH<PointT> >;
-
-        using PointCloud = pcl::PointCloud<PointT>;
-        using PointCloudPtr = typename PointCloud::Ptr;
-        using PointCloudConstPtr = typename PointCloud::ConstPtr;
-
-        struct Vertex
-        {
-          Vertex () : cloud () {}
-          PointCloudPtr cloud;
-          Eigen::Affine3f transform;
-        };
-
-        /** \brief graph structure to hold the SLAM graph */
-        using LoopGraph = boost::adjacency_list<
-          boost::listS, boost::eigen_vecS, boost::undirectedS,
-          Vertex,
-          boost::no_property>;
-
-        using LoopGraphPtr = shared_ptr<LoopGraph>;
-
-        using Registration = pcl::Registration<PointT, PointT>;
-        using RegistrationPtr = typename Registration::Ptr;
-        using RegistrationConstPtr = typename Registration::ConstPtr;
-
-        /** \brief Empty constructor. */
-        ELCH () : 
-          loop_graph_ (new LoopGraph), 
-          loop_start_ (0), 
-          loop_end_ (0), 
-          reg_ (new pcl::IterativeClosestPoint<PointT, PointT>), 
-          compute_loop_ (true),
-          vd_ ()
-        {};
-      
-        /** \brief Empty destructor */
-        ~ELCH () {}
-
-        /** \brief Add a new point cloud to the internal graph.
-         * \param[in] cloud the new point cloud
-         */
-        inline void
-        addPointCloud (PointCloudPtr cloud)
-        {
-          typename boost::graph_traits<LoopGraph>::vertex_descriptor vd = add_vertex (*loop_graph_);
-          (*loop_graph_)[vd].cloud = cloud;
-          if (num_vertices (*loop_graph_) > 1)
-            add_edge (vd_, vd, *loop_graph_);
-          vd_ = vd;
-        }
-
-        /** \brief Getter for the internal graph. */
-        inline LoopGraphPtr
-        getLoopGraph ()
-        {
-          return (loop_graph_);
-        }
-
-        /** \brief Setter for a new internal graph.
-         * \param[in] loop_graph the new graph
-         */
-        inline void
-        setLoopGraph (LoopGraphPtr loop_graph)
-        {
-          loop_graph_ = loop_graph;
-        }
-
-        /** \brief Getter for the first scan of a loop. */
-        inline typename boost::graph_traits<LoopGraph>::vertex_descriptor
-        getLoopStart ()
-        {
-          return (loop_start_);
-        }
-
-        /** \brief Setter for the first scan of a loop.
-         * \param[in] loop_start the scan that starts the loop
-         */
-        inline void
-        setLoopStart (const typename boost::graph_traits<LoopGraph>::vertex_descriptor &loop_start)
-        {
-          loop_start_ = loop_start;
-        }
-
-        /** \brief Getter for the last scan of a loop. */
-        inline typename boost::graph_traits<LoopGraph>::vertex_descriptor
-        getLoopEnd ()
-        {
-          return (loop_end_);
-        }
-
-        /** \brief Setter for the last scan of a loop.
-         * \param[in] loop_end the scan that ends the loop
-         */
-        inline void
-        setLoopEnd (const typename boost::graph_traits<LoopGraph>::vertex_descriptor &loop_end)
-        {
-          loop_end_ = loop_end;
-        }
-
-        /** \brief Getter for the registration algorithm. */
-        inline RegistrationPtr
-        getReg ()
-        {
-          return (reg_);
-        }
-
-        /** \brief Setter for the registration algorithm.
-         * \param[in] reg the registration algorithm used to compute the transformation between the start and the end of the loop
-         */
-        inline void
-        setReg (RegistrationPtr reg)
-        {
-          reg_ = reg;
-        }
-
-        /** \brief Getter for the transformation between the first and the last scan. */
-        inline Eigen::Matrix4f
-        getLoopTransform ()
-        {
-          return (loop_transform_);
-        }
-
-        /** \brief Setter for the transformation between the first and the last scan.
-         * \param[in] loop_transform the transformation between the first and the last scan
-         */
-        inline void
-        setLoopTransform (const Eigen::Matrix4f &loop_transform)
-        {
-          loop_transform_ = loop_transform;
-          compute_loop_ = false;
-        }
-
-        /** \brief Computes new poses for all point clouds by closing the loop
-         * between start and end point cloud. This will transform all given point
-         * clouds for now!
-         */
-        void
-        compute ();
-
-      protected:
-        using PCLBase<PointT>::deinitCompute;
-
-        /** \brief This method should get called before starting the actual computation. */
-        virtual bool
-        initCompute ();
-
-      private:
-        /** \brief graph structure for the internal optimization graph */
-        using LOAGraph = boost::adjacency_list<
-          boost::listS, boost::vecS, boost::undirectedS,
-          boost::no_property,
-          boost::property< boost::edge_weight_t, double > >;
-
-        /**
-         * graph balancer algorithm computes the weights
-         * @param[in] g the graph
-         * @param[in] f index of the first node
-         * @param[in] l index of the last node
-         * @param[out] weights array for the weights
-         */
-        void
-        loopOptimizerAlgorithm (LOAGraph &g, double *weights);
-
-        /** \brief The internal loop graph. */
-        LoopGraphPtr loop_graph_;
-
-        /** \brief The first scan of the loop. */
-        typename boost::graph_traits<LoopGraph>::vertex_descriptor loop_start_;
-
-        /** \brief The last scan of the loop. */
-        typename boost::graph_traits<LoopGraph>::vertex_descriptor loop_end_;
-
-        /** \brief The registration object used to close the loop. */
-        RegistrationPtr reg_;
-
-        /** \brief The transformation between that start and end of the loop. */
-        Eigen::Matrix4f loop_transform_;
-        bool compute_loop_;
-
-        /** \brief previously added node in the loop_graph_. */
-        typename boost::graph_traits<LoopGraph>::vertex_descriptor vd_;
-
-      public:
-        PCL_MAKE_ALIGNED_OPERATOR_NEW
-    };
+    return (reg_);
   }
-}
+
+  /** \brief Setter for the registration algorithm.
+   * \param[in] reg the registration algorithm used to compute the transformation
+   * between the start and the end of the loop
+   */
+  inline void
+  setReg(RegistrationPtr reg)
+  {
+    reg_ = reg;
+  }
+
+  /** \brief Getter for the transformation between the first and the last scan. */
+  inline Eigen::Matrix4f
+  getLoopTransform()
+  {
+    return (loop_transform_);
+  }
+
+  /** \brief Setter for the transformation between the first and the last scan.
+   * \param[in] loop_transform the transformation between the first and the last scan
+   */
+  inline void
+  setLoopTransform(const Eigen::Matrix4f& loop_transform)
+  {
+    loop_transform_ = loop_transform;
+    compute_loop_ = false;
+  }
+
+  /** \brief Computes new poses for all point clouds by closing the loop
+   * between start and end point cloud. This will transform all given point
+   * clouds for now!
+   */
+  void
+  compute();
+
+protected:
+  using PCLBase<PointT>::deinitCompute;
+
+  /** \brief This method should get called before starting the actual computation. */
+  virtual bool
+  initCompute();
+
+private:
+  /** \brief graph structure for the internal optimization graph */
+  using LOAGraph = boost::adjacency_list<boost::listS,
+                                         boost::vecS,
+                                         boost::undirectedS,
+                                         boost::no_property,
+                                         boost::property<boost::edge_weight_t, double>>;
+
+  /**
+   * graph balancer algorithm computes the weights
+   * @param[in] g the graph
+   * @param[in] f index of the first node
+   * @param[in] l index of the last node
+   * @param[out] weights array for the weights
+   */
+  void
+  loopOptimizerAlgorithm(LOAGraph& g, double* weights);
+
+  /** \brief The internal loop graph. */
+  LoopGraphPtr loop_graph_;
+
+  /** \brief The first scan of the loop. */
+  typename boost::graph_traits<LoopGraph>::vertex_descriptor loop_start_;
+
+  /** \brief The last scan of the loop. */
+  typename boost::graph_traits<LoopGraph>::vertex_descriptor loop_end_;
+
+  /** \brief The registration object used to close the loop. */
+  RegistrationPtr reg_;
+
+  /** \brief The transformation between that start and end of the loop. */
+  Eigen::Matrix4f loop_transform_;
+  bool compute_loop_;
+
+  /** \brief previously added node in the loop_graph_. */
+  typename boost::graph_traits<LoopGraph>::vertex_descriptor vd_;
+
+public:
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+};
+} // namespace registration
+} // namespace pcl
 
 #include <pcl/registration/impl/elch.hpp>
diff --git a/registration/include/pcl/registration/exceptions.h b/registration/include/pcl/registration/exceptions.h
index 85fb8b29..0d11feb7 100644
--- a/registration/include/pcl/registration/exceptions.h
+++ b/registration/include/pcl/registration/exceptions.h
@@ -42,34 +42,31 @@
 
 #include <pcl/exceptions.h>
 
-namespace pcl
-{
-  /** \class SolverDidntConvergeException
-    * \brief An exception that is thrown when the non linear solver didn't converge
-    */
-  class PCL_EXPORTS SolverDidntConvergeException : public PCLException
-  {
-    public:
-    
-    SolverDidntConvergeException (const std::string& error_description,
-                                  const char* file_name = nullptr,
-                                  const char* function_name = nullptr,
-                                  unsigned line_number = 0)
-      : pcl::PCLException (error_description, file_name, function_name, line_number) { }
-  } ;
+namespace pcl {
+/** \class SolverDidntConvergeException
+ * \brief An exception that is thrown when the non linear solver didn't converge
+ */
+class PCL_EXPORTS SolverDidntConvergeException : public PCLException {
+public:
+  SolverDidntConvergeException(const std::string& error_description,
+                               const char* file_name = nullptr,
+                               const char* function_name = nullptr,
+                               unsigned line_number = 0)
+  : pcl::PCLException(error_description, file_name, function_name, line_number)
+  {}
+};
 
- /** \class NotEnoughPointsException
-    * \brief An exception that is thrown when the number of correspondents is not equal
-    * to the minimum required
-    */
-  class PCL_EXPORTS NotEnoughPointsException : public PCLException
-  {
-    public:
-    
-    NotEnoughPointsException (const std::string& error_description,
-                              const char* file_name = nullptr,
-                              const char* function_name = nullptr,
-                              unsigned line_number = 0)
-      : pcl::PCLException (error_description, file_name, function_name, line_number) { }
-  } ;
-}
+/** \class NotEnoughPointsException
+ * \brief An exception that is thrown when the number of correspondents is not equal
+ * to the minimum required
+ */
+class PCL_EXPORTS NotEnoughPointsException : public PCLException {
+public:
+  NotEnoughPointsException(const std::string& error_description,
+                           const char* file_name = nullptr,
+                           const char* function_name = nullptr,
+                           unsigned line_number = 0)
+  : pcl::PCLException(error_description, file_name, function_name, line_number)
+  {}
+};
+} // namespace pcl
diff --git a/registration/include/pcl/registration/gicp.h b/registration/include/pcl/registration/gicp.h
index e3b8ea6f..002dc53f 100644
--- a/registration/include/pcl/registration/gicp.h
+++ b/registration/include/pcl/registration/gicp.h
@@ -40,362 +40,408 @@
 
 #pragma once
 
-#include <pcl/registration/icp.h>
 #include <pcl/registration/bfgs.h>
+#include <pcl/registration/icp.h>
+
+namespace pcl {
+/** \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
+ * after closest point assignments have been made.
+ * The original code uses GSL and ANN while in ours we use an eigen mapped BFGS and
+ * FLANN.
+ * \author Nizar Sallem
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget>
+class GeneralizedIterativeClosestPoint
+: public IterativeClosestPoint<PointSource, PointTarget> {
+public:
+  using IterativeClosestPoint<PointSource, PointTarget>::reg_name_;
+  using IterativeClosestPoint<PointSource, PointTarget>::getClassName;
+  using IterativeClosestPoint<PointSource, PointTarget>::indices_;
+  using IterativeClosestPoint<PointSource, PointTarget>::target_;
+  using IterativeClosestPoint<PointSource, PointTarget>::input_;
+  using IterativeClosestPoint<PointSource, PointTarget>::tree_;
+  using IterativeClosestPoint<PointSource, PointTarget>::tree_reciprocal_;
+  using IterativeClosestPoint<PointSource, PointTarget>::nr_iterations_;
+  using IterativeClosestPoint<PointSource, PointTarget>::max_iterations_;
+  using IterativeClosestPoint<PointSource, PointTarget>::previous_transformation_;
+  using IterativeClosestPoint<PointSource, PointTarget>::final_transformation_;
+  using IterativeClosestPoint<PointSource, PointTarget>::transformation_;
+  using IterativeClosestPoint<PointSource, PointTarget>::transformation_epsilon_;
+  using IterativeClosestPoint<PointSource, PointTarget>::converged_;
+  using IterativeClosestPoint<PointSource, PointTarget>::corr_dist_threshold_;
+  using IterativeClosestPoint<PointSource, PointTarget>::inlier_threshold_;
+  using IterativeClosestPoint<PointSource, PointTarget>::min_number_correspondences_;
+  using IterativeClosestPoint<PointSource, PointTarget>::update_visualizer_;
+
+  using PointCloudSource = pcl::PointCloud<PointSource>;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+
+  using PointCloudTarget = pcl::PointCloud<PointTarget>;
+  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
+  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
+
+  using PointIndicesPtr = PointIndices::Ptr;
+  using PointIndicesConstPtr = PointIndices::ConstPtr;
+
+  using MatricesVector =
+      std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>>;
+  using MatricesVectorPtr = shared_ptr<MatricesVector>;
+  using MatricesVectorConstPtr = shared_ptr<const MatricesVector>;
+
+  using InputKdTree = typename Registration<PointSource, PointTarget>::KdTree;
+  using InputKdTreePtr = typename Registration<PointSource, PointTarget>::KdTreePtr;
+
+  using Ptr = shared_ptr<GeneralizedIterativeClosestPoint<PointSource, PointTarget>>;
+  using ConstPtr =
+      shared_ptr<const GeneralizedIterativeClosestPoint<PointSource, PointTarget>>;
+
+  using Vector6d = Eigen::Matrix<double, 6, 1>;
+
+  /** \brief Empty constructor. */
+  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";
+    max_iterations_ = 200;
+    transformation_epsilon_ = 5e-4;
+    corr_dist_threshold_ = 5.;
+    rigid_transformation_estimation_ = [this](const PointCloudSource& cloud_src,
+                                              const pcl::Indices& indices_src,
+                                              const PointCloudTarget& cloud_tgt,
+                                              const pcl::Indices& indices_tgt,
+                                              Eigen::Matrix4f& transformation_matrix) {
+      estimateRigidTransformationBFGS(
+          cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
+    };
+  }
+
+  /** \brief Provide a pointer to the input dataset
+   * \param cloud the const boost shared pointer to a PointCloud message
+   */
+  inline void
+  setInputSource(const PointCloudSourceConstPtr& cloud) override
+  {
+
+    if (cloud->points.empty()) {
+      PCL_ERROR(
+          "[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n",
+          getClassName().c_str());
+      return;
+    }
+    PointCloudSource input = *cloud;
+    // 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<PointSource, PointTarget>::setInputSource(cloud);
+    input_covariances_.reset();
+  }
+
+  /** \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] covariances the input source covariances
+   */
+  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
+  setInputTarget(const PointCloudTargetConstPtr& target) override
+  {
+    pcl::IterativeClosestPoint<PointSource, PointTarget>::setInputTarget(target);
+    target_covariances_.reset();
+  }
+
+  /** \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] covariances the input target covariances
+   */
+  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 \param[in] indices_src the vector of
+   * indices describing the points of interest in \a cloud_src
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[in] indices_tgt the vector of indices describing
+   * the correspondences of the interest points from \a indices_src
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  void
+  estimateRigidTransformationBFGS(const PointCloudSource& cloud_src,
+                                  const pcl::Indices& indices_src,
+                                  const PointCloudTarget& cloud_tgt,
+                                  const pcl::Indices& 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
+  {
+    assert(index < mahalanobis_.size());
+    return mahalanobis_[index];
+  }
+
+  /** \brief Computes rotation matrix derivative.
+   * rotation matrix is obtainded from rotation angles x[3], x[4] and x[5]
+   * \return d/d_rx, d/d_ry and d/d_rz respectively in g[3], g[4] and g[5]
+   * param x array representing 3D transformation
+   * param R rotation matrix
+   * param g gradient vector
+   */
+  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
+   * converged to the final solution.
+   * \param epsilon the rotation epsilon
+   */
+  inline void
+  setRotationEpsilon(double epsilon)
+  {
+    rotation_epsilon_ = epsilon;
+  }
+
+  /** \brief Get the rotation epsilon (maximum allowable difference between two
+   * consecutive rotations) as set by the user.
+   */
+  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
+   * 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
+   */
+  int
+  getCorrespondenceRandomness() const
+  {
+    return k_correspondences_;
+  }
+
+  /** \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;
+  }
+
+  /** \brief Return maximum number of iterations at the optimization step
+   */
+  int
+  getMaximumOptimizerIterations() const
+  {
+    return max_inner_iterations_;
+  }
+
+  /** \brief Set the minimal translation gradient threshold for early optimization stop
+   * \param[in] tolerance 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] tolerance rotation gradient threshold in radians
+   */
+  void
+  setRotationGradientTolerance(double tolerance)
+  {
+    rotation_gradient_tolerance_ = tolerance;
+  }
 
-namespace pcl
-{
-  /** \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
-    * after closest point assignments have been made.
-    * The original code uses GSL and ANN while in ours we use an eigen mapped BFGS and
-    * FLANN.
-    * \author Nizar Sallem
-    * \ingroup registration
-    */
-  template <typename PointSource, typename PointTarget>
-  class GeneralizedIterativeClosestPoint : public IterativeClosestPoint<PointSource, PointTarget>
+  /** \brief Return the minimal rotation gradient threshold for early optimization stop
+   */
+  double
+  getRotationGradientTolerance() const
   {
-    public:
-      using IterativeClosestPoint<PointSource, PointTarget>::reg_name_;
-      using IterativeClosestPoint<PointSource, PointTarget>::getClassName;
-      using IterativeClosestPoint<PointSource, PointTarget>::indices_;
-      using IterativeClosestPoint<PointSource, PointTarget>::target_;
-      using IterativeClosestPoint<PointSource, PointTarget>::input_;
-      using IterativeClosestPoint<PointSource, PointTarget>::tree_;
-      using IterativeClosestPoint<PointSource, PointTarget>::tree_reciprocal_;
-      using IterativeClosestPoint<PointSource, PointTarget>::nr_iterations_;
-      using IterativeClosestPoint<PointSource, PointTarget>::max_iterations_;
-      using IterativeClosestPoint<PointSource, PointTarget>::previous_transformation_;
-      using IterativeClosestPoint<PointSource, PointTarget>::final_transformation_;
-      using IterativeClosestPoint<PointSource, PointTarget>::transformation_;
-      using IterativeClosestPoint<PointSource, PointTarget>::transformation_epsilon_;
-      using IterativeClosestPoint<PointSource, PointTarget>::converged_;
-      using IterativeClosestPoint<PointSource, PointTarget>::corr_dist_threshold_;
-      using IterativeClosestPoint<PointSource, PointTarget>::inlier_threshold_;
-      using IterativeClosestPoint<PointSource, PointTarget>::min_number_correspondences_;
-      using IterativeClosestPoint<PointSource, PointTarget>::update_visualizer_;
-
-      using PointCloudSource = pcl::PointCloud<PointSource>;
-      using PointCloudSourcePtr = typename PointCloudSource::Ptr;
-      using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
-
-      using PointCloudTarget = pcl::PointCloud<PointTarget>;
-      using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
-      using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
-
-      using PointIndicesPtr = PointIndices::Ptr;
-      using PointIndicesConstPtr = PointIndices::ConstPtr;
-
-      using MatricesVector = std::vector< Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d> >;
-      using MatricesVectorPtr = shared_ptr<MatricesVector>;
-      using MatricesVectorConstPtr = shared_ptr<const MatricesVector>;
-
-      using InputKdTree = typename Registration<PointSource, PointTarget>::KdTree;
-      using InputKdTreePtr = typename Registration<PointSource, PointTarget>::KdTreePtr;
-
-      using Ptr = shared_ptr< GeneralizedIterativeClosestPoint<PointSource, PointTarget> >;
-      using ConstPtr = shared_ptr< const GeneralizedIterativeClosestPoint<PointSource, PointTarget> >;
-
-
-      using Vector6d = Eigen::Matrix<double, 6, 1>;
-
-      /** \brief Empty constructor. */
-      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";
-        max_iterations_ = 200;
-        transformation_epsilon_ = 5e-4;
-        corr_dist_threshold_ = 5.;
-        rigid_transformation_estimation_ = [this] (const PointCloudSource& cloud_src,
-                                                   const std::vector<int>& indices_src,
-                                                   const PointCloudTarget& cloud_tgt,
-                                                   const std::vector<int>& indices_tgt,
-                                                   Eigen::Matrix4f& transformation_matrix)
-        {
-          estimateRigidTransformationBFGS (cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
-        };
-      }
-
-      /** \brief Provide a pointer to the input dataset
-        * \param cloud the const boost shared pointer to a PointCloud message
-        */
-      inline void
-      setInputSource (const PointCloudSourceConstPtr &cloud) override
-      {
-
-        if (cloud->points.empty ())
-        {
-          PCL_ERROR ("[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
-          return;
-        }
-        PointCloudSource input = *cloud;
-        // 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<PointSource, PointTarget>::setInputSource (cloud);
-        input_covariances_.reset ();
-      }
-
-      /** \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] covariances the input source covariances
-        */
-      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
-      setInputTarget (const PointCloudTargetConstPtr &target) override
-      {
-        pcl::IterativeClosestPoint<PointSource, PointTarget>::setInputTarget(target);
-        target_covariances_.reset ();
-      }
-
-      /** \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] covariances the input target covariances
-        */
-	    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
-        * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
-        * \param[in] cloud_tgt the target point cloud dataset
-        * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from \a indices_src
-        * \param[out] transformation_matrix the resultant transformation matrix
-        */
-      void
-      estimateRigidTransformationBFGS (const PointCloudSource &cloud_src,
-                                       const std::vector<int> &indices_src,
-                                       const PointCloudTarget &cloud_tgt,
-                                       const std::vector<int> &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
-      {
-        assert(index < mahalanobis_.size());
-        return mahalanobis_[index];
-      }
-
-      /** \brief Computes rotation matrix derivative.
-        * rotation matrix is obtainded from rotation angles x[3], x[4] and x[5]
-        * \return d/d_rx, d/d_ry and d/d_rz respectively in g[3], g[4] and g[5]
-        * param x array representing 3D transformation
-        * param R rotation matrix
-        * param g gradient vector
-        */
-      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
-        * converged to the final solution.
-        * \param epsilon the rotation epsilon
-        */
-      inline void
-      setRotationEpsilon (double epsilon) { rotation_epsilon_ = epsilon; }
-
-      /** \brief Get the rotation epsilon (maximum allowable difference between two
-        * consecutive rotations) as set by the user.
-        */
-      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
-        * 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
-        */
-      int
-      getCorrespondenceRandomness () const { return k_correspondences_; }
-
-      /** \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; }
-
-      /** \brief Return maximum number of iterations at the optimization step
-	  */
-      int
-      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.
-        * default: 20
-        */
-      int k_correspondences_;
-
-      /** \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
-        * is split in rotation part and translation part).
-        * default: 2e-3
-        */
-      double rotation_epsilon_;
-
-      /** \brief base transformation */
-      Eigen::Matrix4f base_transformation_;
-
-      /** \brief Temporary pointer to the source dataset. */
-      const PointCloudSource *tmp_src_;
-
-      /** \brief Temporary pointer to the target dataset. */
-      const PointCloudTarget  *tmp_tgt_;
-
-      /** \brief Temporary pointer to the source dataset indices. */
-      const std::vector<int> *tmp_idx_src_;
-
-      /** \brief Temporary pointer to the target dataset indices. */
-      const std::vector<int> *tmp_idx_tgt_;
-
-      /** \brief Input cloud points covariances. */
-      MatricesVectorPtr input_covariances_;
-
-      /** \brief Target cloud points covariances. */
-      MatricesVectorPtr target_covariances_;
-
-      /** \brief Mahalanobis matrices holder. */
-      std::vector<Eigen::Matrix3d> mahalanobis_;
-
-      /** \brief maximum number of optimizations */
-      int max_inner_iterations_;
-
-	  /** \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<typename PointT>
-      void computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud,
-                              const typename pcl::search::KdTree<PointT>::Ptr tree,
-                              MatricesVector& cloud_covariances);
-
-      /** \return trace of mat1^t . mat2
-        * \param mat1 matrix of dimension nxm
-        * \param mat2 matrix of dimension nxp
-        */
-      inline double
-      matricesInnerProd(const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2) const
-      {
-        double r = 0.;
-        std::size_t n = mat1.rows();
-        // tr(mat1^t.mat2)
-        for(std::size_t i = 0; i < n; i++)
-          for(std::size_t j = 0; j < n; j++)
-            r += mat1 (j, i) * mat2 (i,j);
-        return r;
-      }
-
-      /** \brief Rigid transformation computation method  with initial guess.
-        * \param output the transformed input point cloud dataset using the rigid transformation found
-        * \param guess the initial guess of the transformation to compute
-        */
-      void
-      computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) override;
-
-      /** \brief Search for the closest nearest neighbor of a given point.
-        * \param query the point to search a nearest neighbour for
-        * \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
-      searchForNeighbors (const PointSource &query, std::vector<int>& index, std::vector<float>& distance)
-      {
-        int k = tree_->nearestKSearch (query, 1, index, distance);
-        if (k == 0)
-          return (false);
-        return (true);
-      }
-
-      /// \brief compute transformation matrix from transformation matrix
-      void applyState(Eigen::Matrix4f &t, const Vector6d& x) const;
-
-      /// \brief optimization functor structure
-      struct OptimizationFunctorWithIndices : public BFGSDummyFunctor<double,6>
-      {
-        OptimizationFunctorWithIndices (const GeneralizedIterativeClosestPoint* gicp)
-          : BFGSDummyFunctor<double,6> (), gicp_(gicp) {}
-        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<void(const pcl::PointCloud<PointSource> &cloud_src,
-                           const std::vector<int> &src_indices,
-                           const pcl::PointCloud<PointTarget> &cloud_tgt,
-                           const std::vector<int> &tgt_indices,
-                           Eigen::Matrix4f &transformation_matrix)> rigid_transformation_estimation_;
+    return rotation_gradient_tolerance_;
+  }
+
+protected:
+  /** \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
+   * default: 0.001
+   */
+  double gicp_epsilon_;
+
+  /** The epsilon constant for rotation error. (In GICP the transformation epsilon
+   * is split in rotation part and translation part).
+   * default: 2e-3
+   */
+  double rotation_epsilon_;
+
+  /** \brief base transformation */
+  Eigen::Matrix4f base_transformation_;
+
+  /** \brief Temporary pointer to the source dataset. */
+  const PointCloudSource* tmp_src_;
+
+  /** \brief Temporary pointer to the target dataset. */
+  const PointCloudTarget* tmp_tgt_;
+
+  /** \brief Temporary pointer to the source dataset indices. */
+  const pcl::Indices* tmp_idx_src_;
+
+  /** \brief Temporary pointer to the target dataset indices. */
+  const pcl::Indices* tmp_idx_tgt_;
+
+  /** \brief Input cloud points covariances. */
+  MatricesVectorPtr input_covariances_;
+
+  /** \brief Target cloud points covariances. */
+  MatricesVectorPtr target_covariances_;
+
+  /** \brief Mahalanobis matrices holder. */
+  std::vector<Eigen::Matrix3d> mahalanobis_;
+
+  /** \brief maximum number of optimizations */
+  int max_inner_iterations_;
+
+  /** \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 <typename PointT>
+  void
+  computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud,
+                     const typename pcl::search::KdTree<PointT>::Ptr tree,
+                     MatricesVector& cloud_covariances);
+
+  /** \return trace of mat1^t . mat2
+   * \param mat1 matrix of dimension nxm
+   * \param mat2 matrix of dimension nxp
+   */
+  inline double
+  matricesInnerProd(const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2) const
+  {
+    double r = 0.;
+    std::size_t n = mat1.rows();
+    // tr(mat1^t.mat2)
+    for (std::size_t i = 0; i < n; i++)
+      for (std::size_t j = 0; j < n; j++)
+        r += mat1(j, i) * mat2(i, j);
+    return r;
+  }
+
+  /** \brief Rigid transformation computation method  with initial guess.
+   * \param output the transformed input point cloud dataset using the rigid
+   * transformation found \param guess the initial guess of the transformation to
+   * compute
+   */
+  void
+  computeTransformation(PointCloudSource& output,
+                        const Eigen::Matrix4f& guess) override;
+
+  /** \brief Search for the closest nearest neighbor of a given point.
+   * \param query the point to search a nearest neighbour for
+   * \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
+  searchForNeighbors(const PointSource& query,
+                     pcl::Indices& index,
+                     std::vector<float>& distance)
+  {
+    int k = tree_->nearestKSearch(query, 1, index, distance);
+    if (k == 0)
+      return (false);
+    return (true);
+  }
+
+  /// \brief compute transformation matrix from transformation matrix
+  void
+  applyState(Eigen::Matrix4f& t, const Vector6d& x) const;
+
+  /// \brief optimization functor structure
+  struct OptimizationFunctorWithIndices : public BFGSDummyFunctor<double, 6> {
+    OptimizationFunctorWithIndices(const GeneralizedIterativeClosestPoint* gicp)
+    : BFGSDummyFunctor<double, 6>(), gicp_(gicp)
+    {}
+    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<void(const pcl::PointCloud<PointSource>& cloud_src,
+                     const pcl::Indices& src_indices,
+                     const pcl::PointCloud<PointTarget>& cloud_tgt,
+                     const pcl::Indices& tgt_indices,
+                     Eigen::Matrix4f& transformation_matrix)>
+      rigid_transformation_estimation_;
+};
+} // namespace pcl
 
 #include <pcl/registration/impl/gicp.hpp>
diff --git a/registration/include/pcl/registration/gicp6d.h b/registration/include/pcl/registration/gicp6d.h
index c69f72f7..2718344b 100644
--- a/registration/include/pcl/registration/gicp6d.h
+++ b/registration/include/pcl/registration/gicp6d.h
@@ -38,172 +38,129 @@
 
 #pragma once
 
+#include <pcl/kdtree/impl/kdtree_flann.hpp>
+#include <pcl/registration/gicp.h>
 #include <pcl/memory.h>
-#include <pcl/pcl_macros.h>
-#include <pcl/point_types.h>
+#include <pcl/pcl_exports.h> // for PCL_EXPORTS
 #include <pcl/point_cloud.h>
 #include <pcl/point_representation.h>
-#include <pcl/kdtree/impl/kdtree_flann.hpp>
-#include <pcl/registration/gicp.h>
+#include <pcl/point_types.h>
 
-namespace pcl
-{
-  struct EIGEN_ALIGN16 _PointXYZLAB
-  {
-    PCL_ADD_POINT4D; // this adds the members x,y,z
-    union
-    {
-      struct
-      {
-        float L;
-        float a;
-        float b;
-      };
-      float data_lab[4];
-    };
-    PCL_MAKE_ALIGNED_OPERATOR_NEW
-  };
+namespace pcl {
+/** \brief GeneralizedIterativeClosestPoint6D integrates L*a*b* color space information
+ * into the Generalized Iterative Closest Point (GICP) algorithm.
+ *
+ * The suggested input is PointXYZRGBA.
+ *
+ * \note If you use this code in any academic work, please cite:
+ *
+ * - M. Korn, M. Holzkothen, J. Pauli
+ * Color Supported Generalized-ICP.
+ * In Proceedings of VISAPP 2014 - International Conference on Computer Vision Theory
+ * and Applications, Lisbon, Portugal, January 2014.
+ *
+ * \author Martin Holzkothen, Michael Korn
+ * \ingroup registration
+ */
+class PCL_EXPORTS GeneralizedIterativeClosestPoint6D
+: public GeneralizedIterativeClosestPoint<PointXYZRGBA, PointXYZRGBA> {
+  using PointSource = PointXYZRGBA;
+  using PointTarget = PointXYZRGBA;
 
-  /** \brief A custom point type for position and CIELAB color value */
-  struct PointXYZLAB : public _PointXYZLAB
-  {
-    inline PointXYZLAB ()
-    {
-      x = y = z = 0.0f; data[3]     = 1.0f;  // important for homogeneous coordinates
-      L = a = b = 0.0f; data_lab[3] = 0.0f;
-    }
-  };
-}
-
-// register the custom point type in PCL
-POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::_PointXYZLAB,
-    (float, x, x)
-    (float, y, y)
-    (float, z, z)
-    (float, L, L)
-    (float, a, a)
-    (float, b, b)
-)
-POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZLAB, pcl::_PointXYZLAB)
-
-namespace pcl
-{
-  /** \brief GeneralizedIterativeClosestPoint6D integrates L*a*b* color space information into the
-   * Generalized Iterative Closest Point (GICP) algorithm.
-   *
-   * The suggested input is PointXYZRGBA.
+public:
+  /** \brief constructor.
    *
-   * \note If you use this code in any academic work, please cite:
+   * \param[in] lab_weight the color weight
+   */
+  GeneralizedIterativeClosestPoint6D(float lab_weight = 0.032f);
+
+  /** \brief Provide a pointer to the input source
+   * (e.g., the point cloud that we want to align to the target)
    *
-   * - M. Korn, M. Holzkothen, J. Pauli
-   * Color Supported Generalized-ICP.
-   * In Proceedings of VISAPP 2014 - International Conference on Computer Vision Theory and Applications,
-   * Lisbon, Portugal, January 2014.
+   * \param[in] cloud the input point cloud source
+   */
+  void
+  setInputSource(const PointCloudSourceConstPtr& cloud) override;
+
+  /** \brief Provide a pointer to the input target
+   * (e.g., the point cloud that we want to align the input source to)
    *
-   * \author Martin Holzkothen, Michael Korn
-   * \ingroup registration
+   * \param[in] cloud the input point cloud target
+   */
+  void
+  setInputTarget(const PointCloudTargetConstPtr& target) override;
+
+protected:
+  /** \brief Rigid transformation computation method  with initial guess.
+   * \param output the transformed input point cloud dataset using the rigid
+   * transformation found \param guess the initial guess of the transformation to
+   * compute
+   */
+  void
+  computeTransformation(PointCloudSource& output,
+                        const Eigen::Matrix4f& guess) override;
+
+  /** \brief Search for the closest nearest neighbor of a given point.
+   * \param query the point to search a nearest neighbour for
+   * \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
    */
-  class PCL_EXPORTS GeneralizedIterativeClosestPoint6D : public GeneralizedIterativeClosestPoint<PointXYZRGBA, PointXYZRGBA>
-  {
-    using PointSource = PointXYZRGBA;
-    using PointTarget = PointXYZRGBA;
-
-    public:
-
-      /** \brief constructor.
-       *
-       * \param[in] lab_weight the color weight
-       */
-      GeneralizedIterativeClosestPoint6D (float lab_weight = 0.032f);
-
-      /** \brief Provide a pointer to the input source
-       * (e.g., the point cloud that we want to align to the target)
-       *
-       * \param[in] cloud the input point cloud source
-       */
-      void
-      setInputSource (const PointCloudSourceConstPtr& cloud) override;
-
-      /** \brief Provide a pointer to the input target
-       * (e.g., the point cloud that we want to align the input source to)
-       *
-       * \param[in] cloud the input point cloud target
-       */
-      void
-      setInputTarget (const PointCloudTargetConstPtr& target) override;
-
-    protected:
-
-      /** \brief Rigid transformation computation method  with initial guess.
-       * \param output the transformed input point cloud dataset using the rigid transformation found
-       * \param guess the initial guess of the transformation to compute
-       */
-      void
-      computeTransformation (PointCloudSource& output,
-          const Eigen::Matrix4f& guess) override;
-
-      /** \brief Search for the closest nearest neighbor of a given point.
-       * \param query the point to search a nearest neighbour for
-       * \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
-      searchForNeighbors (const PointXYZLAB& query, std::vector<int>& index, std::vector<float>& distance);
-
-    protected:
-      /** \brief Holds the converted (LAB) data cloud. */
-      pcl::PointCloud<PointXYZLAB>::Ptr cloud_lab_;
-
-      /** \brief Holds the converted (LAB) model cloud. */
-      pcl::PointCloud<PointXYZLAB>::Ptr target_lab_;
-
-      /** \brief 6d-tree to search in model cloud. */
-      KdTreeFLANN<PointXYZLAB> target_tree_lab_;
-
-      /** \brief The color weight. */
-      float lab_weight_;
-
-      /**  \brief Custom point representation to perform kdtree searches in more than 3 (i.e. in all 6) dimensions. */
-      class MyPointRepresentation : public PointRepresentation<PointXYZLAB>
-      {
-          using PointRepresentation<PointXYZLAB>::nr_dimensions_;
-          using PointRepresentation<PointXYZLAB>::trivial_;
-
-        public:
-          using Ptr = shared_ptr<MyPointRepresentation>;
-          using ConstPtr = shared_ptr<const MyPointRepresentation>;
-
-          MyPointRepresentation ()
-          {
-            nr_dimensions_ = 6;
-            trivial_ = false;
-          }
-
-          
-          ~MyPointRepresentation ()
-          {
-          }
-
-          inline Ptr
-          makeShared () const
-          {
-            return Ptr (new MyPointRepresentation (*this));
-          }
-
-          void
-          copyToFloatArray (const PointXYZLAB &p, float * out) const override
-          {
-            // copy all of the six values
-            out[0] = p.x;
-            out[1] = p.y;
-            out[2] = p.z;
-            out[3] = p.L;
-            out[4] = p.a;
-            out[5] = p.b;
-          }
-      };
-
-      /** \brief Enables 6d searches with kd-tree class using the color weight. */
-      MyPointRepresentation point_rep_;
+  inline bool
+  searchForNeighbors(const PointXYZLAB& query,
+                     pcl::Indices& index,
+                     std::vector<float>& distance);
+
+protected:
+  /** \brief Holds the converted (LAB) data cloud. */
+  pcl::PointCloud<PointXYZLAB>::Ptr cloud_lab_;
+
+  /** \brief Holds the converted (LAB) model cloud. */
+  pcl::PointCloud<PointXYZLAB>::Ptr target_lab_;
+
+  /** \brief 6d-tree to search in model cloud. */
+  KdTreeFLANN<PointXYZLAB> target_tree_lab_;
+
+  /** \brief The color weight. */
+  float lab_weight_;
+
+  /**  \brief Custom point representation to perform kdtree searches in more than 3
+   * (i.e. in all 6) dimensions. */
+  class MyPointRepresentation : public PointRepresentation<PointXYZLAB> {
+    using PointRepresentation<PointXYZLAB>::nr_dimensions_;
+    using PointRepresentation<PointXYZLAB>::trivial_;
+
+  public:
+    using Ptr = shared_ptr<MyPointRepresentation>;
+    using ConstPtr = shared_ptr<const MyPointRepresentation>;
+
+    MyPointRepresentation()
+    {
+      nr_dimensions_ = 6;
+      trivial_ = false;
+    }
+
+    ~MyPointRepresentation() {}
+
+    inline Ptr
+    makeShared() const
+    {
+      return Ptr(new MyPointRepresentation(*this));
+    }
+
+    void
+    copyToFloatArray(const PointXYZLAB& p, float* out) const override
+    {
+      // copy all of the six values
+      out[0] = p.x;
+      out[1] = p.y;
+      out[2] = p.z;
+      out[3] = p.L;
+      out[4] = p.a;
+      out[5] = p.b;
+    }
   };
-}
+
+  /** \brief Enables 6d searches with kd-tree class using the color weight. */
+  MyPointRepresentation point_rep_;
+};
+} // namespace pcl
diff --git a/registration/include/pcl/registration/graph_handler.h b/registration/include/pcl/registration/graph_handler.h
index ef960a5b..4b05b8f1 100644
--- a/registration/include/pcl/registration/graph_handler.h
+++ b/registration/include/pcl/registration/graph_handler.h
@@ -40,174 +40,187 @@
 
 #pragma once
 
-#include <pcl/registration/vertex_estimates.h>
 #include <pcl/registration/edge_measurements.h>
+#include <pcl/registration/vertex_estimates.h>
 #include <pcl/exceptions.h>
+
 #include "boost/graph/graph_traits.hpp"
 
-namespace pcl
-{
-  namespace registration
+namespace pcl {
+namespace registration {
+/** \brief @b GraphHandler class is a wrapper for a general SLAM graph
+ * The actual graph class must fulfill the following boost::graph concepts:
+ * - BidirectionalGraph
+ * - AdjacencyGraph
+ * - VertexAndEdgeListGraph
+ * - MutableGraph
+ *
+ * Other valid expressions:
+ * - add_edge (m,g)          add a new edge according to the given measurement. Return
+ * type: std::pair<edge_descriptor, bool>
+ * - add_vertex (e,g)        add a new vertex according to the given estimate. Return
+ * type: vertex_descriptor
+ * - get_pose (v,g)          retrieve the pose estimate for v, if any. Return type:
+ * Eigen::Matrix4f
+ * - get_cloud (v,g)         retrieve the cloud pointer associated to v, if any. Return
+ * type: pcl::PointCloud<PointT>::ConstPtr
+ * - set_estimate (v,e,g)    set the estimate for an existing vertex. Return type: void.
+ * - set_measurement (d,m,g) set the measurement for an existing edge. Return type:
+ * void. Notation:
+ * - m                       an edge measurement
+ * - e                       a vertex estimate
+ * - v                       a vertex
+ * - d                       an edge
+ * - g                       a graph
+ * A valid graph implementation should accept at least the PoseEstimate estimate and the
+ * PoseMeasurement measurement
+ *
+ * If a specific graph implementation needs initialization and/or finalization,
+ * specialize the protected methods init() and deinit() for your graph type
+ *
+ * \author Nicola Fioraio
+ * \ingroup registration
+ */
+template <typename GraphT>
+class GraphHandler {
+public:
+  using Ptr = shared_ptr<GraphHandler<GraphT>>;
+  using ConstPtr = shared_ptr<const GraphHandler<GraphT>>;
+  using GraphPtr = shared_ptr<GraphT>;
+  using GraphConstPtr = shared_ptr<const GraphT>;
+
+  using Vertex = typename boost::graph_traits<GraphT>::vertex_descriptor;
+  using Edge = typename boost::graph_traits<GraphT>::edge_descriptor;
+
+  /** \brief Empty constructor. */
+  GraphHandler() : graph_impl_(new GraphT())
+  {
+    if (!init())
+      throw InitFailedException("Graph Initialization Failed",
+                                __FILE__,
+                                "pcl::registration::GraphHandler::GraphHandler ()",
+                                __LINE__);
+  }
+
+  /** \brief Destructor. */
+  ~GraphHandler() { deinit(); }
+
+  /** \brief Clear the graph */
+  inline void
+  clear()
+  {
+    deinit();
+    graph_impl_.reset(new GraphT());
+    if (!init())
+      throw InitFailedException("Graph Initialization Failed",
+                                __FILE__,
+                                "pcl::registration::GraphHandler::clear ()",
+                                __LINE__);
+  }
+
+  /** \brief Get a pointer to the BGL graph */
+  inline GraphConstPtr
+  getGraph() const
+  {
+    return graph_impl_;
+  }
+
+  /** \brief Get a pointer to the BGL graph */
+  inline GraphPtr
+  getGraph()
+  {
+    return graph_impl_;
+  }
+
+  /** \brief Add a new point cloud to the graph and return the new vertex
+   * \param cloud the new point cloud
+   * \param pose the pose estimate
+   * \return a reference to the new vertex
+   */
+  template <class PointT>
+  inline Vertex
+  addPointCloud(const typename pcl::PointCloud<PointT>::ConstPtr& cloud,
+                const Eigen::Matrix4f& pose)
+  {
+    return add_vertex(PoseEstimate<PointT>(cloud, pose), *graph_impl_);
+  }
+
+  /** \brief Add a new generic vertex created according to the given estimate
+   * \param estimate the parameters' estimate
+   * \return a reference to the new vertex
+   */
+  template <class EstimateT>
+  inline Vertex
+  addGenericVertex(const EstimateT& estimate)
+  {
+    return add_vertex(estimate, *graph_impl_);
+  }
+
+  /** \brief Add a new constraint between two poses
+   * \param v_start the first pose
+   * \param v_end the second pose
+   * \param relative_transformation the transformation from v_start to v_end
+   * \param information_matrix the uncertainty
+   * \return a reference to the new edge
+   */
+  template <class InformationT>
+  inline Edge
+  addPoseConstraint(const Vertex& v_start,
+                    const Vertex& v_end,
+                    const Eigen::Matrix4f& relative_transformation,
+                    const InformationT& information_matrix)
+  {
+    return add_edge(PoseMeasurement<Vertex, InformationT>(
+                        v_start, v_end, relative_transformation, information_matrix),
+                    *graph_impl_);
+  }
+
+  /** \brief Add a generic constraint created according to the given measurement
+   * \param measurement the measurement
+   * \return a reference to the new edge
+   */
+  template <class MeasurementT>
+  inline Edge
+  addGenericConstraint(const MeasurementT& measurement)
+  {
+    return add_edge(measurement, *graph_impl_);
+  }
+
+  /** \brief Remove a vertex from the graph
+   * \param v the vertex
+   */
+  inline void
+  removeVertex(const Vertex& v)
   {
-    /** \brief @b GraphHandler class is a wrapper for a general SLAM graph
-      * The actual graph class must fulfill the following boost::graph concepts:
-      * - BidirectionalGraph
-      * - AdjacencyGraph
-      * - VertexAndEdgeListGraph
-      * - MutableGraph
-      *
-      * Other valid expressions:
-      * - add_edge (m,g)          add a new edge according to the given measurement. Return type: std::pair<edge_descriptor, bool>
-      * - add_vertex (e,g)        add a new vertex according to the given estimate. Return type: vertex_descriptor
-      * - get_pose (v,g)          retrieve the pose estimate for v, if any. Return type: Eigen::Matrix4f
-      * - get_cloud (v,g)         retrieve the cloud pointer associated to v, if any. Return type: pcl::PointCloud<PointT>::ConstPtr
-      * - set_estimate (v,e,g)    set the estimate for an existing vertex. Return type: void.
-      * - set_measurement (d,m,g) set the measurement for an existing edge. Return type: void.
-      * Notation:
-      * - m                       an edge measurement
-      * - e                       a vertex estimate
-      * - v                       a vertex
-      * - d                       an edge
-      * - g                       a graph
-      * A valid graph implementation should accept at least the PoseEstimate estimate and the PoseMeasurement measurement
-      *
-      * If a specific graph implementation needs initialization and/or finalization,
-      * specialize the protected methods init() and deinit() for your graph type
-      *
-      * \author Nicola Fioraio
-      * \ingroup registration
-      */
-    template <typename GraphT>
-    class GraphHandler
-    {
-      public:
-        using Ptr = shared_ptr<GraphHandler<GraphT> >;
-        using ConstPtr = shared_ptr<const GraphHandler<GraphT> >;
-        using GraphPtr = shared_ptr<GraphT>;
-        using GraphConstPtr = shared_ptr<const GraphT>;
-
-        using Vertex = typename boost::graph_traits<GraphT>::vertex_descriptor;
-        using Edge = typename boost::graph_traits<GraphT>::edge_descriptor;
-
-        /** \brief Empty constructor. */
-        GraphHandler () : graph_impl_ (new GraphT ())
-        {
-          if (!init ())
-            throw InitFailedException ("Graph Initialization Failed", __FILE__, "pcl::registration::GraphHandler::GraphHandler ()", __LINE__);
-        }
-
-        /** \brief Destructor. */
-        ~GraphHandler ()
-        {
-          deinit ();
-        }
-
-        /** \brief Clear the graph */
-        inline void
-        clear ()
-        {
-          deinit ();
-          graph_impl_.reset (new GraphT ());
-          if (!init ())
-            throw InitFailedException ("Graph Initialization Failed", __FILE__, "pcl::registration::GraphHandler::clear ()", __LINE__);
-        }
-
-        /** \brief Get a pointer to the BGL graph */
-        inline GraphConstPtr
-        getGraph () const
-        {
-          return graph_impl_;
-        }
-
-        /** \brief Get a pointer to the BGL graph */
-        inline GraphPtr
-        getGraph ()
-        {
-          return graph_impl_;
-        }
-
-        /** \brief Add a new point cloud to the graph and return the new vertex
-          * \param cloud the new point cloud
-          * \param pose the pose estimate
-          * \return a reference to the new vertex
-          */
-        template <class PointT> inline Vertex
-        addPointCloud (const typename pcl::PointCloud<PointT>::ConstPtr& cloud, const Eigen::Matrix4f& pose)
-        {
-          return add_vertex (PoseEstimate<PointT> (cloud, pose), *graph_impl_);
-        }
-
-        /** \brief Add a new generic vertex created according to the given estimate
-          * \param estimate the parameters' estimate
-          * \return a reference to the new vertex
-          */
-        template <class EstimateT> inline Vertex
-        addGenericVertex (const EstimateT& estimate)
-        {
-          return add_vertex (estimate, *graph_impl_);
-        }
-
-        /** \brief Add a new constraint between two poses
-          * \param v_start the first pose
-          * \param v_end the second pose
-          * \param relative_transformation the transformation from v_start to v_end
-          * \param information_matrix the uncertainty
-          * \return a reference to the new edge
-          */
-        template <class InformationT> inline Edge
-        addPoseConstraint ( const Vertex& v_start, const Vertex& v_end,
-                            const Eigen::Matrix4f& relative_transformation,
-                            const InformationT& information_matrix)
-        {
-          return add_edge ( PoseMeasurement<Vertex, InformationT> (v_start, v_end, relative_transformation, information_matrix),
-                            *graph_impl_);
-        }
-
-        /** \brief Add a generic constraint created according to the given measurement
-          * \param measurement the measurement
-          * \return a reference to the new edge
-          */
-        template <class MeasurementT> inline Edge
-        addGenericConstraint (const MeasurementT& measurement)
-        {
-          return add_edge (measurement, *graph_impl_);
-        }
-
-        /** \brief Remove a vertex from the graph
-          * \param v the vertex
-          */
-        inline void
-        removeVertex (const Vertex& v)
-        {
-          remove_vertex (v.v_, *graph_impl_);
-        }
-
-        /** \brief Remove a constraint from the graph
-          * \param e the edge
-          */
-        inline void
-        removeConstraint (const Edge& e)
-        {
-          remove_edge(e.e_, *graph_impl_);
-        }
-
-      protected:
-        /** \brief This method is called right after the creation of graph_impl_ */
-        inline bool
-        init ()
-        {
-          return true;
-        }
-
-        /** \brief This method is called when graph_impl_ is going to be destroyed */
-        inline bool
-        deinit ()
-        {
-          return true;
-        }
-
-      private:
-        GraphPtr graph_impl_;
-    };
+    remove_vertex(v.v_, *graph_impl_);
   }
-}
+
+  /** \brief Remove a constraint from the graph
+   * \param e the edge
+   */
+  inline void
+  removeConstraint(const Edge& e)
+  {
+    remove_edge(e.e_, *graph_impl_);
+  }
+
+protected:
+  /** \brief This method is called right after the creation of graph_impl_ */
+  inline bool
+  init()
+  {
+    return true;
+  }
+
+  /** \brief This method is called when graph_impl_ is going to be destroyed */
+  inline bool
+  deinit()
+  {
+    return true;
+  }
+
+private:
+  GraphPtr graph_impl_;
+};
+} // namespace registration
+} // namespace pcl
diff --git a/registration/include/pcl/registration/graph_optimizer.h b/registration/include/pcl/registration/graph_optimizer.h
index eee576d2..97a5494f 100644
--- a/registration/include/pcl/registration/graph_optimizer.h
+++ b/registration/include/pcl/registration/graph_optimizer.h
@@ -42,27 +42,24 @@
 
 #include <pcl/registration/graph_handler.h>
 
-namespace pcl
-{
-  namespace registration
-  {
-    /** \brief @b GraphOptimizer class; derive and specialize for each graph type
-      * \author Nicola Fioraio
-      * \ingroup registration
-      */
-    template <typename GraphT>
-    class GraphOptimizer
-    {
-      public:
-        /** \brief Optimize the given graph
-          * \param inout_graph the graph
-          * \return false if the optimization has not been performed
-          */
-        virtual inline bool
-        optimize(GraphHandler<GraphT>& inout_graph) = 0;
-      
-      /** \brief Empty destructor */
-      virtual ~GraphOptimizer () {}
-    };
-  }
-}
+namespace pcl {
+namespace registration {
+/** \brief @b GraphOptimizer class; derive and specialize for each graph type
+ * \author Nicola Fioraio
+ * \ingroup registration
+ */
+template <typename GraphT>
+class GraphOptimizer {
+public:
+  /** \brief Optimize the given graph
+   * \param inout_graph the graph
+   * \return false if the optimization has not been performed
+   */
+  virtual inline bool
+  optimize(GraphHandler<GraphT>& inout_graph) = 0;
+
+  /** \brief Empty destructor */
+  virtual ~GraphOptimizer() {}
+};
+} // namespace registration
+} // namespace pcl
diff --git a/registration/include/pcl/registration/graph_registration.h b/registration/include/pcl/registration/graph_registration.h
index 3f6ba8b9..0f302a53 100644
--- a/registration/include/pcl/registration/graph_registration.h
+++ b/registration/include/pcl/registration/graph_registration.h
@@ -40,85 +40,86 @@
 
 #pragma once
 
-#include <pcl/point_cloud.h>
 #include <pcl/registration/graph_handler.h>
+#include <pcl/point_cloud.h>
 
-namespace pcl
-{
-  /** \brief @b GraphRegistration class is the base class for graph-based registration methods
-    * \author Nicola Fioraio
-    * \ingroup registration
-    */
-  template <typename GraphT>
-  class GraphRegistration
-  {
-    public:
-      using GraphHandler = pcl::registration::GraphHandler<GraphT>;
-      using GraphHandlerPtr = typename pcl::registration::GraphHandler<GraphT>::Ptr;
-      using GraphHandlerConstPtr = typename pcl::registration::GraphHandler<GraphT>::ConstPtr;
-      using GraphHandlerVertex = typename pcl::registration::GraphHandler<GraphT>::Vertex;
+namespace pcl {
+/** \brief @b GraphRegistration class is the base class for graph-based registration
+ * methods \author Nicola Fioraio \ingroup registration
+ */
+template <typename GraphT>
+class GraphRegistration {
+public:
+  using GraphHandler = pcl::registration::GraphHandler<GraphT>;
+  using GraphHandlerPtr = typename pcl::registration::GraphHandler<GraphT>::Ptr;
+  using GraphHandlerConstPtr =
+      typename pcl::registration::GraphHandler<GraphT>::ConstPtr;
+  using GraphHandlerVertex = typename pcl::registration::GraphHandler<GraphT>::Vertex;
+
+  /** \brief Empty constructor */
+  GraphRegistration()
+  : graph_handler_(new GraphHandler)
+  , last_aligned_vertex_(boost::graph_traits<GraphT>::null_vertex())
+  , last_vertices_()
+  {}
 
-      /** \brief Empty constructor */
-      GraphRegistration ()  : graph_handler_ (new GraphHandler),
-                              last_aligned_vertex_ (boost::graph_traits<GraphT>::null_vertex ()),
-                              last_vertices_ ()
-      {}
-      
-      /** \brief Empty destructor */
-      virtual ~GraphRegistration () {}
+  /** \brief Empty destructor */
+  virtual ~GraphRegistration() {}
 
-      /** \brief Add a point cloud and the associated camera pose to the graph */
-      template <typename PointT> inline void
-      addPointCloud (const typename pcl::PointCloud<PointT>::ConstPtr& cloud, const Eigen::Matrix4f& pose)
-      {
-        last_vertices_.push_back (graph_handler_->addPointCloud (cloud, pose));
-      }
+  /** \brief Add a point cloud and the associated camera pose to the graph */
+  template <typename PointT>
+  inline void
+  addPointCloud(const typename pcl::PointCloud<PointT>::ConstPtr& cloud,
+                const Eigen::Matrix4f& pose)
+  {
+    last_vertices_.push_back(graph_handler_->addPointCloud(cloud, pose));
+  }
 
-      /** \brief Set the graph handler */
-      inline void
-      setGraphHandler (GraphHandlerPtr& gh)
-      {
-        graph_handler_ = gh;
-      }
+  /** \brief Set the graph handler */
+  inline void
+  setGraphHandler(GraphHandlerPtr& gh)
+  {
+    graph_handler_ = gh;
+  }
 
-      /** \brief Get a pointer to the graph handler */
-      inline GraphHandlerPtr
-      getGraphHandler ()
-      {
-        return graph_handler_;
-      }
+  /** \brief Get a pointer to the graph handler */
+  inline GraphHandlerPtr
+  getGraphHandler()
+  {
+    return graph_handler_;
+  }
 
-      /** \brief Get a pointer to the graph handler */
-      inline GraphHandlerConstPtr
-      getGraphHandler () const
-      {
-        return graph_handler_;
-      }
+  /** \brief Get a pointer to the graph handler */
+  inline GraphHandlerConstPtr
+  getGraphHandler() const
+  {
+    return graph_handler_;
+  }
 
-      /** \brief Check if new poses have been added, then call the registration
-        * method which is implemented by the subclasses
-        */
-      inline void
-      compute ()
-      {
-        if (last_vertices_.empty ())
-          return;
-        computeRegistration ();
-        last_aligned_vertex_ = last_vertices_.back ();
-        last_vertices_.clear ();
-      }
+  /** \brief Check if new poses have been added, then call the registration
+   * method which is implemented by the subclasses
+   */
+  inline void
+  compute()
+  {
+    if (last_vertices_.empty())
+      return;
+    computeRegistration();
+    last_aligned_vertex_ = last_vertices_.back();
+    last_vertices_.clear();
+  }
 
-    protected:
-      /** \brief The graph handler */
-      GraphHandlerPtr graph_handler_;
-      /** \brief The last estimated pose */
-      GraphHandlerVertex last_aligned_vertex_;
-      /** \brief The vertices added to the graph since the last call to compute */
-      std::vector<GraphHandlerVertex> last_vertices_;
+protected:
+  /** \brief The graph handler */
+  GraphHandlerPtr graph_handler_;
+  /** \brief The last estimated pose */
+  GraphHandlerVertex last_aligned_vertex_;
+  /** \brief The vertices added to the graph since the last call to compute */
+  std::vector<GraphHandlerVertex> last_vertices_;
 
-    private:
-      /** \brief The registration method */
-      virtual void
-      computeRegistration () = 0;
-  };
-}
+private:
+  /** \brief The registration method */
+  virtual void
+  computeRegistration() = 0;
+};
+} // namespace pcl
diff --git a/registration/include/pcl/registration/ia_fpcs.h b/registration/include/pcl/registration/ia_fpcs.h
index 4a24ffda..171a98d7 100644
--- a/registration/include/pcl/registration/ia_fpcs.h
+++ b/registration/include/pcl/registration/ia_fpcs.h
@@ -20,7 +20,7 @@
  *     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
+ *  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
@@ -38,530 +38,523 @@
 #pragma once
 
 #include <pcl/common/common.h>
-#include <pcl/registration/registration.h>
 #include <pcl/registration/matching_candidate.h>
+#include <pcl/registration/registration.h>
 
-namespace pcl
-{
-  /** \brief Compute the mean point density of a given point cloud.
-    * \param[in] cloud pointer to the input point cloud
-    * \param[in] max_dist maximum distance of a point to be considered as a neighbor
-    * \param[in] nr_threads number of threads to use (default = 1, only used if OpenMP flag is set)
-    * \return the mean point density of a given point cloud
-    */
-  template <typename PointT> inline float
-  getMeanPointDensity (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, float max_dist, int nr_threads = 1);
-
-  /** \brief Compute the mean point density of a given point cloud.
-    * \param[in] cloud pointer to the input point cloud
-    * \param[in] indices the vector of point indices to use from \a cloud
-    * \param[in] max_dist maximum distance of a point to be considered as a neighbor
-    * \param[in] nr_threads number of threads to use (default = 1, only used if OpenMP flag is set)
-    * \return the mean point density of a given point cloud
-    */
-  template <typename PointT> inline float
-  getMeanPointDensity (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, const std::vector <int> &indices,
-    float max_dist, int nr_threads = 1);
-  
-  
-  namespace registration
+namespace pcl {
+/** \brief Compute the mean point density of a given point cloud.
+ * \param[in] cloud pointer to the input point cloud
+ * \param[in] max_dist maximum distance of a point to be considered as a neighbor
+ * \param[in] nr_threads number of threads to use (default = 1, only used if OpenMP flag
+ * is set) \return the mean point density of a given point cloud
+ */
+template <typename PointT>
+inline float
+getMeanPointDensity(const typename pcl::PointCloud<PointT>::ConstPtr& cloud,
+                    float max_dist,
+                    int nr_threads = 1);
+
+/** \brief Compute the mean point density of a given point cloud.
+ * \param[in] cloud pointer to the input point cloud
+ * \param[in] indices the vector of point indices to use from \a cloud
+ * \param[in] max_dist maximum distance of a point to be considered as a neighbor
+ * \param[in] nr_threads number of threads to use (default = 1, only used if OpenMP flag
+ * is set) \return the mean point density of a given point cloud
+ */
+template <typename PointT>
+inline float
+getMeanPointDensity(const typename pcl::PointCloud<PointT>::ConstPtr& cloud,
+                    const pcl::Indices& indices,
+                    float max_dist,
+                    int nr_threads = 1);
+
+namespace registration {
+/** \brief FPCSInitialAlignment computes corresponding four point congruent sets as
+ * described in: "4-points congruent sets for robust pairwise surface registration",
+ * Dror Aiger, Niloy Mitra, Daniel Cohen-Or. ACM Transactions on Graphics, vol. 27(3),
+ * 2008 \author P.W.Theiler \ingroup registration
+ */
+template <typename PointSource,
+          typename PointTarget,
+          typename NormalT = pcl::Normal,
+          typename Scalar = float>
+class FPCSInitialAlignment : public Registration<PointSource, PointTarget, Scalar> {
+public:
+  /** \cond */
+  using Ptr =
+      shared_ptr<FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>>;
+  using ConstPtr =
+      shared_ptr<const FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>>;
+
+  using KdTreeReciprocal = pcl::search::KdTree<PointSource>;
+  using KdTreeReciprocalPtr = typename KdTreeReciprocal::Ptr;
+
+  using PointCloudTarget = pcl::PointCloud<PointTarget>;
+  using PointCloudSource = pcl::PointCloud<PointSource>;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceIterator = typename PointCloudSource::iterator;
+
+  using Normals = pcl::PointCloud<NormalT>;
+  using NormalsConstPtr = typename Normals::ConstPtr;
+
+  using MatchingCandidate = pcl::registration::MatchingCandidate;
+  using MatchingCandidates = pcl::registration::MatchingCandidates;
+  /** \endcond */
+
+  /** \brief Constructor.
+   * Resets the maximum number of iterations to 0 thus forcing an internal computation
+   * if not set by the user. Sets the number of RANSAC iterations to 1000 and the
+   * standard transformation estimation to TransformationEstimation3Point.
+   */
+  FPCSInitialAlignment();
+
+  /** \brief Destructor. */
+  ~FPCSInitialAlignment(){};
+
+  /** \brief Provide a pointer to the vector of target indices.
+   * \param[in] target_indices a pointer to the target indices
+   */
+  inline void
+  setTargetIndices(const IndicesPtr& target_indices)
   {
-    /** \brief FPCSInitialAlignment computes corresponding four point congruent sets as described in:
-    * "4-points congruent sets for robust pairwise surface registration", Dror Aiger, Niloy Mitra, Daniel Cohen-Or.
-    * ACM Transactions on Graphics, vol. 27(3), 2008
-    * \author P.W.Theiler
-    * \ingroup registration
-    */
-    template <typename PointSource, typename PointTarget, typename NormalT = pcl::Normal, typename Scalar = float>
-    class FPCSInitialAlignment : public Registration <PointSource, PointTarget, Scalar>
-    {
-    public:
-      /** \cond */
-      using Ptr = shared_ptr <FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar> >;
-      using ConstPtr = shared_ptr <const FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar> >;
-
-      using KdTreeReciprocal = pcl::search::KdTree<PointSource>;
-      using KdTreeReciprocalPtr = typename KdTreeReciprocal::Ptr;
-
-      using PointCloudTarget = pcl::PointCloud<PointTarget>;
-      using PointCloudSource = pcl::PointCloud<PointSource>;
-      using PointCloudSourcePtr = typename PointCloudSource::Ptr;
-      using PointCloudSourceIterator = typename PointCloudSource::iterator;      
-
-      using Normals = pcl::PointCloud<NormalT>;
-      using NormalsConstPtr = typename Normals::ConstPtr;
-
-      using MatchingCandidate = pcl::registration::MatchingCandidate;
-      using MatchingCandidates = pcl::registration::MatchingCandidates;
-      /** \endcond */
-
-
-      /** \brief Constructor.
-        * Resets the maximum number of iterations to 0 thus forcing an internal computation if not set by the user.
-        * Sets the number of RANSAC iterations to 1000 and the standard transformation estimation to TransformationEstimation3Point.
-        */
-      FPCSInitialAlignment ();
-
-      /** \brief Destructor. */
-      ~FPCSInitialAlignment ()
-      {};
-
-
-      /** \brief Provide a pointer to the vector of target indices.
-        * \param[in] target_indices a pointer to the target indices
-        */
-      inline void
-      setTargetIndices (const IndicesPtr &target_indices)
-      {
-        target_indices_ = target_indices;
-      };
-
-      /** \return a pointer to the vector of target indices. */
-      inline IndicesPtr
-      getTargetIndices () const
-      {
-        return (target_indices_);
-      };
-
-
-      /** \brief Provide a pointer to the normals of the source point cloud.
-        * \param[in] source_normals pointer to the normals of the source pointer cloud.
-        */
-      inline void
-      setSourceNormals (const NormalsConstPtr &source_normals)
-      {
-        source_normals_ = source_normals;
-      };
-
-      /** \return the normals of the source point cloud. */
-      inline NormalsConstPtr
-      getSourceNormals () const
-      {
-        return (source_normals_);
-      };
-
-
-      /** \brief Provide a pointer to the normals of the target point cloud.
-        * \param[in] target_normals point to the normals of the target point cloud.
-        */
-      inline void
-      setTargetNormals (const NormalsConstPtr &target_normals)
-      {
-        target_normals_ = target_normals;
-      };
-
-      /** \return the normals of the target point cloud. */
-      inline NormalsConstPtr
-      getTargetNormals () const
-      {
-        return (target_normals_);
-      };
-
-
-      /** \brief Set the number of used threads if OpenMP is activated.
-        * \param[in] nr_threads the number of used threads
-        */
-      inline void
-      setNumberOfThreads (int nr_threads)
-      {
-        nr_threads_ = nr_threads;
-      };
-
-      /** \return the number of threads used if OpenMP is activated. */
-      inline int
-      getNumberOfThreads () const
-      {
-        return (nr_threads_);
-      };
-
-
-      /** \brief Set the constant factor delta which weights the internally calculated parameters.
-        * \param[in] delta the weight factor delta
-        * \param[in] normalize flag if delta should be normalized according to point cloud density
-        */
-      inline void
-      setDelta (float delta, bool normalize = false)
-      {
-        delta_ = delta;
-        normalize_delta_ = normalize;
-      };
-
-      /** \return the constant factor delta which weights the internally calculated parameters. */
-      inline float
-      getDelta () const
-      {
-        return (delta_);
-      };
-
-
-      /** \brief Set the approximate overlap between source and target.
-        * \param[in] approx_overlap the estimated overlap
-        */
-      inline void
-      setApproxOverlap (float approx_overlap)
-      {
-        approx_overlap_ = approx_overlap;
-      };
-
-      /** \return the approximated overlap between source and target. */
-      inline float
-      getApproxOverlap () const
-      {
-        return (approx_overlap_);
-      };
-
-
-      /** \brief Set the scoring threshold used for early finishing the method.
-        * \param[in] score_threshold early terminating score criteria
-        */
-      inline void
-      setScoreThreshold (float score_threshold)
-      {
-        score_threshold_ = score_threshold;
-      };
-
-      /** \return the scoring threshold used for early finishing the method. */
-      inline float
-      getScoreThreshold () const
-      {
-        return (score_threshold_);
-      };
-
-
-      /** \brief Set the number of source samples to use during alignment.
-        * \param[in] nr_samples the number of source samples
-        */
-      inline void
-      setNumberOfSamples (int nr_samples)
-      {
-        nr_samples_ = nr_samples;
-      };
-
-      /** \return the number of source samples to use during alignment. */
-      inline int
-      getNumberOfSamples () const
-      {
-        return (nr_samples_);
-      };
-
-
-      /** \brief Set the maximum normal difference between valid point correspondences in degree.
-        * \param[in] max_norm_diff the maximum difference in degree
-        */
-      inline void
-      setMaxNormalDifference (float max_norm_diff)
-      {
-        max_norm_diff_ = max_norm_diff;
-      };
-
-      /** \return the maximum normal difference between valid point correspondences in degree. */
-      inline float
-      getMaxNormalDifference () const
-      {
-        return (max_norm_diff_);
-      };
-
-
-      /** \brief Set the maximum computation time in seconds.
-        * \param[in] max_runtime the maximum runtime of the method in seconds
-        */
-      inline void
-      setMaxComputationTime (int max_runtime)
-      {
-        max_runtime_ = max_runtime;
-      };
-
-      /** \return the maximum computation time in seconds. */
-      inline int
-      getMaxComputationTime () const
-      {
-        return (max_runtime_);
-      };
-
-
-      /** \return the fitness score of the best scored four-point match. */
-      inline float
-      getFitnessScore () const
-      {
-        return (fitness_score_);
-      };
-
-    protected:
-
-      using PCLBase <PointSource>::deinitCompute;
-      using PCLBase <PointSource>::input_;
-      using PCLBase <PointSource>::indices_;
-
-      using Registration <PointSource, PointTarget, Scalar>::reg_name_;
-      using Registration <PointSource, PointTarget, Scalar>::target_;
-      using Registration <PointSource, PointTarget, Scalar>::tree_;
-      using Registration <PointSource, PointTarget, Scalar>::correspondences_;
-      using Registration <PointSource, PointTarget, Scalar>::target_cloud_updated_;
-      using Registration <PointSource, PointTarget, Scalar>::final_transformation_;
-      using Registration <PointSource, PointTarget, Scalar>::max_iterations_;
-      using Registration <PointSource, PointTarget, Scalar>::ransac_iterations_;
-      using Registration <PointSource, PointTarget, Scalar>::transformation_estimation_;
-      using Registration <PointSource, PointTarget, Scalar>::converged_;
-
-
-      /** \brief Rigid transformation computation method.
-        * \param output the transformed input point cloud dataset using the rigid transformation found
-        * \param guess The computed transforamtion
-        */
-      void
-      computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) override;
-
-
-      /** \brief Internal computation initialization. */
-      virtual bool
-      initCompute ();
-
-      /** \brief Select an approximately coplanar set of four points from the source cloud.
-        * \param[out] base_indices selected source cloud indices, further used as base (B)
-        * \param[out] ratio the two diagonal intersection ratios (r1,r2) of the base points
-        * \return
-        * * < 0 no coplanar four point sets with large enough sampling distance was found
-        * * = 0 a set of four congruent points was selected
-        */
-      int
-      selectBase (std::vector <int> &base_indices, float (&ratio)[2]);
-
-      /** \brief Select randomly a triplet of points with large point-to-point distances. The minimum point
-        * sampling distance is calculated based on the estimated point cloud overlap during initialization.
-        *
-        * \param[out] base_indices indices of base B
-        * \return
-        * * < 0 no triangle with large enough base lines could be selected
-        * * = 0 base triangle succesully selected
-        */
-      int
-      selectBaseTriangle (std::vector <int> &base_indices);
-
-      /** \brief Setup the base (four coplanar points) by ordering the points and computing intersection
-        * ratios and segment to segment distances of base diagonal.
-        *
-        * \param[in,out] base_indices indices of base B (will be reordered)
-        * \param[out] ratio diagonal intersection ratios of base points
-        */
-      void
-      setupBase (std::vector <int> &base_indices, float (&ratio)[2]);
-
-      /** \brief Calculate intersection ratios and segment to segment distances of base diagonals.
-        * \param[in] base_indices indices of base B
-        * \param[out] ratio diagonal intersection ratios of base points
-        * \return quality value of diagonal intersection
-        */
-      float
-      segmentToSegmentDist (const std::vector <int> &base_indices, float (&ratio)[2]);
-
-      /** \brief Search for corresponding point pairs given the distance between two base points.
-        *
-        * \param[in] idx1 first index of current base segment (in source cloud)
-        * \param[in] idx2 second index of current base segment (in source cloud)
-        * \param[out] pairs resulting point pairs with point-to-point distance close to ref_dist
-        * \return
-        * * < 0 no corresponding point pair was found
-        * * = 0 at least one point pair candidate was found
-        */
-      virtual int
-      bruteForceCorrespondences (int idx1, int idx2, pcl::Correspondences &pairs);
-
-      /** \brief Determine base matches by combining the point pair candidate and search for coinciding
-        * intersection points using the diagonal segment ratios of base B. The coincidation threshold is
-        * calculated during initialization (coincidation_limit_).
-        *
-        * \param[in] base_indices indices of base B
-        * \param[out] matches vector of candidate matches w.r.t the base B
-        * \param[in] pairs_a point pairs corresponding to points of 1st diagonal of base B
-        * \param[in] pairs_b point pairs corresponding to points of 2nd diagonal of base B
-        * \param[in] ratio diagonal intersection ratios of base points
-        * \return
-        * * < 0 no base match could be found
-        * * = 0 at least one base match was found
-        */
-      virtual int
-      determineBaseMatches (
-        const std::vector <int> &base_indices,
-        std::vector <std::vector <int> > &matches,
-        const pcl::Correspondences &pairs_a,
-        const pcl::Correspondences &pairs_b,
-        const float (&ratio)[2]);
-
-      /** \brief Check if outer rectangle distance of matched points fit with the base rectangle.
-        *
-        * \param[in] match_indices indices of match M
-        * \param[in] ds edge lengths of base B
-        * \return
-        * * < 0 at least one edge of the match M has no corresponding one in the base B
-        * * = 0 edges of match M fits to the ones of base B
-        */
-      int
-      checkBaseMatch (const std::vector <int> &match_indices, const float (&ds)[4]);
-
-      /** \brief Method to handle current candidate matches. Here we validate and evaluate the matches w.r.t the
-        * base and store the best fitting match (together with its score and estimated transformation).
-        * \note For forwards compatibility the results are stored in 'vectors of size 1'.
-        *
-        * \param[in] base_indices indices of base B
-        * \param[in,out] matches vector of candidate matches w.r.t the base B. The candidate matches are 
-        * reordered during this step.
-        * \param[out] candidates vector which contains the candidates matches M
-        */
-      virtual void
-      handleMatches (
-        const std::vector <int> &base_indices,
-        std::vector <std::vector <int> > &matches,
-        MatchingCandidates &candidates);
-
-      /** \brief Sets the correspondences between the base B and the match M by using the distance of each point
-        * to the centroid of the rectangle.
-        *
-        * \param[in] base_indices indices of base B
-        * \param[in] match_indices indices of match M
-        * \param[out] correspondences resulting correspondences
-        */
-      virtual void
-      linkMatchWithBase (
-        const std::vector <int> &base_indices,
-        std::vector <int> &match_indices,
-        pcl::Correspondences &correspondences);
-
-      /** \brief Validate the matching by computing the transformation between the source and target based on the
-        * four matched points and by comparing the mean square error (MSE) to a threshold. The MSE limit was
-        * calculated during initialization (max_mse_).
-        *
-        * \param[in] base_indices indices of base B
-        * \param[in] match_indices indices of match M
-        * \param[in] correspondences corresondences between source and target
-        * \param[out] transformation resulting transformation matrix
-        * \return
-        * * < 0 MSE bigger than max_mse_
-        * * = 0 MSE smaller than max_mse_
-        */
-      virtual int
-      validateMatch (
-        const std::vector <int> &base_indices,
-        const std::vector <int> &match_indices,
-        const pcl::Correspondences &correspondences,
-        Eigen::Matrix4f &transformation);
-
-      /** \brief Validate the transformation by calculating the number of inliers after transforming the source cloud.
-        * The resulting fitness score is later used as the decision criteria of the best fitting match.
-        *
-        * \param[out] transformation updated orientation matrix using all inliers
-        * \param[out] fitness_score current best fitness_score
-        * \note fitness score is only updated if the score of the current transformation exceeds the input one.
-        * \return
-        * * < 0 if previous result is better than the current one (score remains)
-        * * = 0 current result is better than the previous one (score updated)
-        */
-      virtual int
-      validateTransformation (Eigen::Matrix4f &transformation, float &fitness_score);
-
-      /** \brief Final computation of best match out of vector of best matches. To avoid cross thread dependencies
-        *  during parallel running, a best match for each try was calculated.
-        * \note For forwards compatibility the candidates are stored in vectors of 'vectors of size 1'.
-        * \param[in] candidates vector of candidate matches
-        */
-      virtual void
-      finalCompute (const std::vector <MatchingCandidates > &candidates);
-
-
-      /** \brief Normals of source point cloud. */
-      NormalsConstPtr source_normals_;
-
-      /** \brief Normals of target point cloud. */
-      NormalsConstPtr target_normals_;
-
-
-      /** \brief Number of threads for parallelization (standard = 1).
-        * \note Only used if run compiled with OpenMP.
-        */
-      int nr_threads_;
-
-      /** \brief Estimated overlap between source and target (standard = 0.5). */
-      float approx_overlap_;
-
-      /** \brief Delta value of 4pcs algorithm (standard = 1.0).
-        * It can be used as:
-        * * absolute value (normalization = false), value should represent the point accuracy to ensure finding neighbors between source <-> target
-        * * relative value (normalization = true), to adjust the internally calculated point accuracy (= point density)
-        */
-      float delta_;
-
-      /** \brief Score threshold to stop calculation with success.
-        * If not set by the user it is equal to the approximated overlap
-        */
-      float score_threshold_;
-
-      /** \brief The number of points to uniformly sample the source point cloud. (standard = 0 => full cloud). */
-      int nr_samples_;
-
-      /** \brief Maximum normal difference of corresponding point pairs in degrees (standard = 90). */
-      float max_norm_diff_;
-
-      /** \brief Maximum allowed computation time in seconds (standard = 0 => ~unlimited). */
-      int max_runtime_;
-
-
-      /** \brief Resulting fitness score of the best match. */
-      float fitness_score_;
-      
-
-      /** \brief Estimated diamter of the target point cloud. */
-      float diameter_;
-
-      /** \brief Estimated squared metric overlap between source and target.
-        * \note Internally calculated using the estimated overlap and the extent of the source cloud.
-        * It is used to derive the minimum sampling distance of the base points as well as to calculated
-        * the number of tries to reliably find a correct match.
-        */
-      float max_base_diameter_sqr_;
-
-      /** \brief Use normals flag. */
-      bool use_normals_;
-
-      /** \brief Normalize delta flag. */
-      bool normalize_delta_;
-
-
-      /** \brief A pointer to the vector of source point indices to use after sampling. */
-      pcl::IndicesPtr source_indices_;
-
-      /** \brief A pointer to the vector of target point indices to use after sampling. */
-      pcl::IndicesPtr target_indices_;
-
-      /** \brief Maximal difference between corresponding point pairs in source and target.
-        * \note Internally calculated using an estimation of the point density.
-        */
-      float max_pair_diff_;
-
-      /** \brief Maximal difference between the length of the base edges and valid match edges.
-        * \note Internally calculated using an estimation of the point density.
-        */
-      float max_edge_diff_;
+    target_indices_ = target_indices;
+  };
 
-      /** \brief Maximal distance between coinciding intersection points to find valid matches.
-        * \note Internally calculated using an estimation of the point density.
-        */
-      float coincidation_limit_;
+  /** \return a pointer to the vector of target indices. */
+  inline IndicesPtr
+  getTargetIndices() const
+  {
+    return (target_indices_);
+  };
+
+  /** \brief Provide a pointer to the normals of the source point cloud.
+   * \param[in] source_normals pointer to the normals of the source pointer cloud.
+   */
+  inline void
+  setSourceNormals(const NormalsConstPtr& source_normals)
+  {
+    source_normals_ = source_normals;
+  };
 
-      /** \brief Maximal mean squared errors of a transformation calculated from a candidate match.
-        * \note Internally calculated using an estimation of the point density.
-        */
-      float max_mse_;
+  /** \return the normals of the source point cloud. */
+  inline NormalsConstPtr
+  getSourceNormals() const
+  {
+    return (source_normals_);
+  };
+
+  /** \brief Provide a pointer to the normals of the target point cloud.
+   * \param[in] target_normals point to the normals of the target point cloud.
+   */
+  inline void
+  setTargetNormals(const NormalsConstPtr& target_normals)
+  {
+    target_normals_ = target_normals;
+  };
 
-      /** \brief Maximal squared point distance between source and target points to count as inlier.
-        * \note Internally calculated using an estimation of the point density.
-        */
-      float max_inlier_dist_sqr_;
+  /** \return the normals of the target point cloud. */
+  inline NormalsConstPtr
+  getTargetNormals() const
+  {
+    return (target_normals_);
+  };
+
+  /** \brief Set the number of used threads if OpenMP is activated.
+   * \param[in] nr_threads the number of used threads
+   */
+  inline void
+  setNumberOfThreads(int nr_threads)
+  {
+    nr_threads_ = nr_threads;
+  };
 
+  /** \return the number of threads used if OpenMP is activated. */
+  inline int
+  getNumberOfThreads() const
+  {
+    return (nr_threads_);
+  };
+
+  /** \brief Set the constant factor delta which weights the internally calculated
+   * parameters. \param[in] delta the weight factor delta \param[in] normalize flag if
+   * delta should be normalized according to point cloud density
+   */
+  inline void
+  setDelta(float delta, bool normalize = false)
+  {
+    delta_ = delta;
+    normalize_delta_ = normalize;
+  };
+
+  /** \return the constant factor delta which weights the internally calculated
+   * parameters. */
+  inline float
+  getDelta() const
+  {
+    return (delta_);
+  };
+
+  /** \brief Set the approximate overlap between source and target.
+   * \param[in] approx_overlap the estimated overlap
+   */
+  inline void
+  setApproxOverlap(float approx_overlap)
+  {
+    approx_overlap_ = approx_overlap;
+  };
 
-      /** \brief Definition of a small error. */
-      const float small_error_;
+  /** \return the approximated overlap between source and target. */
+  inline float
+  getApproxOverlap() const
+  {
+    return (approx_overlap_);
+  };
+
+  /** \brief Set the scoring threshold used for early finishing the method.
+   * \param[in] score_threshold early terminating score criteria
+   */
+  inline void
+  setScoreThreshold(float score_threshold)
+  {
+    score_threshold_ = score_threshold;
+  };
+
+  /** \return the scoring threshold used for early finishing the method. */
+  inline float
+  getScoreThreshold() const
+  {
+    return (score_threshold_);
+  };
+
+  /** \brief Set the number of source samples to use during alignment.
+   * \param[in] nr_samples the number of source samples
+   */
+  inline void
+  setNumberOfSamples(int nr_samples)
+  {
+    nr_samples_ = nr_samples;
+  };
+
+  /** \return the number of source samples to use during alignment. */
+  inline int
+  getNumberOfSamples() const
+  {
+    return (nr_samples_);
+  };
+
+  /** \brief Set the maximum normal difference between valid point correspondences in
+   * degree. \param[in] max_norm_diff the maximum difference in degree
+   */
+  inline void
+  setMaxNormalDifference(float max_norm_diff)
+  {
+    max_norm_diff_ = max_norm_diff;
+  };
+
+  /** \return the maximum normal difference between valid point correspondences in
+   * degree. */
+  inline float
+  getMaxNormalDifference() const
+  {
+    return (max_norm_diff_);
+  };
+
+  /** \brief Set the maximum computation time in seconds.
+   * \param[in] max_runtime the maximum runtime of the method in seconds
+   */
+  inline void
+  setMaxComputationTime(int max_runtime)
+  {
+    max_runtime_ = max_runtime;
+  };
 
-    };
-  }; // namespace registration  
-}; // namespace pcl 
+  /** \return the maximum computation time in seconds. */
+  inline int
+  getMaxComputationTime() const
+  {
+    return (max_runtime_);
+  };
+
+  /** \return the fitness score of the best scored four-point match. */
+  inline float
+  getFitnessScore() const
+  {
+    return (fitness_score_);
+  };
+
+protected:
+  using PCLBase<PointSource>::deinitCompute;
+  using PCLBase<PointSource>::input_;
+  using PCLBase<PointSource>::indices_;
+
+  using Registration<PointSource, PointTarget, Scalar>::reg_name_;
+  using Registration<PointSource, PointTarget, Scalar>::target_;
+  using Registration<PointSource, PointTarget, Scalar>::tree_;
+  using Registration<PointSource, PointTarget, Scalar>::correspondences_;
+  using Registration<PointSource, PointTarget, Scalar>::target_cloud_updated_;
+  using Registration<PointSource, PointTarget, Scalar>::final_transformation_;
+  using Registration<PointSource, PointTarget, Scalar>::max_iterations_;
+  using Registration<PointSource, PointTarget, Scalar>::ransac_iterations_;
+  using Registration<PointSource, PointTarget, Scalar>::transformation_estimation_;
+  using Registration<PointSource, PointTarget, Scalar>::converged_;
+
+  /** \brief Rigid transformation computation method.
+   * \param output the transformed input point cloud dataset using the rigid
+   * transformation found \param guess The computed transforamtion
+   */
+  void
+  computeTransformation(PointCloudSource& output,
+                        const Eigen::Matrix4f& guess) override;
+
+  /** \brief Internal computation initialization. */
+  virtual bool
+  initCompute();
+
+  /** \brief Select an approximately coplanar set of four points from the source cloud.
+   * \param[out] base_indices selected source cloud indices, further used as base (B)
+   * \param[out] ratio the two diagonal intersection ratios (r1,r2) of the base points
+   * \return
+   * * < 0 no coplanar four point sets with large enough sampling distance was found
+   * * = 0 a set of four congruent points was selected
+   */
+  int
+  selectBase(pcl::Indices& base_indices, float (&ratio)[2]);
+
+  /** \brief Select randomly a triplet of points with large point-to-point distances.
+   * The minimum point sampling distance is calculated based on the estimated point
+   * cloud overlap during initialization.
+   *
+   * \param[out] base_indices indices of base B
+   * \return
+   * * < 0 no triangle with large enough base lines could be selected
+   * * = 0 base triangle succesully selected
+   */
+  int
+  selectBaseTriangle(pcl::Indices& base_indices);
+
+  /** \brief Setup the base (four coplanar points) by ordering the points and computing
+   * intersection ratios and segment to segment distances of base diagonal.
+   *
+   * \param[in,out] base_indices indices of base B (will be reordered)
+   * \param[out] ratio diagonal intersection ratios of base points
+   */
+  void
+  setupBase(pcl::Indices& base_indices, float (&ratio)[2]);
+
+  /** \brief Calculate intersection ratios and segment to segment distances of base
+   * diagonals. \param[in] base_indices indices of base B \param[out] ratio diagonal
+   * intersection ratios of base points \return quality value of diagonal intersection
+   */
+  float
+  segmentToSegmentDist(const pcl::Indices& base_indices, float (&ratio)[2]);
+
+  /** \brief Search for corresponding point pairs given the distance between two base
+   * points.
+   *
+   * \param[in] idx1 first index of current base segment (in source cloud)
+   * \param[in] idx2 second index of current base segment (in source cloud)
+   * \param[out] pairs resulting point pairs with point-to-point distance close to
+   * ref_dist \return
+   * * < 0 no corresponding point pair was found
+   * * = 0 at least one point pair candidate was found
+   */
+  virtual int
+  bruteForceCorrespondences(int idx1, int idx2, pcl::Correspondences& pairs);
+
+  /** \brief Determine base matches by combining the point pair candidate and search for
+   * coinciding intersection points using the diagonal segment ratios of base B. The
+   * coincidation threshold is calculated during initialization (coincidation_limit_).
+   *
+   * \param[in] base_indices indices of base B
+   * \param[out] matches vector of candidate matches w.r.t the base B
+   * \param[in] pairs_a point pairs corresponding to points of 1st diagonal of base B
+   * \param[in] pairs_b point pairs corresponding to points of 2nd diagonal of base B
+   * \param[in] ratio diagonal intersection ratios of base points
+   * \return
+   * * < 0 no base match could be found
+   * * = 0 at least one base match was found
+   */
+  virtual int
+  determineBaseMatches(const pcl::Indices& base_indices,
+                       std::vector<pcl::Indices>& matches,
+                       const pcl::Correspondences& pairs_a,
+                       const pcl::Correspondences& pairs_b,
+                       const float (&ratio)[2]);
+
+  /** \brief Check if outer rectangle distance of matched points fit with the base
+   * rectangle.
+   *
+   * \param[in] match_indices indices of match M
+   * \param[in] ds edge lengths of base B
+   * \return
+   * * < 0 at least one edge of the match M has no corresponding one in the base B
+   * * = 0 edges of match M fits to the ones of base B
+   */
+  int
+  checkBaseMatch(const pcl::Indices& match_indices, const float (&ds)[4]);
+
+  /** \brief Method to handle current candidate matches. Here we validate and evaluate
+   * the matches w.r.t the base and store the best fitting match (together with its
+   * score and estimated transformation). \note For forwards compatibility the results
+   * are stored in 'vectors of size 1'.
+   *
+   * \param[in] base_indices indices of base B
+   * \param[in,out] matches vector of candidate matches w.r.t the base B. The candidate
+   * matches are reordered during this step. \param[out] candidates vector which
+   * contains the candidates matches M
+   */
+  virtual void
+  handleMatches(const pcl::Indices& base_indices,
+                std::vector<pcl::Indices>& matches,
+                MatchingCandidates& candidates);
+
+  /** \brief Sets the correspondences between the base B and the match M by using the
+   * distance of each point to the centroid of the rectangle.
+   *
+   * \param[in] base_indices indices of base B
+   * \param[in] match_indices indices of match M
+   * \param[out] correspondences resulting correspondences
+   */
+  virtual void
+  linkMatchWithBase(const pcl::Indices& base_indices,
+                    pcl::Indices& match_indices,
+                    pcl::Correspondences& correspondences);
+
+  /** \brief Validate the matching by computing the transformation between the source
+   * and target based on the four matched points and by comparing the mean square error
+   * (MSE) to a threshold. The MSE limit was calculated during initialization
+   * (max_mse_).
+   *
+   * \param[in] base_indices indices of base B
+   * \param[in] match_indices indices of match M
+   * \param[in] correspondences corresondences between source and target
+   * \param[out] transformation resulting transformation matrix
+   * \return
+   * * < 0 MSE bigger than max_mse_
+   * * = 0 MSE smaller than max_mse_
+   */
+  virtual int
+  validateMatch(const pcl::Indices& base_indices,
+                const pcl::Indices& match_indices,
+                const pcl::Correspondences& correspondences,
+                Eigen::Matrix4f& transformation);
+
+  /** \brief Validate the transformation by calculating the number of inliers after
+   * transforming the source cloud. The resulting fitness score is later used as the
+   * decision criteria of the best fitting match.
+   *
+   * \param[out] transformation updated orientation matrix using all inliers
+   * \param[out] fitness_score current best fitness_score
+   * \note fitness score is only updated if the score of the current transformation
+   * exceeds the input one. \return
+   * * < 0 if previous result is better than the current one (score remains)
+   * * = 0 current result is better than the previous one (score updated)
+   */
+  virtual int
+  validateTransformation(Eigen::Matrix4f& transformation, float& fitness_score);
+
+  /** \brief Final computation of best match out of vector of best matches. To avoid
+   * cross thread dependencies during parallel running, a best match for each try was
+   * calculated. \note For forwards compatibility the candidates are stored in vectors
+   * of 'vectors of size 1'. \param[in] candidates vector of candidate matches
+   */
+  virtual void
+  finalCompute(const std::vector<MatchingCandidates>& candidates);
+
+  /** \brief Normals of source point cloud. */
+  NormalsConstPtr source_normals_;
+
+  /** \brief Normals of target point cloud. */
+  NormalsConstPtr target_normals_;
+
+  /** \brief Number of threads for parallelization (standard = 1).
+   * \note Only used if run compiled with OpenMP.
+   */
+  int nr_threads_;
+
+  /** \brief Estimated overlap between source and target (standard = 0.5). */
+  float approx_overlap_;
+
+  /** \brief Delta value of 4pcs algorithm (standard = 1.0).
+   * It can be used as:
+   * * absolute value (normalization = false), value should represent the point accuracy
+   * to ensure finding neighbors between source <-> target
+   * * relative value (normalization = true), to adjust the internally calculated point
+   * accuracy (= point density)
+   */
+  float delta_;
+
+  /** \brief Score threshold to stop calculation with success.
+   * If not set by the user it depends on the size of the approximated overlap
+   */
+  float score_threshold_;
+
+  /** \brief The number of points to uniformly sample the source point cloud. (standard
+   * = 0 => full cloud). */
+  int nr_samples_;
+
+  /** \brief Maximum normal difference of corresponding point pairs in degrees (standard
+   * = 90). */
+  float max_norm_diff_;
+
+  /** \brief Maximum allowed computation time in seconds (standard = 0 => ~unlimited).
+   */
+  int max_runtime_;
+
+  /** \brief Resulting fitness score of the best match. */
+  float fitness_score_;
+
+  /** \brief Estimated diamter of the target point cloud. */
+  float diameter_;
+
+  /** \brief Estimated squared metric overlap between source and target.
+   * \note Internally calculated using the estimated overlap and the extent of the
+   * source cloud. It is used to derive the minimum sampling distance of the base points
+   * as well as to calculated the number of tries to reliably find a correct match.
+   */
+  float max_base_diameter_sqr_;
+
+  /** \brief Use normals flag. */
+  bool use_normals_;
+
+  /** \brief Normalize delta flag. */
+  bool normalize_delta_;
+
+  /** \brief A pointer to the vector of source point indices to use after sampling. */
+  pcl::IndicesPtr source_indices_;
+
+  /** \brief A pointer to the vector of target point indices to use after sampling. */
+  pcl::IndicesPtr target_indices_;
+
+  /** \brief Maximal difference between corresponding point pairs in source and target.
+   * \note Internally calculated using an estimation of the point density.
+   */
+  float max_pair_diff_;
+
+  /** \brief Maximal difference between the length of the base edges and valid match
+   * edges. \note Internally calculated using an estimation of the point density.
+   */
+  float max_edge_diff_;
+
+  /** \brief Maximal distance between coinciding intersection points to find valid
+   * matches. \note Internally calculated using an estimation of the point density.
+   */
+  float coincidation_limit_;
+
+  /** \brief Maximal mean squared errors of a transformation calculated from a candidate
+   * match. \note Internally calculated using an estimation of the point density.
+   */
+  float max_mse_;
+
+  /** \brief Maximal squared point distance between source and target points to count as
+   * inlier. \note Internally calculated using an estimation of the point density.
+   */
+  float max_inlier_dist_sqr_;
+
+  /** \brief Definition of a small error. */
+  const float small_error_;
+};
+}; // namespace registration
+}; // namespace pcl
 
 #include <pcl/registration/impl/ia_fpcs.hpp>
diff --git a/registration/include/pcl/registration/ia_kfpcs.h b/registration/include/pcl/registration/ia_kfpcs.h
index 400e602a..8f2fb106 100644
--- a/registration/include/pcl/registration/ia_kfpcs.h
+++ b/registration/include/pcl/registration/ia_kfpcs.h
@@ -19,7 +19,7 @@
  *     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
+ *  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
@@ -38,222 +38,228 @@
 
 #include <pcl/registration/ia_fpcs.h>
 
-namespace pcl
-{
-  namespace registration
+namespace pcl {
+namespace registration {
+/** \brief KFPCSInitialAlignment computes corresponding four point congruent sets based
+ * on keypoints as described in: "Markerless point cloud registration with
+ * keypoint-based 4-points congruent sets", Pascal Theiler, Jan Dirk Wegner, Konrad
+ * Schindler. ISPRS Annals II-5/W2, 2013. Presented at ISPRS Workshop Laser Scanning,
+ * Antalya, Turkey, 2013. \note Method has since been improved and some variations to
+ * the paper exist. \author P.W.Theiler \ingroup registration
+ */
+template <typename PointSource,
+          typename PointTarget,
+          typename NormalT = pcl::Normal,
+          typename Scalar = float>
+class KFPCSInitialAlignment
+: public virtual FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar> {
+public:
+  /** \cond */
+  using Ptr =
+      shared_ptr<KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>>;
+  using ConstPtr = shared_ptr<
+      const KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>>;
+
+  using PointCloudSource = pcl::PointCloud<PointSource>;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceIterator = typename PointCloudSource::iterator;
+
+  using PointCloudTarget = pcl::PointCloud<PointTarget>;
+  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
+  using PointCloudTargetIterator = typename PointCloudTarget::iterator;
+
+  using MatchingCandidate = pcl::registration::MatchingCandidate;
+  using MatchingCandidates = pcl::registration::MatchingCandidates;
+  /** \endcond */
+
+  /** \brief Constructor. */
+  KFPCSInitialAlignment();
+
+  /** \brief Destructor. */
+  ~KFPCSInitialAlignment(){};
+
+  /** \brief Set the upper translation threshold used for score evaluation.
+   * \param[in] upper_trl_boundary upper translation threshold
+   */
+  inline void
+  setUpperTranslationThreshold(float upper_trl_boundary)
   {
-    /** \brief KFPCSInitialAlignment computes corresponding four point congruent sets based on keypoints
-      * as described in: "Markerless point cloud registration with keypoint-based 4-points congruent sets",
-      * Pascal Theiler, Jan Dirk Wegner, Konrad Schindler. ISPRS Annals II-5/W2, 2013. Presented at ISPRS Workshop
-      * Laser Scanning, Antalya, Turkey, 2013.
-      * \note Method has since been improved and some variations to the paper exist.
-      * \author P.W.Theiler
-      * \ingroup registration
-      */
-    template <typename PointSource, typename PointTarget, typename NormalT = pcl::Normal, typename Scalar = float>
-    class KFPCSInitialAlignment : public virtual FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>
-    {
-    public:
-      /** \cond */
-      using Ptr = shared_ptr <KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar> >;
-      using ConstPtr = shared_ptr <const KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar> >;
-
-      using PointCloudSource = pcl::PointCloud<PointSource>;
-      using PointCloudSourcePtr = typename PointCloudSource::Ptr;
-      using PointCloudSourceIterator = typename PointCloudSource::iterator;
-
-      using PointCloudTarget = pcl::PointCloud<PointTarget>;
-      using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
-      using PointCloudTargetIterator = typename PointCloudTarget::iterator;
-
-      using MatchingCandidate = pcl::registration::MatchingCandidate;
-      using MatchingCandidates = pcl::registration::MatchingCandidates;
-      /** \endcond */
-
-
-      /** \brief Constructor. */
-      KFPCSInitialAlignment ();
-
-      /** \brief Destructor. */
-      ~KFPCSInitialAlignment ()
-      {};
-
-
-      /** \brief Set the upper translation threshold used for score evaluation.
-        * \param[in] upper_trl_boundary upper translation threshold
-        */
-      inline void
-      setUpperTranslationThreshold (float upper_trl_boundary)
-      {
-        upper_trl_boundary_ = upper_trl_boundary;
-      };
-
-      /** \return the upper translation threshold used for score evaluation. */
-      inline float
-      getUpperTranslationThreshold () const
-      {
-        return (upper_trl_boundary_);
-      };
-
-
-      /** \brief Set the lower translation threshold used for score evaluation.
-        * \param[in] lower_trl_boundary lower translation threshold
-        */
-      inline void
-      setLowerTranslationThreshold (float lower_trl_boundary)
-      {
-        lower_trl_boundary_ = lower_trl_boundary;
-      };
-
-      /** \return the lower translation threshold used for score evaluation. */
-      inline float
-      getLowerTranslationThreshold () const
-      {
-        return (lower_trl_boundary_);
-      };
-
-
-      /** \brief Set the weighting factor of the translation cost term. 
-        * \param[in] lambda the weighting factor of the translation cost term
-        */
-      inline void
-      setLambda (float lambda)
-      {
-        lambda_ = lambda;
-      };
-
-      /** \return the weighting factor of the translation cost term. */
-      inline float
-      getLambda () const
-      {
-        return (lambda_);
-      };
-
-
-      /** \brief Get the N best unique candidate matches according to their fitness score.
-        * The method only returns unique transformations comparing the translation
-        * and the 3D rotation to already returned transformations.
-        *
-        * \note The method may return less than N candidates, if the number of unique candidates
-        * is smaller than N
-        *
-        * \param[in] n number of best candidates to return
-        * \param[in] min_angle3d minimum 3D angle difference in radian
-        * \param[in] min_translation3d minimum 3D translation difference
-        * \param[out] candidates vector of unique candidates
-        */
-      void
-      getNBestCandidates (int n, float min_angle3d, float min_translation3d, MatchingCandidates &candidates);
-
-      /** \brief Get all unique candidate matches with fitness scores above a threshold t.
-        * The method only returns unique transformations comparing the translation
-        * and the 3D rotation to already returned transformations.
-        *
-        * \param[in] t fitness score threshold
-        * \param[in] min_angle3d minimum 3D angle difference in radian
-        * \param[in] min_translation3d minimum 3D translation difference
-        * \param[out] candidates vector of unique candidates
-        */
-      void
-      getTBestCandidates (float t, float min_angle3d, float min_translation3d, MatchingCandidates &candidates);
-      
+    upper_trl_boundary_ = upper_trl_boundary;
+  };
 
-    protected:
-      
-      using PCLBase <PointSource>::deinitCompute;
-      using PCLBase <PointSource>::input_;
-      using PCLBase <PointSource>::indices_;
-
-      using Registration <PointSource, PointTarget, Scalar>::reg_name_;
-      using Registration <PointSource, PointTarget, Scalar>::tree_;
-      using Registration <PointSource, PointTarget, Scalar>::final_transformation_;
-      using Registration <PointSource, PointTarget, Scalar>::ransac_iterations_;
-      using Registration <PointSource, PointTarget, Scalar>::correspondences_;
-      using Registration <PointSource, PointTarget, Scalar>::converged_;
-
-      using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::delta_;
-      using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::approx_overlap_;
-      using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::max_pair_diff_;
-      using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::max_edge_diff_;
-      using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::coincidation_limit_;
-      using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::max_mse_;
-      using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::max_inlier_dist_sqr_;
-      using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::diameter_;
-      using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::normalize_delta_;
-      using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::fitness_score_;
-      using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::score_threshold_;
-      using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::linkMatchWithBase;
-      using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::validateMatch;
-      
-
-      /** \brief Internal computation initialization. */
-      bool
-      initCompute () override;
-
-      /** \brief Method to handle current candidate matches. Here we validate and evaluate the matches w.r.t the
-        * base and store the sorted matches (together with score values and estimated transformations).
-        *
-        * \param[in] base_indices indices of base B
-        * \param[in,out] matches vector of candidate matches w.r.t the base B. The candidate matches are 
-        * reordered during this step.
-        * \param[out] candidates vector which contains the candidates matches M
-        */
-      void
-      handleMatches (
-        const std::vector <int> &base_indices,
-        std::vector <std::vector <int> > &matches,
-        MatchingCandidates &candidates) override;
-
-      /** \brief Validate the transformation by calculating the score value after transforming the input source cloud.
-        * The resulting score is later used as the decision criteria of the best fitting match.
-        *
-        * \param[out] transformation updated orientation matrix using all inliers
-        * \param[out] fitness_score current best score
-        * \note fitness score is only updated if the score of the current transformation exceeds the input one.
-        * \return
-        * * < 0 if previous result is better than the current one (score remains)
-        * * = 0 current result is better than the previous one (score updated)
-        */
-      int
-      validateTransformation (Eigen::Matrix4f &transformation, float &fitness_score) override;
-
-      /** \brief Final computation of best match out of vector of matches. To avoid cross thread dependencies
-        *  during parallel running, a best match for each try was calculated.
-        * \note For forwards compatibility the candidates are stored in vectors of 'vectors of size 1'.
-        * \param[in] candidates vector of candidate matches
-        */
-      void
-      finalCompute (const std::vector <MatchingCandidates > &candidates) override;
-
-
-      /** \brief Lower boundary for translation costs calculation.
-        * \note If not set by the user, the translation costs are not used during evaluation.
-        */
-      float lower_trl_boundary_;
-
-      /** \brief Upper boundary for translation costs calculation.
-        * \note If not set by the user, it is calculated from the estimated overlap and the diameter
-        * of the point cloud.
-        */
-      float upper_trl_boundary_;
-
-      /** \brief Weighting factor for translation costs (standard = 0.5). */
-      float lambda_;
-
-
-      /** \brief Container for resulting vector of registration candidates. */
-      MatchingCandidates candidates_;
-
-      /** \brief Flag if translation score should be used in validation (internal calculation). */
-      bool use_trl_score_;
+  /** \return the upper translation threshold used for score evaluation. */
+  inline float
+  getUpperTranslationThreshold() const
+  {
+    return (upper_trl_boundary_);
+  };
+
+  /** \brief Set the lower translation threshold used for score evaluation.
+   * \param[in] lower_trl_boundary lower translation threshold
+   */
+  inline void
+  setLowerTranslationThreshold(float lower_trl_boundary)
+  {
+    lower_trl_boundary_ = lower_trl_boundary;
+  };
 
-      /** \brief Subset of input indices on which we evaluate candidates.
-        * To speed up the evaluation, we only use a fix number of indices defined during initialization.
-        */
-      pcl::IndicesPtr indices_validation_;
+  /** \return the lower translation threshold used for score evaluation. */
+  inline float
+  getLowerTranslationThreshold() const
+  {
+    return (lower_trl_boundary_);
+  };
+
+  /** \brief Set the weighting factor of the translation cost term.
+   * \param[in] lambda the weighting factor of the translation cost term
+   */
+  inline void
+  setLambda(float lambda)
+  {
+    lambda_ = lambda;
+  };
 
-    };
-  }; // namespace registration
-}; // namespace pcl 
+  /** \return the weighting factor of the translation cost term. */
+  inline float
+  getLambda() const
+  {
+    return (lambda_);
+  };
+
+  /** \brief Get the N best unique candidate matches according to their fitness score.
+   * The method only returns unique transformations comparing the translation
+   * and the 3D rotation to already returned transformations.
+   *
+   * \note The method may return less than N candidates, if the number of unique
+   * candidates is smaller than N
+   *
+   * \param[in] n number of best candidates to return
+   * \param[in] min_angle3d minimum 3D angle difference in radian
+   * \param[in] min_translation3d minimum 3D translation difference
+   * \param[out] candidates vector of unique candidates
+   */
+  void
+  getNBestCandidates(int n,
+                     float min_angle3d,
+                     float min_translation3d,
+                     MatchingCandidates& candidates);
+
+  /** \brief Get all unique candidate matches with fitness scores above a threshold t.
+   * The method only returns unique transformations comparing the translation
+   * and the 3D rotation to already returned transformations.
+   *
+   * \param[in] t fitness score threshold
+   * \param[in] min_angle3d minimum 3D angle difference in radian
+   * \param[in] min_translation3d minimum 3D translation difference
+   * \param[out] candidates vector of unique candidates
+   */
+  void
+  getTBestCandidates(float t,
+                     float min_angle3d,
+                     float min_translation3d,
+                     MatchingCandidates& candidates);
+
+protected:
+  using PCLBase<PointSource>::deinitCompute;
+  using PCLBase<PointSource>::input_;
+  using PCLBase<PointSource>::indices_;
+
+  using Registration<PointSource, PointTarget, Scalar>::reg_name_;
+  using Registration<PointSource, PointTarget, Scalar>::tree_;
+  using Registration<PointSource, PointTarget, Scalar>::final_transformation_;
+  using Registration<PointSource, PointTarget, Scalar>::ransac_iterations_;
+  using Registration<PointSource, PointTarget, Scalar>::correspondences_;
+  using Registration<PointSource, PointTarget, Scalar>::converged_;
+
+  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::delta_;
+  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
+      approx_overlap_;
+  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::max_pair_diff_;
+  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::max_edge_diff_;
+  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
+      coincidation_limit_;
+  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::max_mse_;
+  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
+      max_inlier_dist_sqr_;
+  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::diameter_;
+  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
+      normalize_delta_;
+  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::fitness_score_;
+  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
+      score_threshold_;
+  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
+      linkMatchWithBase;
+  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::validateMatch;
+
+  /** \brief Internal computation initialization. */
+  bool
+  initCompute() override;
+
+  /** \brief Method to handle current candidate matches. Here we validate and evaluate
+   * the matches w.r.t the base and store the sorted matches (together with score values
+   * and estimated transformations).
+   *
+   * \param[in] base_indices indices of base B
+   * \param[in,out] matches vector of candidate matches w.r.t the base B. The candidate
+   * matches are reordered during this step. \param[out] candidates vector which
+   * contains the candidates matches M
+   */
+  void
+  handleMatches(const pcl::Indices& base_indices,
+                std::vector<pcl::Indices>& matches,
+                MatchingCandidates& candidates) override;
+
+  /** \brief Validate the transformation by calculating the score value after
+   * transforming the input source cloud. The resulting score is later used as the
+   * decision criteria of the best fitting match.
+   *
+   * \param[out] transformation updated orientation matrix using all inliers
+   * \param[out] fitness_score current best score
+   * \note fitness score is only updated if the score of the current transformation
+   * exceeds the input one. \return
+   * * < 0 if previous result is better than the current one (score remains)
+   * * = 0 current result is better than the previous one (score updated)
+   */
+  int
+  validateTransformation(Eigen::Matrix4f& transformation,
+                         float& fitness_score) override;
+
+  /** \brief Final computation of best match out of vector of matches. To avoid cross
+   * thread dependencies during parallel running, a best match for each try was
+   * calculated. \note For forwards compatibility the candidates are stored in vectors
+   * of 'vectors of size 1'. \param[in] candidates vector of candidate matches
+   */
+  void
+  finalCompute(const std::vector<MatchingCandidates>& candidates) override;
+
+  /** \brief Lower boundary for translation costs calculation.
+   * \note If not set by the user, the translation costs are not used during evaluation.
+   */
+  float lower_trl_boundary_;
+
+  /** \brief Upper boundary for translation costs calculation.
+   * \note If not set by the user, it is calculated from the estimated overlap and the
+   * diameter of the point cloud.
+   */
+  float upper_trl_boundary_;
+
+  /** \brief Weighting factor for translation costs (standard = 0.5). */
+  float lambda_;
+
+  /** \brief Container for resulting vector of registration candidates. */
+  MatchingCandidates candidates_;
+
+  /** \brief Flag if translation score should be used in validation (internal
+   * calculation). */
+  bool use_trl_score_;
+
+  /** \brief Subset of input indices on which we evaluate candidates.
+   * To speed up the evaluation, we only use a fix number of indices defined during
+   * initialization.
+   */
+  pcl::IndicesPtr indices_validation_;
+};
+}; // namespace registration
+}; // namespace pcl
 
 #include <pcl/registration/impl/ia_kfpcs.hpp>
diff --git a/registration/include/pcl/registration/ia_ransac.h b/registration/include/pcl/registration/ia_ransac.h
index 07ac6364..828f3225 100644
--- a/registration/include/pcl/registration/ia_ransac.h
+++ b/registration/include/pcl/registration/ia_ransac.h
@@ -40,243 +40,296 @@
 
 #pragma once
 
-#include <pcl/memory.h>
 #include <pcl/registration/registration.h>
 #include <pcl/registration/transformation_estimation_svd.h>
+#include <pcl/memory.h>
+
+namespace pcl {
+/** \brief @b SampleConsensusInitialAlignment is an implementation of the initial
+ * alignment algorithm described in section IV of "Fast Point Feature Histograms (FPFH)
+ * for 3D Registration," Rusu et al. \author Michael Dixon, Radu B. Rusu
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename FeatureT>
+class SampleConsensusInitialAlignment : public Registration<PointSource, PointTarget> {
+public:
+  using Registration<PointSource, PointTarget>::reg_name_;
+  using Registration<PointSource, PointTarget>::input_;
+  using Registration<PointSource, PointTarget>::indices_;
+  using Registration<PointSource, PointTarget>::target_;
+  using Registration<PointSource, PointTarget>::final_transformation_;
+  using Registration<PointSource, PointTarget>::transformation_;
+  using Registration<PointSource, PointTarget>::corr_dist_threshold_;
+  using Registration<PointSource, PointTarget>::min_number_correspondences_;
+  using Registration<PointSource, PointTarget>::max_iterations_;
+  using Registration<PointSource, PointTarget>::tree_;
+  using Registration<PointSource, PointTarget>::transformation_estimation_;
+  using Registration<PointSource, PointTarget>::converged_;
+  using Registration<PointSource, PointTarget>::getClassName;
+
+  using PointCloudSource =
+      typename Registration<PointSource, PointTarget>::PointCloudSource;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+
+  using PointCloudTarget =
+      typename Registration<PointSource, PointTarget>::PointCloudTarget;
+
+  using PointIndicesPtr = PointIndices::Ptr;
+  using PointIndicesConstPtr = PointIndices::ConstPtr;
+
+  using FeatureCloud = pcl::PointCloud<FeatureT>;
+  using FeatureCloudPtr = typename FeatureCloud::Ptr;
+  using FeatureCloudConstPtr = typename FeatureCloud::ConstPtr;
+
+  using Ptr =
+      shared_ptr<SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>>;
+  using ConstPtr = shared_ptr<
+      const SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>>;
+
+  class ErrorFunctor {
+  public:
+    using Ptr = shared_ptr<ErrorFunctor>;
+    using ConstPtr = shared_ptr<const ErrorFunctor>;
+
+    virtual ~ErrorFunctor() = default;
+    virtual float
+    operator()(float d) const = 0;
+  };
+
+  class HuberPenalty : public ErrorFunctor {
+  private:
+    HuberPenalty() {}
+
+  public:
+    HuberPenalty(float threshold) : threshold_(threshold) {}
+    virtual float
+    operator()(float e) const
+    {
+      if (e <= threshold_)
+        return (0.5 * e * e);
+      return (0.5 * threshold_ * (2.0 * std::fabs(e) - threshold_));
+    }
+
+  protected:
+    float threshold_;
+  };
+
+  class TruncatedError : public ErrorFunctor {
+  private:
+    TruncatedError() {}
+
+  public:
+    ~TruncatedError() {}
+
+    TruncatedError(float threshold) : threshold_(threshold) {}
+    float
+    operator()(float e) const override
+    {
+      if (e <= threshold_)
+        return (e / threshold_);
+      return (1.0);
+    }
+
+  protected:
+    float threshold_;
+  };
+
+  using ErrorFunctorPtr = typename ErrorFunctor::Ptr;
+
+  using FeatureKdTreePtr = typename KdTreeFLANN<FeatureT>::Ptr;
+  /** \brief Constructor. */
+  SampleConsensusInitialAlignment()
+  : input_features_()
+  , target_features_()
+  , nr_samples_(3)
+  , min_sample_distance_(0.0f)
+  , k_correspondences_(10)
+  , feature_tree_(new pcl::KdTreeFLANN<FeatureT>)
+  , error_functor_()
+  {
+    reg_name_ = "SampleConsensusInitialAlignment";
+    max_iterations_ = 1000;
+
+    // Setting a non-std::numeric_limits<double>::max () value to corr_dist_threshold_
+    // to make it play nicely with TruncatedError
+    corr_dist_threshold_ = 100.0f;
+    transformation_estimation_.reset(
+        new pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>);
+  };
 
-namespace pcl
-{
-  /** \brief @b SampleConsensusInitialAlignment is an implementation of the initial alignment algorithm described in
-    *  section IV of "Fast Point Feature Histograms (FPFH) for 3D Registration," Rusu et al.
-    * \author Michael Dixon, Radu B. Rusu
-    * \ingroup registration
-    */
-  template <typename PointSource, typename PointTarget, typename FeatureT>
-  class SampleConsensusInitialAlignment : public Registration<PointSource, PointTarget>
+  /** \brief Provide a shared pointer to the source point cloud's feature descriptors
+   * \param features the source point cloud's features
+   */
+  void
+  setSourceFeatures(const FeatureCloudConstPtr& features);
+
+  /** \brief Get a pointer to the source point cloud's features */
+  inline FeatureCloudConstPtr const
+  getSourceFeatures()
+  {
+    return (input_features_);
+  }
+
+  /** \brief Provide a shared pointer to the target point cloud's feature descriptors
+   * \param features the target point cloud's features
+   */
+  void
+  setTargetFeatures(const FeatureCloudConstPtr& features);
+
+  /** \brief Get a pointer to the target point cloud's features */
+  inline FeatureCloudConstPtr const
+  getTargetFeatures()
+  {
+    return (target_features_);
+  }
+
+  /** \brief Set the minimum distances between samples
+   * \param min_sample_distance the minimum distances between samples
+   */
+  void
+  setMinSampleDistance(float min_sample_distance)
+  {
+    min_sample_distance_ = min_sample_distance;
+  }
+
+  /** \brief Get the minimum distances between samples, as set by the user */
+  float
+  getMinSampleDistance()
+  {
+    return (min_sample_distance_);
+  }
+
+  /** \brief Set the number of samples to use during each iteration
+   * \param nr_samples the number of samples to use during each iteration
+   */
+  void
+  setNumberOfSamples(int nr_samples)
+  {
+    nr_samples_ = nr_samples;
+  }
+
+  /** \brief Get the number of samples to use during each iteration, as set by the user
+   */
+  int
+  getNumberOfSamples()
   {
-    public:
-      using Registration<PointSource, PointTarget>::reg_name_;
-      using Registration<PointSource, PointTarget>::input_;
-      using Registration<PointSource, PointTarget>::indices_;
-      using Registration<PointSource, PointTarget>::target_;
-      using Registration<PointSource, PointTarget>::final_transformation_;
-      using Registration<PointSource, PointTarget>::transformation_;
-      using Registration<PointSource, PointTarget>::corr_dist_threshold_;
-      using Registration<PointSource, PointTarget>::min_number_correspondences_;
-      using Registration<PointSource, PointTarget>::max_iterations_;
-      using Registration<PointSource, PointTarget>::tree_;
-      using Registration<PointSource, PointTarget>::transformation_estimation_;
-      using Registration<PointSource, PointTarget>::converged_;
-      using Registration<PointSource, PointTarget>::getClassName;
-
-      using PointCloudSource = typename Registration<PointSource, PointTarget>::PointCloudSource;
-      using PointCloudSourcePtr = typename PointCloudSource::Ptr;
-      using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
-
-      using PointCloudTarget = typename Registration<PointSource, PointTarget>::PointCloudTarget;
-
-      using PointIndicesPtr = PointIndices::Ptr;
-      using PointIndicesConstPtr = PointIndices::ConstPtr;
-
-      using FeatureCloud = pcl::PointCloud<FeatureT>;
-      using FeatureCloudPtr = typename FeatureCloud::Ptr;
-      using FeatureCloudConstPtr = typename FeatureCloud::ConstPtr;
-
-      using Ptr = shared_ptr<SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT> >;
-      using ConstPtr = shared_ptr<const SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT> >;
-
-
-      class ErrorFunctor
-      {
-        public:
-          using Ptr = shared_ptr<ErrorFunctor>;
-          using ConstPtr = shared_ptr<const ErrorFunctor>;
-
-          virtual ~ErrorFunctor () = default;
-          virtual float operator () (float d) const = 0;
-      };
-
-      class HuberPenalty : public ErrorFunctor
-      {
-        private:
-          HuberPenalty () {}
-        public:
-          HuberPenalty (float threshold)  : threshold_ (threshold) {}
-          virtual float operator () (float e) const
-          { 
-            if (e <= threshold_)
-              return (0.5 * e*e); 
-            return (0.5 * threshold_ * (2.0 * std::fabs (e) - threshold_));
-          }
-        protected:
-          float threshold_;
-      };
-
-      class TruncatedError : public ErrorFunctor
-      {
-        private:
-          TruncatedError () {}
-        public:
-          ~TruncatedError () {}
-
-          TruncatedError (float threshold) : threshold_ (threshold) {}
-          float operator () (float e) const override
-          { 
-            if (e <= threshold_)
-              return (e / threshold_);
-            return (1.0);
-          }
-        protected:
-          float threshold_;
-      };
-
-      using ErrorFunctorPtr = typename ErrorFunctor::Ptr;
-
-      using FeatureKdTreePtr = typename KdTreeFLANN<FeatureT>::Ptr; 
-      /** \brief Constructor. */
-      SampleConsensusInitialAlignment () : 
-        input_features_ (), target_features_ (), 
-        nr_samples_(3), min_sample_distance_ (0.0f), k_correspondences_ (10), 
-        feature_tree_ (new pcl::KdTreeFLANN<FeatureT>),
-        error_functor_ ()
-      {
-        reg_name_ = "SampleConsensusInitialAlignment";
-        max_iterations_ = 1000;
-
-        // Setting a non-std::numeric_limits<double>::max () value to corr_dist_threshold_ to make it play nicely with TruncatedError
-        corr_dist_threshold_ = 100.0f;
-        transformation_estimation_.reset (new pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>);
-      };
-
-      /** \brief Provide a shared pointer to the source point cloud's feature descriptors
-        * \param features the source point cloud's features
-        */
-      void 
-      setSourceFeatures (const FeatureCloudConstPtr &features);
-
-      /** \brief Get a pointer to the source point cloud's features */
-      inline FeatureCloudConstPtr const 
-      getSourceFeatures () { return (input_features_); }
-
-      /** \brief Provide a shared pointer to the target point cloud's feature descriptors
-        * \param features the target point cloud's features
-        */
-      void 
-      setTargetFeatures (const FeatureCloudConstPtr &features);
-
-      /** \brief Get a pointer to the target point cloud's features */
-      inline FeatureCloudConstPtr const 
-      getTargetFeatures () { return (target_features_); }
-
-      /** \brief Set the minimum distances between samples
-        * \param min_sample_distance the minimum distances between samples
-        */
-      void 
-      setMinSampleDistance (float min_sample_distance) { min_sample_distance_ = min_sample_distance; }
-
-      /** \brief Get the minimum distances between samples, as set by the user */
-      float 
-      getMinSampleDistance () { return (min_sample_distance_); }
-
-      /** \brief Set the number of samples to use during each iteration
-        * \param nr_samples the number of samples to use during each iteration
-        */
-      void 
-      setNumberOfSamples (int nr_samples) { nr_samples_ = nr_samples; }
-
-      /** \brief Get the number of samples to use during each iteration, as set by the user */
-      int 
-      getNumberOfSamples () { return (nr_samples_); }
-
-      /** \brief Set the number of neighbors to use when selecting a random feature correspondence.  A higher value will
-        * add more randomness to the feature matching.
-        * \param k the number of neighbors to use when selecting a random feature correspondence.
-        */
-      void
-      setCorrespondenceRandomness (int k) { k_correspondences_ = k; }
-
-      /** \brief Get the number of neighbors used when selecting a random feature correspondence, as set by the user */
-      int
-      getCorrespondenceRandomness () { return (k_correspondences_); }
-
-      /** \brief Specify the error function to minimize
-       * \note This call is optional.  TruncatedError will be used by default
-       * \param[in] error_functor a shared pointer to a subclass of SampleConsensusInitialAlignment::ErrorFunctor
-       */
-      void
-      setErrorFunction (const ErrorFunctorPtr & error_functor) { error_functor_ = error_functor; }
-
-      /** \brief Get a shared pointer to the ErrorFunctor that is to be minimized  
-       * \return A shared pointer to a subclass of SampleConsensusInitialAlignment::ErrorFunctor
-       */
-      ErrorFunctorPtr
-      getErrorFunction () { return (error_functor_); }
-
-    protected:
-      /** \brief Choose a random index between 0 and n-1
-        * \param n the number of possible indices to choose from
-        */
-      inline int 
-      getRandomIndex (int n) { return (static_cast<int> (n * (rand () / (RAND_MAX + 1.0)))); };
-      
-      /** \brief Select \a nr_samples sample points from cloud while making sure that their pairwise distances are 
-        * greater than a user-defined minimum distance, \a min_sample_distance.
-        * \param cloud the input point cloud
-        * \param nr_samples the number of samples to select
-        * \param min_sample_distance the minimum distance between any two samples
-        * \param sample_indices the resulting sample indices
-        */
-      void 
-      selectSamples (const PointCloudSource &cloud, int nr_samples, float min_sample_distance, 
-                     std::vector<int> &sample_indices);
-
-      /** \brief For each of the sample points, find a list of points in the target cloud whose features are similar to 
-        * the sample points' features. From these, select one randomly which will be considered that sample point's 
-        * correspondence. 
-        * \param input_features a cloud of feature descriptors
-        * \param sample_indices the indices of each sample point
-        * \param corresponding_indices the resulting indices of each sample's corresponding point in the target cloud
-        */
-      void 
-      findSimilarFeatures (const FeatureCloud &input_features, const std::vector<int> &sample_indices, 
-                           std::vector<int> &corresponding_indices);
-
-      /** \brief An error metric for that computes the quality of the alignment between the given cloud and the target.
-        * \param cloud the input cloud
-        * \param threshold distances greater than this value are capped
-        */
-      float 
-      computeErrorMetric (const PointCloudSource &cloud, float threshold);
-
-      /** \brief Rigid transformation computation method.
-        * \param output the transformed input point cloud dataset using the rigid transformation found
-        * \param guess The computed transforamtion
-        */
-      void 
-      computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) override;
-
-      /** \brief The source point cloud's feature descriptors. */
-      FeatureCloudConstPtr input_features_;
-
-      /** \brief The target point cloud's feature descriptors. */
-      FeatureCloudConstPtr target_features_;  
-
-      /** \brief The number of samples to use during each iteration. */
-      int nr_samples_;
-
-      /** \brief The minimum distances between samples. */
-      float min_sample_distance_;
-
-      /** \brief The number of neighbors to use when selecting a random feature correspondence. */
-      int k_correspondences_;
-     
-      /** \brief The KdTree used to compare feature descriptors. */
-      FeatureKdTreePtr feature_tree_;               
-
-      ErrorFunctorPtr error_functor_;
-    public:
-      PCL_MAKE_ALIGNED_OPERATOR_NEW
+    return (nr_samples_);
+  }
+
+  /** \brief Set the number of neighbors to use when selecting a random feature
+   * correspondence.  A higher value will add more randomness to the feature matching.
+   * \param k the number of neighbors to use when selecting a random feature
+   * correspondence.
+   */
+  void
+  setCorrespondenceRandomness(int k)
+  {
+    k_correspondences_ = k;
+  }
+
+  /** \brief Get the number of neighbors used when selecting a random feature
+   * correspondence, as set by the user */
+  int
+  getCorrespondenceRandomness()
+  {
+    return (k_correspondences_);
+  }
+
+  /** \brief Specify the error function to minimize
+   * \note This call is optional.  TruncatedError will be used by default
+   * \param[in] error_functor a shared pointer to a subclass of
+   * SampleConsensusInitialAlignment::ErrorFunctor
+   */
+  void
+  setErrorFunction(const ErrorFunctorPtr& error_functor)
+  {
+    error_functor_ = error_functor;
+  }
+
+  /** \brief Get a shared pointer to the ErrorFunctor that is to be minimized
+   * \return A shared pointer to a subclass of
+   * SampleConsensusInitialAlignment::ErrorFunctor
+   */
+  ErrorFunctorPtr
+  getErrorFunction()
+  {
+    return (error_functor_);
+  }
+
+protected:
+  /** \brief Choose a random index between 0 and n-1
+   * \param n the number of possible indices to choose from
+   */
+  inline pcl::index_t
+  getRandomIndex(int n)
+  {
+    return (static_cast<pcl::index_t>(n * (rand() / (RAND_MAX + 1.0))));
   };
-}
+
+  /** \brief Select \a nr_samples sample points from cloud while making sure that their
+   * pairwise distances are greater than a user-defined minimum distance, \a
+   * min_sample_distance. \param cloud the input point cloud \param nr_samples the
+   * number of samples to select \param min_sample_distance the minimum distance between
+   * any two samples \param sample_indices the resulting sample indices
+   */
+  void
+  selectSamples(const PointCloudSource& cloud,
+                unsigned int nr_samples,
+                float min_sample_distance,
+                pcl::Indices& sample_indices);
+
+  /** \brief For each of the sample points, find a list of points in the target cloud
+   * whose features are similar to the sample points' features. From these, select one
+   * randomly which will be considered that sample point's correspondence. \param
+   * input_features a cloud of feature descriptors \param sample_indices the indices of
+   * each sample point \param corresponding_indices the resulting indices of each
+   * sample's corresponding point in the target cloud
+   */
+  void
+  findSimilarFeatures(const FeatureCloud& input_features,
+                      const pcl::Indices& sample_indices,
+                      pcl::Indices& corresponding_indices);
+
+  /** \brief An error metric for that computes the quality of the alignment between the
+   * given cloud and the target. \param cloud the input cloud \param threshold distances
+   * greater than this value are capped
+   */
+  float
+  computeErrorMetric(const PointCloudSource& cloud, float threshold);
+
+  /** \brief Rigid transformation computation method.
+   * \param output the transformed input point cloud dataset using the rigid
+   * transformation found \param guess The computed transforamtion
+   */
+  void
+  computeTransformation(PointCloudSource& output,
+                        const Eigen::Matrix4f& guess) override;
+
+  /** \brief The source point cloud's feature descriptors. */
+  FeatureCloudConstPtr input_features_;
+
+  /** \brief The target point cloud's feature descriptors. */
+  FeatureCloudConstPtr target_features_;
+
+  /** \brief The number of samples to use during each iteration. */
+  int nr_samples_;
+
+  /** \brief The minimum distances between samples. */
+  float min_sample_distance_;
+
+  /** \brief The number of neighbors to use when selecting a random feature
+   * correspondence. */
+  int k_correspondences_;
+
+  /** \brief The KdTree used to compare feature descriptors. */
+  FeatureKdTreePtr feature_tree_;
+
+  ErrorFunctorPtr error_functor_;
+
+public:
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+};
+} // namespace pcl
 
 #include <pcl/registration/impl/ia_ransac.hpp>
diff --git a/registration/include/pcl/registration/icp.h b/registration/include/pcl/registration/icp.h
index b068d5b6..3a3240a1 100644
--- a/registration/include/pcl/registration/icp.h
+++ b/registration/include/pcl/registration/icp.h
@@ -41,363 +41,417 @@
 #pragma once
 
 // PCL includes
-#include <pcl/memory.h>  // for dynamic_pointer_cast, pcl::make_shared, shared_ptr
+#include <pcl/registration/correspondence_estimation.h>
+#include <pcl/registration/default_convergence_criteria.h>
 #include <pcl/registration/registration.h>
-#include <pcl/registration/transformation_estimation_svd.h>
 #include <pcl/registration/transformation_estimation_point_to_plane_lls.h>
+#include <pcl/registration/transformation_estimation_svd.h>
 #include <pcl/registration/transformation_estimation_symmetric_point_to_plane_lls.h>
-#include <pcl/registration/correspondence_estimation.h>
-#include <pcl/registration/default_convergence_criteria.h>
+#include <pcl/memory.h> // for dynamic_pointer_cast, pcl::make_shared, shared_ptr
 
-
-namespace pcl
-{
-  /** \brief @b IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm. 
-    * The transformation is estimated based on Singular Value Decomposition (SVD).
-    *
-    * The algorithm has several termination criteria:
-    *
-    * <ol>
-    * <li>Number of iterations has reached the maximum user imposed number of iterations (via \ref setMaximumIterations)</li>
-    * <li>The epsilon (difference) between the previous transformation and the current estimated transformation is smaller than an user imposed value (via \ref setTransformationEpsilon)</li>
-    * <li>The sum of Euclidean squared errors is smaller than a user defined threshold (via \ref setEuclideanFitnessEpsilon)</li>
-    * </ol>
-    *
-    *
-    * Usage example:
-    * \code
-    * IterativeClosestPoint<PointXYZ, PointXYZ> icp;
-    * // Set the input source and target
-    * icp.setInputCloud (cloud_source);
-    * icp.setInputTarget (cloud_target);
-    *
-    * // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
-    * icp.setMaxCorrespondenceDistance (0.05);
-    * // Set the maximum number of iterations (criterion 1)
-    * icp.setMaximumIterations (50);
-    * // Set the transformation epsilon (criterion 2)
-    * icp.setTransformationEpsilon (1e-8);
-    * // Set the euclidean distance difference epsilon (criterion 3)
-    * icp.setEuclideanFitnessEpsilon (1);
-    *
-    * // Perform the alignment
-    * icp.align (cloud_source_registered);
-    *
-    * // Obtain the transformation that aligned cloud_source to cloud_source_registered
-    * Eigen::Matrix4f transformation = icp.getFinalTransformation ();
-    * \endcode
-    *
-    * \author Radu B. Rusu, Michael Dixon
-    * \ingroup registration
-    */
-  template <typename PointSource, typename PointTarget, typename Scalar = float>
-  class IterativeClosestPoint : public Registration<PointSource, PointTarget, Scalar>
+namespace pcl {
+/** \brief @b IterativeClosestPoint provides a base implementation of the Iterative
+ * Closest Point algorithm. The transformation is estimated based on Singular Value
+ * Decomposition (SVD).
+ *
+ * The algorithm has several termination criteria:
+ *
+ * <ol>
+ * <li>Number of iterations has reached the maximum user imposed number of iterations
+ * (via \ref setMaximumIterations)</li> <li>The epsilon (difference) between the
+ * previous transformation and the current estimated transformation is smaller than an
+ * user imposed value (via \ref setTransformationEpsilon)</li> <li>The sum of Euclidean
+ * squared errors is smaller than a user defined threshold (via \ref
+ * setEuclideanFitnessEpsilon)</li>
+ * </ol>
+ *
+ *
+ * Usage example:
+ * \code
+ * IterativeClosestPoint<PointXYZ, PointXYZ> icp;
+ * // Set the input source and target
+ * icp.setInputSource (cloud_source);
+ * icp.setInputTarget (cloud_target);
+ *
+ * // Set the max correspondence distance to 5cm (e.g., correspondences with higher
+ * // distances will be ignored)
+ * icp.setMaxCorrespondenceDistance (0.05);
+ * // Set the maximum number of iterations (criterion 1)
+ * icp.setMaximumIterations (50);
+ * // Set the transformation epsilon (criterion 2)
+ * icp.setTransformationEpsilon (1e-8);
+ * // Set the euclidean distance difference epsilon (criterion 3)
+ * icp.setEuclideanFitnessEpsilon (1);
+ *
+ * // Perform the alignment
+ * icp.align (cloud_source_registered);
+ *
+ * // Obtain the transformation that aligned cloud_source to cloud_source_registered
+ * Eigen::Matrix4f transformation = icp.getFinalTransformation ();
+ * \endcode
+ *
+ * \author Radu B. Rusu, Michael Dixon
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename Scalar = float>
+class IterativeClosestPoint : public Registration<PointSource, PointTarget, Scalar> {
+public:
+  using PointCloudSource =
+      typename Registration<PointSource, PointTarget, Scalar>::PointCloudSource;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+
+  using PointCloudTarget =
+      typename Registration<PointSource, PointTarget, Scalar>::PointCloudTarget;
+  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
+  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
+
+  using PointIndicesPtr = PointIndices::Ptr;
+  using PointIndicesConstPtr = PointIndices::ConstPtr;
+
+  using Ptr = shared_ptr<IterativeClosestPoint<PointSource, PointTarget, Scalar>>;
+  using ConstPtr =
+      shared_ptr<const IterativeClosestPoint<PointSource, PointTarget, Scalar>>;
+
+  using Registration<PointSource, PointTarget, Scalar>::reg_name_;
+  using Registration<PointSource, PointTarget, Scalar>::getClassName;
+  using Registration<PointSource, PointTarget, Scalar>::input_;
+  using Registration<PointSource, PointTarget, Scalar>::indices_;
+  using Registration<PointSource, PointTarget, Scalar>::target_;
+  using Registration<PointSource, PointTarget, Scalar>::nr_iterations_;
+  using Registration<PointSource, PointTarget, Scalar>::max_iterations_;
+  using Registration<PointSource, PointTarget, Scalar>::previous_transformation_;
+  using Registration<PointSource, PointTarget, Scalar>::final_transformation_;
+  using Registration<PointSource, PointTarget, Scalar>::transformation_;
+  using Registration<PointSource, PointTarget, Scalar>::transformation_epsilon_;
+  using Registration<PointSource, PointTarget, Scalar>::
+      transformation_rotation_epsilon_;
+  using Registration<PointSource, PointTarget, Scalar>::converged_;
+  using Registration<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
+  using Registration<PointSource, PointTarget, Scalar>::inlier_threshold_;
+  using Registration<PointSource, PointTarget, Scalar>::min_number_correspondences_;
+  using Registration<PointSource, PointTarget, Scalar>::update_visualizer_;
+  using Registration<PointSource, PointTarget, Scalar>::euclidean_fitness_epsilon_;
+  using Registration<PointSource, PointTarget, Scalar>::correspondences_;
+  using Registration<PointSource, PointTarget, Scalar>::transformation_estimation_;
+  using Registration<PointSource, PointTarget, Scalar>::correspondence_estimation_;
+  using Registration<PointSource, PointTarget, Scalar>::correspondence_rejectors_;
+
+  typename pcl::registration::DefaultConvergenceCriteria<Scalar>::Ptr
+      convergence_criteria_;
+  using Matrix4 = typename Registration<PointSource, PointTarget, Scalar>::Matrix4;
+
+  /** \brief Empty constructor. */
+  IterativeClosestPoint()
+  : x_idx_offset_(0)
+  , y_idx_offset_(0)
+  , z_idx_offset_(0)
+  , nx_idx_offset_(0)
+  , ny_idx_offset_(0)
+  , nz_idx_offset_(0)
+  , use_reciprocal_correspondence_(false)
+  , source_has_normals_(false)
+  , target_has_normals_(false)
   {
-    public:
-      using PointCloudSource = typename Registration<PointSource, PointTarget, Scalar>::PointCloudSource;
-      using PointCloudSourcePtr = typename PointCloudSource::Ptr;
-      using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
-
-      using PointCloudTarget = typename Registration<PointSource, PointTarget, Scalar>::PointCloudTarget;
-      using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
-      using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
-
-      using PointIndicesPtr = PointIndices::Ptr;
-      using PointIndicesConstPtr = PointIndices::ConstPtr;
-
-      using Ptr = shared_ptr<IterativeClosestPoint<PointSource, PointTarget, Scalar> >;
-      using ConstPtr = shared_ptr<const IterativeClosestPoint<PointSource, PointTarget, Scalar> >;
-
-      using Registration<PointSource, PointTarget, Scalar>::reg_name_;
-      using Registration<PointSource, PointTarget, Scalar>::getClassName;
-      using Registration<PointSource, PointTarget, Scalar>::input_;
-      using Registration<PointSource, PointTarget, Scalar>::indices_;
-      using Registration<PointSource, PointTarget, Scalar>::target_;
-      using Registration<PointSource, PointTarget, Scalar>::nr_iterations_;
-      using Registration<PointSource, PointTarget, Scalar>::max_iterations_;
-      using Registration<PointSource, PointTarget, Scalar>::previous_transformation_;
-      using Registration<PointSource, PointTarget, Scalar>::final_transformation_;
-      using Registration<PointSource, PointTarget, Scalar>::transformation_;
-      using Registration<PointSource, PointTarget, Scalar>::transformation_epsilon_;
-      using Registration<PointSource, PointTarget, Scalar>::transformation_rotation_epsilon_;
-      using Registration<PointSource, PointTarget, Scalar>::converged_;
-      using Registration<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
-      using Registration<PointSource, PointTarget, Scalar>::inlier_threshold_;
-      using Registration<PointSource, PointTarget, Scalar>::min_number_correspondences_;
-      using Registration<PointSource, PointTarget, Scalar>::update_visualizer_;
-      using Registration<PointSource, PointTarget, Scalar>::euclidean_fitness_epsilon_;
-      using Registration<PointSource, PointTarget, Scalar>::correspondences_;
-      using Registration<PointSource, PointTarget, Scalar>::transformation_estimation_;
-      using Registration<PointSource, PointTarget, Scalar>::correspondence_estimation_;
-      using Registration<PointSource, PointTarget, Scalar>::correspondence_rejectors_;
-
-      typename pcl::registration::DefaultConvergenceCriteria<Scalar>::Ptr convergence_criteria_;
-      using Matrix4 = typename Registration<PointSource, PointTarget, Scalar>::Matrix4;
-
-      /** \brief Empty constructor. */
-      IterativeClosestPoint () 
-        : x_idx_offset_ (0)
-        , y_idx_offset_ (0)
-        , z_idx_offset_ (0)
-        , nx_idx_offset_ (0)
-        , ny_idx_offset_ (0)
-        , nz_idx_offset_ (0)
-        , use_reciprocal_correspondence_ (false)
-        , source_has_normals_ (false)
-        , target_has_normals_ (false)
-      {
-        reg_name_ = "IterativeClosestPoint";
-        transformation_estimation_.reset (new pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar> ());
-        correspondence_estimation_.reset (new pcl::registration::CorrespondenceEstimation<PointSource, PointTarget, Scalar>);
-        convergence_criteria_.reset(new pcl::registration::DefaultConvergenceCriteria<Scalar> (nr_iterations_, transformation_, *correspondences_));
-      };
-
-      /** \brief Empty destructor */
-      ~IterativeClosestPoint () {}
-
-      /** \brief Returns a pointer to the DefaultConvergenceCriteria used by the IterativeClosestPoint class.
-        * This allows to check the convergence state after the align() method as well as to configure
-        * DefaultConvergenceCriteria's parameters not available through the ICP API before the align()
-        * method is called. Please note that the align method sets max_iterations_,
-        * euclidean_fitness_epsilon_ and transformation_epsilon_ and therefore overrides the default / set
-        * values of the DefaultConvergenceCriteria instance.
-        * \return Pointer to the IterativeClosestPoint's DefaultConvergenceCriteria.
-        */
-      inline typename pcl::registration::DefaultConvergenceCriteria<Scalar>::Ptr
-      getConvergeCriteria ()
-      {
-        return convergence_criteria_;
-      }
-
-      /** \brief Provide a pointer to the input source 
-        * (e.g., the point cloud that we want to align to the target)
-        *
-        * \param[in] cloud the input point cloud source
-        */
-      void
-      setInputSource (const PointCloudSourceConstPtr &cloud) override
-      {
-        Registration<PointSource, PointTarget, Scalar>::setInputSource (cloud);
-        const auto fields = pcl::getFields<PointSource> ();
-        source_has_normals_ = false;
-        for (const auto &field : fields)
-        {
-          if      (field.name == "x") x_idx_offset_ = field.offset;
-          else if (field.name == "y") y_idx_offset_ = field.offset;
-          else if (field.name == "z") z_idx_offset_ = field.offset;
-          else if (field.name == "normal_x") 
-          {
-            source_has_normals_ = true;
-            nx_idx_offset_ = field.offset;
-          }
-          else if (field.name == "normal_y") 
-          {
-            source_has_normals_ = true;
-            ny_idx_offset_ = field.offset;
-          }
-          else if (field.name == "normal_z") 
-          {
-            source_has_normals_ = true;
-            nz_idx_offset_ = field.offset;
-          }
-        }
-      }
-      
-      /** \brief Provide a pointer to the input target 
-        * (e.g., the point cloud that we want to align to the target)
-        *
-        * \param[in] cloud the input point cloud target
-        */
-      void
-      setInputTarget (const PointCloudTargetConstPtr &cloud) override
-      {
-        Registration<PointSource, PointTarget, Scalar>::setInputTarget (cloud);
-        const auto fields = pcl::getFields<PointSource> ();
-        target_has_normals_ = false;
-        for (const auto &field : fields)
-        {
-          if (field.name == "normal_x" || field.name == "normal_y" || field.name == "normal_z") 
-          {
-            target_has_normals_ = true;
-            break;
-          }
-        }
-      }
-
-      /** \brief Set whether to use reciprocal correspondence or not
-        *
-        * \param[in] use_reciprocal_correspondence whether to use reciprocal correspondence or not
-        */
-      inline void
-      setUseReciprocalCorrespondences (bool use_reciprocal_correspondence)
-      {
-        use_reciprocal_correspondence_ = use_reciprocal_correspondence;
-      }
-
-      /** \brief Obtain whether reciprocal correspondence are used or not */
-      inline bool
-      getUseReciprocalCorrespondences () const
-      {
-        return (use_reciprocal_correspondence_);
-      }
-
-    protected:
-
-      /** \brief Apply a rigid transform to a given dataset. Here we check whether whether
-        * the dataset has surface normals in addition to XYZ, and rotate normals as well.
-        * \param[in] input the input point cloud
-        * \param[out] output the resultant output point cloud
-        * \param[in] transform a 4x4 rigid transformation
-        * \note Can be used with cloud_in equal to cloud_out
-        */
-      virtual void 
-      transformCloud (const PointCloudSource &input, 
-                      PointCloudSource &output, 
-                      const Matrix4 &transform);
-
-      /** \brief Rigid transformation computation method  with initial guess.
-        * \param output the transformed input point cloud dataset using the rigid transformation found
-        * \param guess the initial guess of the transformation to compute
-        */
-      void 
-      computeTransformation (PointCloudSource &output, const Matrix4 &guess) override;
-
-      /** \brief Looks at the Estimators and Rejectors and determines whether their blob-setter methods need to be called */
-      virtual void
-      determineRequiredBlobData ();
-
-      /** \brief XYZ fields offset. */
-      std::size_t x_idx_offset_, y_idx_offset_, z_idx_offset_;
-
-      /** \brief Normal fields offset. */
-      std::size_t nx_idx_offset_, ny_idx_offset_, nz_idx_offset_;
-
-      /** \brief The correspondence type used for correspondence estimation. */
-      bool use_reciprocal_correspondence_;
-
-      /** \brief Internal check whether source dataset has normals or not. */
-      bool source_has_normals_;
-      /** \brief Internal check whether target dataset has normals or not. */
-      bool target_has_normals_;
-
-      /** \brief Checks for whether estimators and rejectors need various data */
-      bool need_source_blob_, need_target_blob_;
+    reg_name_ = "IterativeClosestPoint";
+    transformation_estimation_.reset(
+        new pcl::registration::
+            TransformationEstimationSVD<PointSource, PointTarget, Scalar>());
+    correspondence_estimation_.reset(
+        new pcl::registration::
+            CorrespondenceEstimation<PointSource, PointTarget, Scalar>);
+    convergence_criteria_.reset(
+        new pcl::registration::DefaultConvergenceCriteria<Scalar>(
+            nr_iterations_, transformation_, *correspondences_));
   };
 
-  /** \brief @b IterativeClosestPointWithNormals is a special case of
-    * IterativeClosestPoint, that uses a transformation estimated based on
-    * Point to Plane distances by default.
-    *
-    * By default, this implementation uses the traditional point to plane objective
-    * and computes point to plane distances using the normals of the target point
-    * cloud. It also provides the option (through setUseSymmetricObjective) of
-    * using the symmetric objective function of [Rusinkiewicz 2019]. This objective
-    * uses the normals of both the source and target point cloud and has a similar
-    * computational cost to the traditional point to plane objective while also
-    * offering improved convergence speed and a wider basin of convergence.
-    *
-    * Note that this implementation not demean the point clouds which can lead
-    * to increased numerical error. If desired, a user can demean the point cloud,
-    * run iterative closest point, and composite the resulting ICP transformation
-    * with the translations from demeaning to obtain a transformation between
-    * the original point clouds.
-    *
-    * \author Radu B. Rusu, Matthew Cong
-    * \ingroup registration
-    */
-  template <typename PointSource, typename PointTarget, typename Scalar = float>
-  class IterativeClosestPointWithNormals : public IterativeClosestPoint<PointSource, PointTarget, Scalar>
+  /**
+   * \brief Due to `convergence_criteria_` holding references to the class members,
+   * it is tricky to correctly implement its copy and move operations correctly. This
+   * can result in subtle bugs and to prevent them, these operations for ICP have
+   * been disabled.
+   *
+   * \todo: remove deleted ctors and assignments operations after resolving the issue
+   */
+  IterativeClosestPoint(const IterativeClosestPoint&) = delete;
+  IterativeClosestPoint(IterativeClosestPoint&&) = delete;
+  IterativeClosestPoint&
+  operator=(const IterativeClosestPoint&) = delete;
+  IterativeClosestPoint&
+  operator=(IterativeClosestPoint&&) = delete;
+
+  /** \brief Empty destructor */
+  ~IterativeClosestPoint() {}
+
+  /** \brief Returns a pointer to the DefaultConvergenceCriteria used by the
+   * IterativeClosestPoint class. This allows to check the convergence state after the
+   * align() method as well as to configure DefaultConvergenceCriteria's parameters not
+   * available through the ICP API before the align() method is called. Please note that
+   * the align method sets max_iterations_, euclidean_fitness_epsilon_ and
+   * transformation_epsilon_ and therefore overrides the default / set values of the
+   * DefaultConvergenceCriteria instance. \return Pointer to the IterativeClosestPoint's
+   * DefaultConvergenceCriteria.
+   */
+  inline typename pcl::registration::DefaultConvergenceCriteria<Scalar>::Ptr
+  getConvergeCriteria()
+  {
+    return convergence_criteria_;
+  }
+
+  /** \brief Provide a pointer to the input source
+   * (e.g., the point cloud that we want to align to the target)
+   *
+   * \param[in] cloud the input point cloud source
+   */
+  void
+  setInputSource(const PointCloudSourceConstPtr& cloud) override
   {
-    public:
-      using PointCloudSource = typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::PointCloudSource;
-      using PointCloudTarget = typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::PointCloudTarget;
-      using Matrix4 = typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::Matrix4;
-
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::reg_name_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::transformation_estimation_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::correspondence_rejectors_;
-
-      using Ptr = shared_ptr<IterativeClosestPoint<PointSource, PointTarget, Scalar> >;
-      using ConstPtr = shared_ptr<const IterativeClosestPoint<PointSource, PointTarget, Scalar> >;
-
-      /** \brief Empty constructor. */
-      IterativeClosestPointWithNormals ()
-      {
-        reg_name_ = "IterativeClosestPointWithNormals";
-        setUseSymmetricObjective (false);
-        setEnforceSameDirectionNormals (true);
-        //correspondence_rejectors_.add
-      };
-      
-      /** \brief Empty destructor */
-      virtual ~IterativeClosestPointWithNormals () {}
-
-      /** \brief Set whether to use a symmetric objective function or not
-        *
-        * \param[in] use_symmetric_objective whether to use a symmetric objective function or not
-        */
-      inline void
-      setUseSymmetricObjective (bool use_symmetric_objective)
-      {
-        use_symmetric_objective_ = use_symmetric_objective;
-        if (use_symmetric_objective_)
-        {
-            auto symmetric_transformation_estimation = pcl::make_shared<pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar> > ();
-            symmetric_transformation_estimation->setEnforceSameDirectionNormals (enforce_same_direction_normals_);
-            transformation_estimation_ = symmetric_transformation_estimation;
-        }
-        else
-        {
-            transformation_estimation_.reset (new pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar> ());
-        }
+    Registration<PointSource, PointTarget, Scalar>::setInputSource(cloud);
+    const auto fields = pcl::getFields<PointSource>();
+    source_has_normals_ = false;
+    for (const auto& field : fields) {
+      if (field.name == "x")
+        x_idx_offset_ = field.offset;
+      else if (field.name == "y")
+        y_idx_offset_ = field.offset;
+      else if (field.name == "z")
+        z_idx_offset_ = field.offset;
+      else if (field.name == "normal_x") {
+        source_has_normals_ = true;
+        nx_idx_offset_ = field.offset;
       }
-
-      /** \brief Obtain whether a symmetric objective is used or not */
-      inline bool
-      getUseSymmetricObjective () const
-      {
-        return use_symmetric_objective_;
+      else if (field.name == "normal_y") {
+        source_has_normals_ = true;
+        ny_idx_offset_ = field.offset;
       }
-
-        /** \brief Set whether or not to negate source or target normals on a per-point basis such that they point in the same direction. Only applicable to the symmetric objective function.
-        *
-        * \param[in] enforce_same_direction_normals whether to negate source or target normals on a per-point basis such that they point in the same direction.
-        */
-      inline void
-      setEnforceSameDirectionNormals (bool enforce_same_direction_normals)
-      {
-        enforce_same_direction_normals_ = enforce_same_direction_normals;
-        auto symmetric_transformation_estimation = dynamic_pointer_cast<pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar> >(transformation_estimation_);
-        if (symmetric_transformation_estimation)
-            symmetric_transformation_estimation->setEnforceSameDirectionNormals (enforce_same_direction_normals_);
+      else if (field.name == "normal_z") {
+        source_has_normals_ = true;
+        nz_idx_offset_ = field.offset;
       }
-
-      /** \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 () const
-      {
-        return enforce_same_direction_normals_;
+    }
+  }
+
+  /** \brief Provide a pointer to the input target
+   * (e.g., the point cloud that we want to align to the target)
+   *
+   * \param[in] cloud the input point cloud target
+   */
+  void
+  setInputTarget(const PointCloudTargetConstPtr& cloud) override
+  {
+    Registration<PointSource, PointTarget, Scalar>::setInputTarget(cloud);
+    const auto fields = pcl::getFields<PointSource>();
+    target_has_normals_ = false;
+    for (const auto& field : fields) {
+      if (field.name == "normal_x" || field.name == "normal_y" ||
+          field.name == "normal_z") {
+        target_has_normals_ = true;
+        break;
       }
+    }
+  }
+
+  /** \brief Set whether to use reciprocal correspondence or not
+   *
+   * \param[in] use_reciprocal_correspondence whether to use reciprocal correspondence
+   * or not
+   */
+  inline void
+  setUseReciprocalCorrespondences(bool use_reciprocal_correspondence)
+  {
+    use_reciprocal_correspondence_ = use_reciprocal_correspondence;
+  }
 
-    protected:
-
-      /** \brief Apply a rigid transform to a given dataset
-        * \param[in] input the input point cloud
-        * \param[out] output the resultant output point cloud
-        * \param[in] transform a 4x4 rigid transformation
-        * \note Can be used with cloud_in equal to cloud_out
-        */
-      virtual void 
-      transformCloud (const PointCloudSource &input, 
-                      PointCloudSource &output, 
-                      const Matrix4 &transform);
-
-      /** \brief Type of objective function (asymmetric vs. symmetric) used for transform estimation */
-      bool use_symmetric_objective_;
-      /** \brief Whether or not to negate source and/or target normals such that they point in the same direction in the symmetric objective function */
-      bool enforce_same_direction_normals_;
+  /** \brief Obtain whether reciprocal correspondence are used or not */
+  inline bool
+  getUseReciprocalCorrespondences() const
+  {
+    return (use_reciprocal_correspondence_);
+  }
+
+protected:
+  /** \brief Apply a rigid transform to a given dataset. Here we check whether
+   * the dataset has surface normals in addition to XYZ, and rotate normals as well.
+   * \param[in] input the input point cloud
+   * \param[out] output the resultant output point cloud
+   * \param[in] transform a 4x4 rigid transformation
+   * \note Can be used with cloud_in equal to cloud_out
+   */
+  virtual void
+  transformCloud(const PointCloudSource& input,
+                 PointCloudSource& output,
+                 const Matrix4& transform);
+
+  /** \brief Rigid transformation computation method  with initial guess.
+   * \param output the transformed input point cloud dataset using the rigid
+   * transformation found \param guess the initial guess of the transformation to
+   * compute
+   */
+  void
+  computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
+
+  /** \brief Looks at the Estimators and Rejectors and determines whether their
+   * blob-setter methods need to be called */
+  virtual void
+  determineRequiredBlobData();
+
+  /** \brief XYZ fields offset. */
+  std::size_t x_idx_offset_, y_idx_offset_, z_idx_offset_;
+
+  /** \brief Normal fields offset. */
+  std::size_t nx_idx_offset_, ny_idx_offset_, nz_idx_offset_;
+
+  /** \brief The correspondence type used for correspondence estimation. */
+  bool use_reciprocal_correspondence_;
+
+  /** \brief Internal check whether source dataset has normals or not. */
+  bool source_has_normals_;
+  /** \brief Internal check whether target dataset has normals or not. */
+  bool target_has_normals_;
+
+  /** \brief Checks for whether estimators and rejectors need various data */
+  bool need_source_blob_, need_target_blob_;
+};
+
+/** \brief @b IterativeClosestPointWithNormals is a special case of
+ * IterativeClosestPoint, that uses a transformation estimated based on
+ * Point to Plane distances by default.
+ *
+ * By default, this implementation uses the traditional point to plane objective
+ * and computes point to plane distances using the normals of the target point
+ * cloud. It also provides the option (through setUseSymmetricObjective) of
+ * using the symmetric objective function of [Rusinkiewicz 2019]. This objective
+ * uses the normals of both the source and target point cloud and has a similar
+ * computational cost to the traditional point to plane objective while also
+ * offering improved convergence speed and a wider basin of convergence.
+ *
+ * Note that this implementation not demean the point clouds which can lead
+ * to increased numerical error. If desired, a user can demean the point cloud,
+ * run iterative closest point, and composite the resulting ICP transformation
+ * with the translations from demeaning to obtain a transformation between
+ * the original point clouds.
+ *
+ * \author Radu B. Rusu, Matthew Cong
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename Scalar = float>
+class IterativeClosestPointWithNormals
+: public IterativeClosestPoint<PointSource, PointTarget, Scalar> {
+public:
+  using PointCloudSource = typename IterativeClosestPoint<PointSource,
+                                                          PointTarget,
+                                                          Scalar>::PointCloudSource;
+  using PointCloudTarget = typename IterativeClosestPoint<PointSource,
+                                                          PointTarget,
+                                                          Scalar>::PointCloudTarget;
+  using Matrix4 =
+      typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::Matrix4;
+
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::reg_name_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
+      transformation_estimation_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
+      correspondence_rejectors_;
+
+  using Ptr = shared_ptr<IterativeClosestPoint<PointSource, PointTarget, Scalar>>;
+  using ConstPtr =
+      shared_ptr<const IterativeClosestPoint<PointSource, PointTarget, Scalar>>;
+
+  /** \brief Empty constructor. */
+  IterativeClosestPointWithNormals()
+  {
+    reg_name_ = "IterativeClosestPointWithNormals";
+    setUseSymmetricObjective(false);
+    setEnforceSameDirectionNormals(true);
+    // correspondence_rejectors_.add
   };
 
-}
+  /** \brief Empty destructor */
+  virtual ~IterativeClosestPointWithNormals() {}
+
+  /** \brief Set whether to use a symmetric objective function or not
+   *
+   * \param[in] use_symmetric_objective whether to use a symmetric objective function or
+   * not
+   */
+  inline void
+  setUseSymmetricObjective(bool use_symmetric_objective)
+  {
+    use_symmetric_objective_ = use_symmetric_objective;
+    if (use_symmetric_objective_) {
+      auto symmetric_transformation_estimation = pcl::make_shared<
+          pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<
+              PointSource,
+              PointTarget,
+              Scalar>>();
+      symmetric_transformation_estimation->setEnforceSameDirectionNormals(
+          enforce_same_direction_normals_);
+      transformation_estimation_ = symmetric_transformation_estimation;
+    }
+    else {
+      transformation_estimation_.reset(
+          new pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource,
+                                                                         PointTarget,
+                                                                         Scalar>());
+    }
+  }
+
+  /** \brief Obtain whether a symmetric objective is used or not */
+  inline bool
+  getUseSymmetricObjective() const
+  {
+    return use_symmetric_objective_;
+  }
+
+  /** \brief Set whether or not to negate source or target normals on a per-point basis
+   * such that they point in the same direction. Only applicable to the symmetric
+   * objective function.
+   *
+   * \param[in] enforce_same_direction_normals whether to negate source or target
+   * normals on a per-point basis such that they point in the same direction.
+   */
+  inline void
+  setEnforceSameDirectionNormals(bool enforce_same_direction_normals)
+  {
+    enforce_same_direction_normals_ = enforce_same_direction_normals;
+    auto symmetric_transformation_estimation = dynamic_pointer_cast<
+        pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<PointSource,
+                                                                            PointTarget,
+                                                                            Scalar>>(
+        transformation_estimation_);
+    if (symmetric_transformation_estimation)
+      symmetric_transformation_estimation->setEnforceSameDirectionNormals(
+          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() const
+  {
+    return enforce_same_direction_normals_;
+  }
+
+protected:
+  /** \brief Apply a rigid transform to a given dataset
+   * \param[in] input the input point cloud
+   * \param[out] output the resultant output point cloud
+   * \param[in] transform a 4x4 rigid transformation
+   * \note Can be used with cloud_in equal to cloud_out
+   */
+  virtual void
+  transformCloud(const PointCloudSource& input,
+                 PointCloudSource& output,
+                 const Matrix4& transform);
+
+  /** \brief Type of objective function (asymmetric vs. symmetric) used for transform
+   * estimation */
+  bool use_symmetric_objective_;
+  /** \brief Whether or not to negate source and/or target normals such that they point
+   * in the same direction in the symmetric objective function */
+  bool enforce_same_direction_normals_;
+};
+
+} // namespace pcl
 
 #include <pcl/registration/impl/icp.hpp>
diff --git a/registration/include/pcl/registration/icp_nl.h b/registration/include/pcl/registration/icp_nl.h
index 4bffbd9a..8bead9be 100644
--- a/registration/include/pcl/registration/icp_nl.h
+++ b/registration/include/pcl/registration/icp_nl.h
@@ -44,47 +44,52 @@
 #include <pcl/registration/icp.h>
 #include <pcl/registration/transformation_estimation_lm.h>
 
-namespace pcl
-{
-  /** \brief @b IterativeClosestPointNonLinear is an ICP variant that uses Levenberg-Marquardt optimization 
-    * backend. The resultant transformation is optimized as a quaternion.
-    *
-    * The algorithm has several termination criteria:
-    *
-    * <ol>
-    * <li>Number of iterations has reached the maximum user imposed number of iterations 
-    *     (via \ref setMaximumIterations)</li>
-    * <li>The epsilon (difference) between the previous transformation and the current estimated transformation is 
-    *     smaller than an user imposed value (via \ref setTransformationEpsilon)</li>
-    * <li>The sum of Euclidean squared errors is smaller than a user defined threshold 
-    *     (via \ref setEuclideanFitnessEpsilon)</li>
-    * </ol>
-    *
-    * \author Radu B. Rusu, Michael Dixon
-    * \ingroup registration
-    */
-  template <typename PointSource, typename PointTarget, typename Scalar = float>
-  class IterativeClosestPointNonLinear : public IterativeClosestPoint<PointSource, PointTarget, Scalar>
-  {
-    using IterativeClosestPoint<PointSource, PointTarget, Scalar>::min_number_correspondences_;
-    using IterativeClosestPoint<PointSource, PointTarget, Scalar>::reg_name_;
-    using IterativeClosestPoint<PointSource, PointTarget, Scalar>::transformation_estimation_;
-    using IterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation;
-
-    public:
+namespace pcl {
+/** \brief @b IterativeClosestPointNonLinear is an ICP variant that uses
+ * Levenberg-Marquardt optimization backend. The resultant transformation is optimized
+ * as a quaternion.
+ *
+ * The algorithm has several termination criteria:
+ *
+ * <ol>
+ * <li>Number of iterations has reached the maximum user imposed number of iterations
+ *     (via \ref setMaximumIterations)</li>
+ * <li>The epsilon (difference) between the previous transformation and the current
+ * estimated transformation is smaller than an user imposed value (via \ref
+ * setTransformationEpsilon)</li> <li>The sum of Euclidean squared errors is smaller
+ * than a user defined threshold (via \ref setEuclideanFitnessEpsilon)</li>
+ * </ol>
+ *
+ * \author Radu B. Rusu, Michael Dixon
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename Scalar = float>
+class IterativeClosestPointNonLinear
+: public IterativeClosestPoint<PointSource, PointTarget, Scalar> {
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
+      min_number_correspondences_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::reg_name_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
+      transformation_estimation_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation;
 
-      using Ptr = shared_ptr< IterativeClosestPointNonLinear<PointSource, PointTarget, Scalar> >;
-      using ConstPtr = shared_ptr< const IterativeClosestPointNonLinear<PointSource, PointTarget, Scalar> >;
+public:
+  using Ptr =
+      shared_ptr<IterativeClosestPointNonLinear<PointSource, PointTarget, Scalar>>;
+  using ConstPtr = shared_ptr<
+      const IterativeClosestPointNonLinear<PointSource, PointTarget, Scalar>>;
 
-      using Matrix4 = typename Registration<PointSource, PointTarget, Scalar>::Matrix4;
+  using Matrix4 = typename Registration<PointSource, PointTarget, Scalar>::Matrix4;
 
-      /** \brief Empty constructor. */
-      IterativeClosestPointNonLinear ()
-      {
-        min_number_correspondences_ = 4;
-        reg_name_ = "IterativeClosestPointNonLinear";
+  /** \brief Empty constructor. */
+  IterativeClosestPointNonLinear()
+  {
+    min_number_correspondences_ = 4;
+    reg_name_ = "IterativeClosestPointNonLinear";
 
-        transformation_estimation_.reset (new pcl::registration::TransformationEstimationLM<PointSource, PointTarget, Scalar>);
-      }
-  };
-}
+    transformation_estimation_.reset(
+        new pcl::registration::
+            TransformationEstimationLM<PointSource, PointTarget, Scalar>);
+  }
+};
+} // namespace pcl
diff --git a/registration/include/pcl/registration/impl/correspondence_estimation.hpp b/registration/include/pcl/registration/impl/correspondence_estimation.hpp
index 2910238d..19cded53 100644
--- a/registration/include/pcl/registration/impl/correspondence_estimation.hpp
+++ b/registration/include/pcl/registration/impl/correspondence_estimation.hpp
@@ -41,73 +41,70 @@
 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_
 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_
 
-#include <pcl/common/io.h>
 #include <pcl/common/copy_point.h>
+#include <pcl/common/io.h>
 
+namespace pcl {
 
-namespace pcl
-{
-
-namespace registration
-{
+namespace registration {
 
-template <typename PointSource, typename PointTarget, typename Scalar> void
-CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::setInputTarget (
-    const PointCloudTargetConstPtr &cloud)
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
+CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::setInputTarget(
+    const PointCloudTargetConstPtr& cloud)
 {
-  if (cloud->points.empty ())
-  {
-    PCL_ERROR ("[pcl::registration::%s::setInputTarget] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
+  if (cloud->points.empty()) {
+    PCL_ERROR("[pcl::registration::%s::setInputTarget] Invalid or empty point cloud "
+              "dataset given!\n",
+              getClassName().c_str());
     return;
   }
   target_ = cloud;
 
   // Set the internal point representation of choice
   if (point_representation_)
-    tree_->setPointRepresentation (point_representation_);
+    tree_->setPointRepresentation(point_representation_);
 
   target_cloud_updated_ = true;
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> bool
-CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute ()
+template <typename PointSource, typename PointTarget, typename Scalar>
+bool
+CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute()
 {
-  if (!target_)
-  {
-    PCL_ERROR ("[pcl::registration::%s::compute] No input target dataset was given!\n", getClassName ().c_str ());
+  if (!target_) {
+    PCL_ERROR("[pcl::registration::%s::compute] No input target dataset was given!\n",
+              getClassName().c_str());
     return (false);
   }
 
   // Only update target kd-tree if a new target cloud was set
-  if (target_cloud_updated_ && !force_no_recompute_)
-  {
+  if (target_cloud_updated_ && !force_no_recompute_) {
     // If the target indices have been given via setIndicesTarget
     if (target_indices_)
-      tree_->setInputCloud (target_, target_indices_);
+      tree_->setInputCloud(target_, target_indices_);
     else
-      tree_->setInputCloud (target_);
+      tree_->setInputCloud(target_);
 
     target_cloud_updated_ = false;
   }
 
-  return (PCLBase<PointSource>::initCompute ());
+  return (PCLBase<PointSource>::initCompute());
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> bool
-CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initComputeReciprocal ()
+template <typename PointSource, typename PointTarget, typename Scalar>
+bool
+CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initComputeReciprocal()
 {
   // Only update source kd-tree if a new target cloud was set
-  if (source_cloud_updated_ && !force_no_recompute_reciprocal_)
-  {
+  if (source_cloud_updated_ && !force_no_recompute_reciprocal_) {
     if (point_representation_)
-      tree_reciprocal_->setPointRepresentation (point_representation_);
+      tree_reciprocal_->setPointRepresentation(point_representation_);
     // If the target indices have been given via setIndicesTarget
     if (indices_)
-      tree_reciprocal_->setInputCloud (getInputSource(), getIndicesSource());
+      tree_reciprocal_->setInputCloud(getInputSource(), getIndicesSource());
     else
-      tree_reciprocal_->setInputCloud (getInputSource());
+      tree_reciprocal_->setInputCloud(getInputSource());
 
     source_cloud_updated_ = false;
   }
@@ -115,70 +112,69 @@ CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initComputeRecip
   return (true);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> void
-CorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineCorrespondences (
-    pcl::Correspondences &correspondences, double max_distance)
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
+CorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineCorrespondences(
+    pcl::Correspondences& correspondences, double max_distance)
 {
-  if (!initCompute ())
+  if (!initCompute())
     return;
 
   double max_dist_sqr = max_distance * max_distance;
 
-  correspondences.resize (indices_->size ());
+  correspondences.resize(indices_->size());
 
-  std::vector<int> index (1);
-  std::vector<float> distance (1);
+  pcl::Indices index(1);
+  std::vector<float> 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<PointSource, PointTarget> ())
-  {
+  // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT
+  // macro!
+  if (isSamePointType<PointSource, PointTarget>()) {
     // Iterate over the input set of source indices
-    for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
-    {
-      tree_->nearestKSearch ((*input_)[*idx], 1, index, distance);
+    for (const auto& idx : (*indices_)) {
+      tree_->nearestKSearch((*input_)[idx], 1, index, distance);
       if (distance[0] > max_dist_sqr)
         continue;
 
-      corr.index_query = *idx;
+      corr.index_query = idx;
       corr.index_match = index[0];
       corr.distance = distance[0];
       correspondences[nr_valid_correspondences++] = corr;
     }
   }
-  else
-  {
+  else {
     PointTarget pt;
 
     // Iterate over the input set of source indices
-    for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
-    {
-      // Copy the source data to a target PointTarget format so we can search in the tree
-      copyPoint ((*input_)[*idx], pt);
+    for (const auto& idx : (*indices_)) {
+      // Copy the source data to a target PointTarget format so we can search in the
+      // tree
+      copyPoint((*input_)[idx], pt);
 
-      tree_->nearestKSearch (pt, 1, index, distance);
+      tree_->nearestKSearch(pt, 1, index, distance);
       if (distance[0] > max_dist_sqr)
         continue;
 
-      corr.index_query = *idx;
+      corr.index_query = idx;
       corr.index_match = index[0];
       corr.distance = distance[0];
       correspondences[nr_valid_correspondences++] = corr;
     }
   }
-  correspondences.resize (nr_valid_correspondences);
-  deinitCompute ();
+  correspondences.resize(nr_valid_correspondences);
+  deinitCompute();
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> void
-CorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineReciprocalCorrespondences (
-    pcl::Correspondences &correspondences, double max_distance)
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
+CorrespondenceEstimation<PointSource, PointTarget, Scalar>::
+    determineReciprocalCorrespondences(pcl::Correspondences& correspondences,
+                                       double max_distance)
 {
-  if (!initCompute ())
+  if (!initCompute())
     return;
 
   // setup tree for reciprocal search
@@ -187,76 +183,77 @@ CorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineReciprocalC
     return;
   double max_dist_sqr = max_distance * max_distance;
 
-  correspondences.resize (indices_->size());
-  std::vector<int> index (1);
-  std::vector<float> distance (1);
-  std::vector<int> index_reciprocal (1);
-  std::vector<float> distance_reciprocal (1);
+  correspondences.resize(indices_->size());
+  pcl::Indices index(1);
+  std::vector<float> distance(1);
+  pcl::Indices index_reciprocal(1);
+  std::vector<float> distance_reciprocal(1);
   pcl::Correspondence corr;
   unsigned int nr_valid_correspondences = 0;
   int target_idx = 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<PointSource, PointTarget> ())
-  {
+  // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT
+  // macro!
+  if (isSamePointType<PointSource, PointTarget>()) {
     // Iterate over the input set of source indices
-    for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
-    {
-      tree_->nearestKSearch ((*input_)[*idx], 1, index, distance);
+    for (const auto& idx : (*indices_)) {
+      tree_->nearestKSearch((*input_)[idx], 1, index, distance);
       if (distance[0] > max_dist_sqr)
         continue;
 
       target_idx = index[0];
 
-      tree_reciprocal_->nearestKSearch ((*target_)[target_idx], 1, index_reciprocal, distance_reciprocal);
-      if (distance_reciprocal[0] > max_dist_sqr || *idx != index_reciprocal[0])
+      tree_reciprocal_->nearestKSearch(
+          (*target_)[target_idx], 1, index_reciprocal, distance_reciprocal);
+      if (distance_reciprocal[0] > max_dist_sqr || idx != index_reciprocal[0])
         continue;
 
-      corr.index_query = *idx;
+      corr.index_query = idx;
       corr.index_match = index[0];
       corr.distance = distance[0];
       correspondences[nr_valid_correspondences++] = corr;
     }
   }
-  else
-  {
+  else {
     PointTarget pt_src;
     PointSource pt_tgt;
 
     // Iterate over the input set of source indices
-    for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
-    {
-      // Copy the source data to a target PointTarget format so we can search in the tree
-      copyPoint ((*input_)[*idx], pt_src);
+    for (const auto& idx : (*indices_)) {
+      // Copy the source data to a target PointTarget format so we can search in the
+      // tree
+      copyPoint((*input_)[idx], pt_src);
 
-      tree_->nearestKSearch (pt_src, 1, index, distance);
+      tree_->nearestKSearch(pt_src, 1, index, distance);
       if (distance[0] > max_dist_sqr)
         continue;
 
       target_idx = index[0];
 
-      // Copy the target data to a target PointSource format so we can search in the tree_reciprocal
-      copyPoint ((*target_)[target_idx], pt_tgt);
+      // Copy the target data to a target PointSource format so we can search in the
+      // tree_reciprocal
+      copyPoint((*target_)[target_idx], pt_tgt);
 
-      tree_reciprocal_->nearestKSearch (pt_tgt, 1, index_reciprocal, distance_reciprocal);
-      if (distance_reciprocal[0] > max_dist_sqr || *idx != index_reciprocal[0])
+      tree_reciprocal_->nearestKSearch(
+          pt_tgt, 1, index_reciprocal, distance_reciprocal);
+      if (distance_reciprocal[0] > max_dist_sqr || idx != index_reciprocal[0])
         continue;
 
-      corr.index_query = *idx;
+      corr.index_query = idx;
       corr.index_match = index[0];
       corr.distance = distance[0];
       correspondences[nr_valid_correspondences++] = corr;
     }
   }
-  correspondences.resize (nr_valid_correspondences);
-  deinitCompute ();
+  correspondences.resize(nr_valid_correspondences);
+  deinitCompute();
 }
 
 } // namespace registration
 } // namespace pcl
 
-//#define PCL_INSTANTIATE_CorrespondenceEstimation(T,U) template class PCL_EXPORTS pcl::registration::CorrespondenceEstimation<T,U>;
+//#define PCL_INSTANTIATE_CorrespondenceEstimation(T,U) template class PCL_EXPORTS
+// pcl::registration::CorrespondenceEstimation<T,U>;
 
 #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 5912870b..1970ed6c 100644
--- a/registration/include/pcl/registration/impl/correspondence_estimation_backprojection.hpp
+++ b/registration/include/pcl/registration/impl/correspondence_estimation_backprojection.hpp
@@ -42,37 +42,39 @@
 
 #include <pcl/common/copy_point.h>
 
+namespace pcl {
 
-namespace pcl
-{
-
-namespace registration
-{
+namespace registration {
 
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> bool
-CorrespondenceEstimationBackProjection<PointSource, PointTarget, NormalT, Scalar>::initCompute ()
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+bool
+CorrespondenceEstimationBackProjection<PointSource, PointTarget, NormalT, Scalar>::
+    initCompute()
 {
-  if (!source_normals_ || !target_normals_)
-  {
-    PCL_WARN ("[pcl::registration::%s::initCompute] Datasets containing normals for source/target have not been given!\n", getClassName ().c_str ());
+  if (!source_normals_ || !target_normals_) {
+    PCL_WARN("[pcl::registration::%s::initCompute] Datasets containing normals for "
+             "source/target have not been given!\n",
+             getClassName().c_str());
     return (false);
   }
 
-  return (CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute ());
+  return (
+      CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute());
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
-CorrespondenceEstimationBackProjection<PointSource, PointTarget, NormalT, Scalar>::determineCorrespondences (
-    pcl::Correspondences &correspondences, double max_distance)
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+void
+CorrespondenceEstimationBackProjection<PointSource, PointTarget, NormalT, Scalar>::
+    determineCorrespondences(pcl::Correspondences& correspondences, double max_distance)
 {
-  if (!initCompute ())
+  if (!initCompute())
     return;
 
-  correspondences.resize (indices_->size ());
+  correspondences.resize(indices_->size());
 
-  std::vector<int> nn_indices (k_);
-  std::vector<float> nn_dists (k_);
+  pcl::Indices nn_indices(k_);
+  std::vector<float> nn_dists(k_);
 
   int min_index = 0;
 
@@ -80,102 +82,105 @@ CorrespondenceEstimationBackProjection<PointSource, PointTarget, NormalT, Scalar
   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<PointSource, PointTarget> ())
-  {
+  // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT
+  // macro!
+  if (isSamePointType<PointSource, PointTarget>()) {
     PointTarget pt;
     // Iterate over the input set of source indices
-    for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
-    {
-      tree_->nearestKSearch ((*input_)[*idx_i], k_, nn_indices, nn_dists);
+    for (const auto& idx_i : (*indices_)) {
+      tree_->nearestKSearch((*input_)[idx_i], k_, nn_indices, nn_dists);
 
-      // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
-      float min_dist = std::numeric_limits<float>::max ();
+      // Among the K nearest neighbours find the one with minimum perpendicular distance
+      // to the normal
+      float min_dist = std::numeric_limits<float>::max();
 
       // Find the best correspondence
-      for (std::size_t j = 0; j < nn_indices.size (); j++)
-      {
-        float cos_angle = (*source_normals_)[*idx_i].normal_x * (*target_normals_)[nn_indices[j]].normal_x +
-                          (*source_normals_)[*idx_i].normal_y * (*target_normals_)[nn_indices[j]].normal_y +
-                          (*source_normals_)[*idx_i].normal_z * (*target_normals_)[nn_indices[j]].normal_z ;
+      for (std::size_t j = 0; j < nn_indices.size(); j++) {
+        float cos_angle = (*source_normals_)[idx_i].normal_x *
+                              (*target_normals_)[nn_indices[j]].normal_x +
+                          (*source_normals_)[idx_i].normal_y *
+                              (*target_normals_)[nn_indices[j]].normal_y +
+                          (*source_normals_)[idx_i].normal_z *
+                              (*target_normals_)[nn_indices[j]].normal_z;
         float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
 
-        if (dist < min_dist)
-        {
+        if (dist < min_dist) {
           min_dist = dist;
-          min_index = static_cast<int> (j);
+          min_index = static_cast<int>(j);
         }
       }
       if (min_dist > max_distance)
         continue;
 
-      corr.index_query = *idx_i;
+      corr.index_query = idx_i;
       corr.index_match = nn_indices[min_index];
-      corr.distance = nn_dists[min_index];//min_dist;
+      corr.distance = nn_dists[min_index]; // min_dist;
       correspondences[nr_valid_correspondences++] = corr;
     }
   }
-  else
-  {
+  else {
     PointTarget pt;
 
     // Iterate over the input set of source indices
-    for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
-    {
-      tree_->nearestKSearch ((*input_)[*idx_i], k_, nn_indices, nn_dists);
+    for (const auto& idx_i : (*indices_)) {
+      tree_->nearestKSearch((*input_)[idx_i], k_, nn_indices, nn_dists);
 
-      // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
-      float min_dist = std::numeric_limits<float>::max ();
+      // Among the K nearest neighbours find the one with minimum perpendicular distance
+      // to the normal
+      float min_dist = std::numeric_limits<float>::max();
 
       // Find the best correspondence
-      for (std::size_t j = 0; j < nn_indices.size (); j++)
-      {
+      for (std::size_t j = 0; j < nn_indices.size(); j++) {
         PointSource pt_src;
-        // Copy the source data to a target PointTarget format so we can search in the tree
-        copyPoint ((*input_)[*idx_i], pt_src);
-
-        float cos_angle = (*source_normals_)[*idx_i].normal_x * (*target_normals_)[nn_indices[j]].normal_x +
-                          (*source_normals_)[*idx_i].normal_y * (*target_normals_)[nn_indices[j]].normal_y +
-                          (*source_normals_)[*idx_i].normal_z * (*target_normals_)[nn_indices[j]].normal_z ;
+        // Copy the source data to a target PointTarget format so we can search in the
+        // tree
+        copyPoint((*input_)[idx_i], pt_src);
+
+        float cos_angle = (*source_normals_)[idx_i].normal_x *
+                              (*target_normals_)[nn_indices[j]].normal_x +
+                          (*source_normals_)[idx_i].normal_y *
+                              (*target_normals_)[nn_indices[j]].normal_y +
+                          (*source_normals_)[idx_i].normal_z *
+                              (*target_normals_)[nn_indices[j]].normal_z;
         float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
 
-        if (dist < min_dist)
-        {
+        if (dist < min_dist) {
           min_dist = dist;
-          min_index = static_cast<int> (j);
+          min_index = static_cast<int>(j);
         }
       }
       if (min_dist > max_distance)
         continue;
 
-      corr.index_query = *idx_i;
+      corr.index_query = idx_i;
       corr.index_match = nn_indices[min_index];
-      corr.distance = nn_dists[min_index];//min_dist;
+      corr.distance = nn_dists[min_index]; // min_dist;
       correspondences[nr_valid_correspondences++] = corr;
     }
   }
-  correspondences.resize (nr_valid_correspondences);
-  deinitCompute ();
+  correspondences.resize(nr_valid_correspondences);
+  deinitCompute();
 }
 
-
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
-CorrespondenceEstimationBackProjection<PointSource, PointTarget, NormalT, Scalar>::determineReciprocalCorrespondences (
-    pcl::Correspondences &correspondences, double max_distance)
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+void
+CorrespondenceEstimationBackProjection<PointSource, PointTarget, NormalT, Scalar>::
+    determineReciprocalCorrespondences(pcl::Correspondences& correspondences,
+                                       double max_distance)
 {
-  if (!initCompute ())
+  if (!initCompute())
     return;
 
   // Set the internal point representation of choice
-  if(!initComputeReciprocal())
+  if (!initComputeReciprocal())
     return;
 
-  correspondences.resize (indices_->size ());
+  correspondences.resize(indices_->size());
 
-  std::vector<int> nn_indices (k_);
-  std::vector<float> nn_dists (k_);
-  std::vector<int> index_reciprocal (1);
-  std::vector<float> distance_reciprocal (1);
+  pcl::Indices nn_indices(k_);
+  std::vector<float> nn_dists(k_);
+  pcl::Indices index_reciprocal(1);
+  std::vector<float> distance_reciprocal(1);
 
   int min_index = 0;
 
@@ -184,30 +189,31 @@ CorrespondenceEstimationBackProjection<PointSource, PointTarget, NormalT, Scalar
   int target_idx = 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<PointSource, PointTarget> ())
-  {
+  // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT
+  // macro!
+  if (isSamePointType<PointSource, PointTarget>()) {
     PointTarget pt;
     // Iterate over the input set of source indices
-    for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
-    {
-      tree_->nearestKSearch ((*input_)[*idx_i], k_, nn_indices, nn_dists);
+    for (const auto& idx_i : (*indices_)) {
+      tree_->nearestKSearch((*input_)[idx_i], k_, nn_indices, nn_dists);
 
-      // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
-      float min_dist = std::numeric_limits<float>::max ();
+      // Among the K nearest neighbours find the one with minimum perpendicular distance
+      // to the normal
+      float min_dist = std::numeric_limits<float>::max();
 
       // Find the best correspondence
-      for (std::size_t j = 0; j < nn_indices.size (); j++)
-      {
-        float cos_angle = (*source_normals_)[*idx_i].normal_x * (*target_normals_)[nn_indices[j]].normal_x +
-                          (*source_normals_)[*idx_i].normal_y * (*target_normals_)[nn_indices[j]].normal_y +
-                          (*source_normals_)[*idx_i].normal_z * (*target_normals_)[nn_indices[j]].normal_z ;
+      for (std::size_t j = 0; j < nn_indices.size(); j++) {
+        float cos_angle = (*source_normals_)[idx_i].normal_x *
+                              (*target_normals_)[nn_indices[j]].normal_x +
+                          (*source_normals_)[idx_i].normal_y *
+                              (*target_normals_)[nn_indices[j]].normal_y +
+                          (*source_normals_)[idx_i].normal_z *
+                              (*target_normals_)[nn_indices[j]].normal_z;
         float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
 
-        if (dist < min_dist)
-        {
+        if (dist < min_dist) {
           min_dist = dist;
-          min_index = static_cast<int> (j);
+          min_index = static_cast<int>(j);
         }
       }
       if (min_dist > max_distance)
@@ -215,45 +221,47 @@ CorrespondenceEstimationBackProjection<PointSource, PointTarget, NormalT, Scalar
 
       // Check if the correspondence is reciprocal
       target_idx = nn_indices[min_index];
-      tree_reciprocal_->nearestKSearch ((*target_)[target_idx], 1, index_reciprocal, distance_reciprocal);
+      tree_reciprocal_->nearestKSearch(
+          (*target_)[target_idx], 1, index_reciprocal, distance_reciprocal);
 
-      if (*idx_i != index_reciprocal[0])
+      if (idx_i != index_reciprocal[0])
         continue;
 
-      corr.index_query = *idx_i;
+      corr.index_query = idx_i;
       corr.index_match = nn_indices[min_index];
-      corr.distance = nn_dists[min_index];//min_dist;
+      corr.distance = nn_dists[min_index]; // min_dist;
       correspondences[nr_valid_correspondences++] = corr;
     }
   }
-  else
-  {
+  else {
     PointTarget pt;
 
     // Iterate over the input set of source indices
-    for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
-    {
-      tree_->nearestKSearch ((*input_)[*idx_i], k_, nn_indices, nn_dists);
+    for (const auto& idx_i : (*indices_)) {
+      tree_->nearestKSearch((*input_)[idx_i], k_, nn_indices, nn_dists);
 
-      // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
-      float min_dist = std::numeric_limits<float>::max ();
+      // Among the K nearest neighbours find the one with minimum perpendicular distance
+      // to the normal
+      float min_dist = std::numeric_limits<float>::max();
 
       // Find the best correspondence
-      for (std::size_t j = 0; j < nn_indices.size (); j++)
-      {
+      for (std::size_t j = 0; j < nn_indices.size(); j++) {
         PointSource pt_src;
-        // Copy the source data to a target PointTarget format so we can search in the tree
-        copyPoint ((*input_)[*idx_i], pt_src);
-
-        float cos_angle = (*source_normals_)[*idx_i].normal_x * (*target_normals_)[nn_indices[j]].normal_x +
-                          (*source_normals_)[*idx_i].normal_y * (*target_normals_)[nn_indices[j]].normal_y +
-                          (*source_normals_)[*idx_i].normal_z * (*target_normals_)[nn_indices[j]].normal_z ;
+        // Copy the source data to a target PointTarget format so we can search in the
+        // tree
+        copyPoint((*input_)[idx_i], pt_src);
+
+        float cos_angle = (*source_normals_)[idx_i].normal_x *
+                              (*target_normals_)[nn_indices[j]].normal_x +
+                          (*source_normals_)[idx_i].normal_y *
+                              (*target_normals_)[nn_indices[j]].normal_y +
+                          (*source_normals_)[idx_i].normal_z *
+                              (*target_normals_)[nn_indices[j]].normal_z;
         float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
 
-        if (dist < min_dist)
-        {
+        if (dist < min_dist) {
           min_dist = dist;
-          min_index = static_cast<int> (j);
+          min_index = static_cast<int>(j);
         }
       }
       if (min_dist > max_distance)
@@ -261,22 +269,23 @@ CorrespondenceEstimationBackProjection<PointSource, PointTarget, NormalT, Scalar
 
       // Check if the correspondence is reciprocal
       target_idx = nn_indices[min_index];
-      tree_reciprocal_->nearestKSearch ((*target_)[target_idx], 1, index_reciprocal, distance_reciprocal);
+      tree_reciprocal_->nearestKSearch(
+          (*target_)[target_idx], 1, index_reciprocal, distance_reciprocal);
 
-      if (*idx_i != index_reciprocal[0])
+      if (idx_i != index_reciprocal[0])
         continue;
 
-      corr.index_query = *idx_i;
+      corr.index_query = idx_i;
       corr.index_match = nn_indices[min_index];
-      corr.distance = nn_dists[min_index];//min_dist;
+      corr.distance = nn_dists[min_index]; // min_dist;
       correspondences[nr_valid_correspondences++] = corr;
     }
   }
-  correspondences.resize (nr_valid_correspondences);
-  deinitCompute ();
+  correspondences.resize(nr_valid_correspondences);
+  deinitCompute();
 }
 
 } // namespace registration
 } // namespace pcl
 
-#endif    // PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_
+#endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_
diff --git a/registration/include/pcl/registration/impl/correspondence_estimation_normal_shooting.hpp b/registration/include/pcl/registration/impl/correspondence_estimation_normal_shooting.hpp
index a1a99775..16f9daf0 100644
--- a/registration/include/pcl/registration/impl/correspondence_estimation_normal_shooting.hpp
+++ b/registration/include/pcl/registration/impl/correspondence_estimation_normal_shooting.hpp
@@ -43,37 +43,38 @@
 
 #include <pcl/common/copy_point.h>
 
+namespace pcl {
 
-namespace pcl
-{
-
-namespace registration
-{
+namespace registration {
 
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> bool
-CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar>::initCompute ()
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+bool
+CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar>::
+    initCompute()
 {
-  if (!source_normals_)
-  {
-    PCL_WARN ("[pcl::registration::%s::initCompute] Datasets containing normals for source have not been given!\n", getClassName ().c_str ());
+  if (!source_normals_) {
+    PCL_WARN("[pcl::registration::%s::initCompute] Datasets containing normals for "
+             "source have not been given!\n",
+             getClassName().c_str());
     return (false);
   }
 
-  return (CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute ());
+  return (
+      CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute());
 }
 
-
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
-CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar>::determineCorrespondences (
-    pcl::Correspondences &correspondences, double max_distance)
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+void
+CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar>::
+    determineCorrespondences(pcl::Correspondences& correspondences, double max_distance)
 {
-  if (!initCompute ())
+  if (!initCompute())
     return;
 
-  correspondences.resize (indices_->size ());
+  correspondences.resize(indices_->size());
 
-  std::vector<int> nn_indices (k_);
-  std::vector<float> nn_dists (k_);
+  pcl::Indices nn_indices(k_);
+  std::vector<float> nn_dists(k_);
 
   int min_index = 0;
 
@@ -81,119 +82,116 @@ CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar
   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<PointSource, PointTarget> ())
-  {
+  // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT
+  // macro!
+  if (isSamePointType<PointSource, PointTarget>()) {
     PointTarget pt;
     // Iterate over the input set of source indices
-    for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
-    {
-      tree_->nearestKSearch ((*input_)[*idx_i], k_, nn_indices, nn_dists);
+    for (const auto& idx_i : (*indices_)) {
+      tree_->nearestKSearch((*input_)[idx_i], k_, nn_indices, nn_dists);
 
-      // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
-      double min_dist = std::numeric_limits<double>::max ();
+      // Among the K nearest neighbours find the one with minimum perpendicular distance
+      // to the normal
+      double min_dist = std::numeric_limits<double>::max();
 
       // Find the best correspondence
-      for (std::size_t j = 0; j < nn_indices.size (); j++)
-      {
+      for (std::size_t j = 0; j < nn_indices.size(); j++) {
         // computing the distance between a point and a line in 3d.
         // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
-        pt.x = (*target_)[nn_indices[j]].x - (*input_)[*idx_i].x;
-        pt.y = (*target_)[nn_indices[j]].y - (*input_)[*idx_i].y;
-        pt.z = (*target_)[nn_indices[j]].z - (*input_)[*idx_i].z;
+        pt.x = (*target_)[nn_indices[j]].x - (*input_)[idx_i].x;
+        pt.y = (*target_)[nn_indices[j]].y - (*input_)[idx_i].y;
+        pt.z = (*target_)[nn_indices[j]].z - (*input_)[idx_i].z;
 
-        const NormalT &normal = (*source_normals_)[*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);
+        const NormalT& normal = (*source_normals_)[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)
-        {
+        double dist = C.dot(C);
+        if (dist < min_dist) {
           min_dist = dist;
-          min_index = static_cast<int> (j);
+          min_index = static_cast<int>(j);
         }
       }
       if (min_dist > max_distance)
         continue;
 
-      corr.index_query = *idx_i;
+      corr.index_query = idx_i;
       corr.index_match = nn_indices[min_index];
-      corr.distance = nn_dists[min_index];//min_dist;
+      corr.distance = nn_dists[min_index]; // min_dist;
       correspondences[nr_valid_correspondences++] = corr;
     }
   }
-  else
-  {
+  else {
     PointTarget pt;
 
     // Iterate over the input set of source indices
-    for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
-    {
-      tree_->nearestKSearch ((*input_)[*idx_i], k_, nn_indices, nn_dists);
+    for (const auto& idx_i : (*indices_)) {
+      tree_->nearestKSearch((*input_)[idx_i], k_, nn_indices, nn_dists);
 
-      // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
-      double min_dist = std::numeric_limits<double>::max ();
+      // Among the K nearest neighbours find the one with minimum perpendicular distance
+      // to the normal
+      double min_dist = std::numeric_limits<double>::max();
 
       // Find the best correspondence
-      for (std::size_t j = 0; j < nn_indices.size (); j++)
-      {
+      for (std::size_t j = 0; j < nn_indices.size(); j++) {
         PointSource pt_src;
-        // Copy the source data to a target PointTarget format so we can search in the tree
-        copyPoint ((*input_)[*idx_i], pt_src);
+        // Copy the source data to a target PointTarget format so we can search in the
+        // tree
+        copyPoint((*input_)[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_)[nn_indices[j]].x - pt_src.x;
         pt.y = (*target_)[nn_indices[j]].y - pt_src.y;
         pt.z = (*target_)[nn_indices[j]].z - pt_src.z;
 
-        const NormalT &normal = (*source_normals_)[*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);
+        const NormalT& normal = (*source_normals_)[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)
-        {
+        double dist = C.dot(C);
+        if (dist < min_dist) {
           min_dist = dist;
-          min_index = static_cast<int> (j);
+          min_index = static_cast<int>(j);
         }
       }
       if (min_dist > max_distance)
         continue;
 
-      corr.index_query = *idx_i;
+      corr.index_query = idx_i;
       corr.index_match = nn_indices[min_index];
-      corr.distance = nn_dists[min_index];//min_dist;
+      corr.distance = nn_dists[min_index]; // min_dist;
       correspondences[nr_valid_correspondences++] = corr;
     }
   }
-  correspondences.resize (nr_valid_correspondences);
-  deinitCompute ();
+  correspondences.resize(nr_valid_correspondences);
+  deinitCompute();
 }
 
-
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
-CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar>::determineReciprocalCorrespondences (
-    pcl::Correspondences &correspondences, double max_distance)
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+void
+CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar>::
+    determineReciprocalCorrespondences(pcl::Correspondences& correspondences,
+                                       double max_distance)
 {
-  if (!initCompute ())
+  if (!initCompute())
     return;
 
   // setup tree for reciprocal search
   // Set the internal point representation of choice
-  if (!initComputeReciprocal ())
+  if (!initComputeReciprocal())
     return;
 
-  correspondences.resize (indices_->size ());
+  correspondences.resize(indices_->size());
 
-  std::vector<int> nn_indices (k_);
-  std::vector<float> nn_dists (k_);
-  std::vector<int> index_reciprocal (1);
-  std::vector<float> distance_reciprocal (1);
+  pcl::Indices nn_indices(k_);
+  std::vector<float> nn_dists(k_);
+  pcl::Indices index_reciprocal(1);
+  std::vector<float> distance_reciprocal(1);
 
   int min_index = 0;
 
@@ -202,38 +200,36 @@ CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar
   int target_idx = 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<PointSource, PointTarget> ())
-  {
+  // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT
+  // macro!
+  if (isSamePointType<PointSource, PointTarget>()) {
     PointTarget pt;
     // Iterate over the input set of source indices
-    for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
-    {
-      tree_->nearestKSearch ((*input_)[*idx_i], k_, nn_indices, nn_dists);
+    for (const auto& idx_i : (*indices_)) {
+      tree_->nearestKSearch((*input_)[idx_i], k_, nn_indices, nn_dists);
 
-      // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
-      double min_dist = std::numeric_limits<double>::max ();
+      // Among the K nearest neighbours find the one with minimum perpendicular distance
+      // to the normal
+      double min_dist = std::numeric_limits<double>::max();
 
       // Find the best correspondence
-      for (std::size_t j = 0; j < nn_indices.size (); j++)
-      {
+      for (std::size_t j = 0; j < nn_indices.size(); j++) {
         // computing the distance between a point and a line in 3d.
         // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
-        pt.x = (*target_)[nn_indices[j]].x - (*input_)[*idx_i].x;
-        pt.y = (*target_)[nn_indices[j]].y - (*input_)[*idx_i].y;
-        pt.z = (*target_)[nn_indices[j]].z - (*input_)[*idx_i].z;
+        pt.x = (*target_)[nn_indices[j]].x - (*input_)[idx_i].x;
+        pt.y = (*target_)[nn_indices[j]].y - (*input_)[idx_i].y;
+        pt.z = (*target_)[nn_indices[j]].z - (*input_)[idx_i].z;
 
-        const NormalT &normal = (*source_normals_)[*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);
+        const NormalT& normal = (*source_normals_)[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)
-        {
+        double dist = C.dot(C);
+        if (dist < min_dist) {
           min_dist = dist;
-          min_index = static_cast<int> (j);
+          min_index = static_cast<int>(j);
         }
       }
       if (min_dist > max_distance)
@@ -241,36 +237,36 @@ CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar
 
       // Check if the correspondence is reciprocal
       target_idx = nn_indices[min_index];
-      tree_reciprocal_->nearestKSearch ((*target_)[target_idx], 1, index_reciprocal, distance_reciprocal);
+      tree_reciprocal_->nearestKSearch(
+          (*target_)[target_idx], 1, index_reciprocal, distance_reciprocal);
 
-      if (*idx_i != index_reciprocal[0])
+      if (idx_i != index_reciprocal[0])
         continue;
 
       // Correspondence IS reciprocal, save it and continue
-      corr.index_query = *idx_i;
+      corr.index_query = idx_i;
       corr.index_match = nn_indices[min_index];
-      corr.distance = nn_dists[min_index];//min_dist;
+      corr.distance = nn_dists[min_index]; // min_dist;
       correspondences[nr_valid_correspondences++] = corr;
     }
   }
-  else
-  {
+  else {
     PointTarget pt;
 
     // Iterate over the input set of source indices
-    for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
-    {
-      tree_->nearestKSearch ((*input_)[*idx_i], k_, nn_indices, nn_dists);
+    for (const auto& idx_i : (*indices_)) {
+      tree_->nearestKSearch((*input_)[idx_i], k_, nn_indices, nn_dists);
 
-      // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
-      double min_dist = std::numeric_limits<double>::max ();
+      // Among the K nearest neighbours find the one with minimum perpendicular distance
+      // to the normal
+      double min_dist = std::numeric_limits<double>::max();
 
       // Find the best correspondence
-      for (std::size_t j = 0; j < nn_indices.size (); j++)
-      {
+      for (std::size_t j = 0; j < nn_indices.size(); j++) {
         PointSource pt_src;
-        // Copy the source data to a target PointTarget format so we can search in the tree
-        copyPoint ((*input_)[*idx_i], pt_src);
+        // Copy the source data to a target PointTarget format so we can search in the
+        // tree
+        copyPoint((*input_)[idx_i], pt_src);
 
         // computing the distance between a point and a line in 3d.
         // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
@@ -278,17 +274,16 @@ CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar
         pt.y = (*target_)[nn_indices[j]].y - pt_src.y;
         pt.z = (*target_)[nn_indices[j]].z - pt_src.z;
 
-        const NormalT &normal = (*source_normals_)[*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);
+        const NormalT& normal = (*source_normals_)[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)
-        {
+        double dist = C.dot(C);
+        if (dist < min_dist) {
           min_dist = dist;
-          min_index = static_cast<int> (j);
+          min_index = static_cast<int>(j);
         }
       }
       if (min_dist > max_distance)
@@ -296,23 +291,24 @@ CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar
 
       // Check if the correspondence is reciprocal
       target_idx = nn_indices[min_index];
-      tree_reciprocal_->nearestKSearch ((*target_)[target_idx], 1, index_reciprocal, distance_reciprocal);
+      tree_reciprocal_->nearestKSearch(
+          (*target_)[target_idx], 1, index_reciprocal, distance_reciprocal);
 
-      if (*idx_i != index_reciprocal[0])
+      if (idx_i != index_reciprocal[0])
         continue;
 
       // Correspondence IS reciprocal, save it and continue
-      corr.index_query = *idx_i;
+      corr.index_query = idx_i;
       corr.index_match = nn_indices[min_index];
-      corr.distance = nn_dists[min_index];//min_dist;
+      corr.distance = nn_dists[min_index]; // min_dist;
       correspondences[nr_valid_correspondences++] = corr;
     }
   }
-  correspondences.resize (nr_valid_correspondences);
-  deinitCompute ();
+  correspondences.resize(nr_valid_correspondences);
+  deinitCompute();
 }
 
 } // namespace registration
 } // namespace pcl
 
-#endif    // PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_
+#endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_
diff --git a/registration/include/pcl/registration/impl/correspondence_estimation_organized_projection.hpp b/registration/include/pcl/registration/impl/correspondence_estimation_organized_projection.hpp
index 66ecbd95..6837e649 100644
--- a/registration/include/pcl/registration/impl/correspondence_estimation_organized_projection.hpp
+++ b/registration/include/pcl/registration/impl/correspondence_estimation_organized_projection.hpp
@@ -38,100 +38,97 @@
  *
  */
 
-
 #ifndef PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_
 #define PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_
 
+namespace pcl {
 
-namespace pcl
-{
-
-namespace registration
-{
+namespace registration {
 
-template <typename PointSource, typename PointTarget, typename Scalar> bool
-CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar>::initCompute ()
+template <typename PointSource, typename PointTarget, typename Scalar>
+bool
+CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar>::
+    initCompute()
 {
-  // Set the target_cloud_updated_ variable to true, so that the kd-tree is not built - it is not needed for this class
+  // 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;
-  if (!CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute ())
+  if (!CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute())
     return (false);
 
   /// Check if the target cloud is organized
-  if (!target_->isOrganized ())
-  {
-    PCL_WARN ("[pcl::registration::%s::initCompute] Target cloud is not organized.\n", getClassName ().c_str ());
+  if (!target_->isOrganized()) {
+    PCL_WARN("[pcl::registration::%s::initCompute] Target cloud is not organized.\n",
+             getClassName().c_str());
     return (false);
   }
 
   /// Put the projection matrix together
-  projection_matrix_ (0, 0) = fx_;
-  projection_matrix_ (1, 1) = fy_;
-  projection_matrix_ (0, 2) = cx_;
-  projection_matrix_ (1, 2) = cy_;
+  projection_matrix_(0, 0) = fx_;
+  projection_matrix_(1, 1) = fy_;
+  projection_matrix_(0, 2) = cx_;
+  projection_matrix_(1, 2) = cy_;
 
   return (true);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> void
-CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar>::determineCorrespondences (
-    pcl::Correspondences &correspondences,
-    double max_distance)
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
+CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar>::
+    determineCorrespondences(pcl::Correspondences& correspondences, double max_distance)
 {
-  if (!initCompute ())
+  if (!initCompute())
     return;
 
-  correspondences.resize (indices_->size ());
+  correspondences.resize(indices_->size());
   std::size_t c_index = 0;
 
-  for (std::vector<int>::const_iterator src_it = indices_->begin (); src_it != indices_->end (); ++src_it)
-  {
-    if (isFinite ((*input_)[*src_it]))
-    {
-      Eigen::Vector4f p_src (src_to_tgt_transformation_ * (*input_)[*src_it].getVector4fMap ());
-      Eigen::Vector3f p_src3 (p_src[0], p_src[1], p_src[2]);
-      Eigen::Vector3f uv (projection_matrix_ * p_src3);
+  for (const auto& src_idx : (*indices_)) {
+    if (isFinite((*input_)[src_idx])) {
+      Eigen::Vector4f p_src(src_to_tgt_transformation_ *
+                            (*input_)[src_idx].getVector4fMap());
+      Eigen::Vector3f p_src3(p_src[0], p_src[1], p_src[2]);
+      Eigen::Vector3f uv(projection_matrix_ * p_src3);
 
       /// Check if the point was behind the camera
       if (uv[2] <= 0)
         continue;
 
-      int u = static_cast<int> (uv[0] / uv[2]);
-      int v = static_cast<int> (uv[1] / uv[2]);
+      int u = static_cast<int>(uv[0] / uv[2]);
+      int v = static_cast<int>(uv[1] / uv[2]);
 
-      if (u >= 0 && u < static_cast<int> (target_->width) &&
-          v >= 0 && v < static_cast<int> (target_->height))
-      {
-        const PointTarget &pt_tgt = target_->at (u, v);
-        if (!isFinite (pt_tgt))
+      if (u >= 0 && u < static_cast<int>(target_->width) && v >= 0 &&
+          v < static_cast<int>(target_->height)) {
+        const PointTarget& pt_tgt = target_->at(u, v);
+        if (!isFinite(pt_tgt))
           continue;
         /// Check if the depth difference is larger than the threshold
-        if (std::abs (uv[2] - pt_tgt.z) > depth_threshold_)
+        if (std::abs(uv[2] - pt_tgt.z) > depth_threshold_)
           continue;
 
-        double dist = (p_src3 - pt_tgt.getVector3fMap ()).norm ();
+        double dist = (p_src3 - pt_tgt.getVector3fMap()).norm();
         if (dist < max_distance)
-          correspondences[c_index++] =  pcl::Correspondence (*src_it, v * target_->width + u, static_cast<float> (dist));
+          correspondences[c_index++] = pcl::Correspondence(
+              src_idx, v * target_->width + u, static_cast<float>(dist));
       }
     }
   }
 
-  correspondences.resize (c_index);
+  correspondences.resize(c_index);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> void
-CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar>::determineReciprocalCorrespondences (
-    pcl::Correspondences &correspondences,
-    double max_distance)
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
+CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar>::
+    determineReciprocalCorrespondences(pcl::Correspondences& correspondences,
+                                       double max_distance)
 {
-  // Call the normal determineCorrespondences (...), as doing it both ways will not improve the results
-  determineCorrespondences (correspondences, max_distance);
+  // Call the normal determineCorrespondences (...), as doing it both ways will not
+  // improve the results
+  determineCorrespondences(correspondences, max_distance);
 }
 
 } // namespace registration
 } // namespace pcl
 
-#endif    // PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_
-
+#endif // PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_
diff --git a/registration/include/pcl/registration/impl/correspondence_rejection_distance.hpp b/registration/include/pcl/registration/impl/correspondence_rejection_distance.hpp
index e7a174fe..680ed690 100644
--- a/registration/include/pcl/registration/impl/correspondence_rejection_distance.hpp
+++ b/registration/include/pcl/registration/impl/correspondence_rejection_distance.hpp
@@ -39,6 +39,5 @@
  */
 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_DISTANCE_HPP_
 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_DISTANCE_HPP_
-
-
+PCL_DEPRECATED_HEADER(1, 15, "")
 #endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_DISTANCE_HPP_ */
diff --git a/registration/include/pcl/registration/impl/correspondence_rejection_features.hpp b/registration/include/pcl/registration/impl/correspondence_rejection_features.hpp
index 85964538..b9c56704 100644
--- a/registration/include/pcl/registration/impl/correspondence_rejection_features.hpp
+++ b/registration/include/pcl/registration/impl/correspondence_rejection_features.hpp
@@ -38,78 +38,83 @@
  *
  */
 
-
 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_FEATURES_HPP_
 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_FEATURES_HPP_
 
 #include <boost/pointer_cast.hpp> // for static_pointer_cast
 
-namespace pcl
-{
+namespace pcl {
 
-namespace registration
-{
+namespace registration {
 
-template <typename FeatureT> inline void
-CorrespondenceRejectorFeatures::setSourceFeature (
-    const typename pcl::PointCloud<FeatureT>::ConstPtr &source_feature, const std::string &key)
+template <typename FeatureT>
+inline void
+CorrespondenceRejectorFeatures::setSourceFeature(
+    const typename pcl::PointCloud<FeatureT>::ConstPtr& source_feature,
+    const std::string& key)
 {
-  if (features_map_.count (key) == 0)
-    features_map_[key].reset (new FeatureContainer<FeatureT>);
-  boost::static_pointer_cast<FeatureContainer<FeatureT> > (features_map_[key])->setSourceFeature (source_feature);
+  if (features_map_.count(key) == 0)
+    features_map_[key].reset(new FeatureContainer<FeatureT>);
+  boost::static_pointer_cast<FeatureContainer<FeatureT>>(features_map_[key])
+      ->setSourceFeature(source_feature);
 }
 
-
-template <typename FeatureT> inline typename pcl::PointCloud<FeatureT>::ConstPtr
-CorrespondenceRejectorFeatures::getSourceFeature (const std::string &key)
+template <typename FeatureT>
+inline typename pcl::PointCloud<FeatureT>::ConstPtr
+CorrespondenceRejectorFeatures::getSourceFeature(const std::string& key)
 {
-  if (features_map_.count (key) == 0)
+  if (features_map_.count(key) == 0)
     return (nullptr);
-  return (boost::static_pointer_cast<FeatureContainer<FeatureT> > (features_map_[key])->getSourceFeature ());
+  return (boost::static_pointer_cast<FeatureContainer<FeatureT>>(features_map_[key])
+              ->getSourceFeature());
 }
 
-
-template <typename FeatureT> inline void
-CorrespondenceRejectorFeatures::setTargetFeature (
-    const typename pcl::PointCloud<FeatureT>::ConstPtr &target_feature, const std::string &key)
+template <typename FeatureT>
+inline void
+CorrespondenceRejectorFeatures::setTargetFeature(
+    const typename pcl::PointCloud<FeatureT>::ConstPtr& target_feature,
+    const std::string& key)
 {
-  if (features_map_.count (key) == 0)
-    features_map_[key].reset (new FeatureContainer<FeatureT>);
-  boost::static_pointer_cast<FeatureContainer<FeatureT> > (features_map_[key])->setTargetFeature (target_feature);
+  if (features_map_.count(key) == 0)
+    features_map_[key].reset(new FeatureContainer<FeatureT>);
+  boost::static_pointer_cast<FeatureContainer<FeatureT>>(features_map_[key])
+      ->setTargetFeature(target_feature);
 }
 
-
-template <typename FeatureT> inline typename pcl::PointCloud<FeatureT>::ConstPtr
-CorrespondenceRejectorFeatures::getTargetFeature (const std::string &key)
+template <typename FeatureT>
+inline typename pcl::PointCloud<FeatureT>::ConstPtr
+CorrespondenceRejectorFeatures::getTargetFeature(const std::string& key)
 {
-  if (features_map_.count (key) == 0)
+  if (features_map_.count(key) == 0)
     return (nullptr);
-  return (boost::static_pointer_cast<FeatureContainer<FeatureT> > (features_map_[key])->getTargetFeature ());
+  return (boost::static_pointer_cast<FeatureContainer<FeatureT>>(features_map_[key])
+              ->getTargetFeature());
 }
 
-
-template <typename FeatureT> inline void
-CorrespondenceRejectorFeatures::setDistanceThreshold (
-    double thresh, const std::string &key)
+template <typename FeatureT>
+inline void
+CorrespondenceRejectorFeatures::setDistanceThreshold(double thresh,
+                                                     const std::string& key)
 {
-  if (features_map_.count (key) == 0)
-    features_map_[key].reset (new FeatureContainer<FeatureT>);
-  boost::static_pointer_cast<FeatureContainer<FeatureT> > (features_map_[key])->setDistanceThreshold (thresh);
+  if (features_map_.count(key) == 0)
+    features_map_[key].reset(new FeatureContainer<FeatureT>);
+  boost::static_pointer_cast<FeatureContainer<FeatureT>>(features_map_[key])
+      ->setDistanceThreshold(thresh);
 }
 
-
-template <typename FeatureT> inline void
-CorrespondenceRejectorFeatures::setFeatureRepresentation (
-  const typename pcl::PointRepresentation<FeatureT>::ConstPtr &fr,
-  const std::string &key)
+template <typename FeatureT>
+inline void
+CorrespondenceRejectorFeatures::setFeatureRepresentation(
+    const typename pcl::PointRepresentation<FeatureT>::ConstPtr& fr,
+    const std::string& key)
 {
-  if (features_map_.count (key) == 0)
-    features_map_[key].reset (new FeatureContainer<FeatureT>);
-  boost::static_pointer_cast<FeatureContainer<FeatureT> > (features_map_[key])->setFeatureRepresentation (fr);
+  if (features_map_.count(key) == 0)
+    features_map_[key].reset(new FeatureContainer<FeatureT>);
+  boost::static_pointer_cast<FeatureContainer<FeatureT>>(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_median_distance.hpp b/registration/include/pcl/registration/impl/correspondence_rejection_median_distance.hpp
index 2884d253..eaa5364e 100644
--- a/registration/include/pcl/registration/impl/correspondence_rejection_median_distance.hpp
+++ b/registration/include/pcl/registration/impl/correspondence_rejection_median_distance.hpp
@@ -39,6 +39,5 @@
  */
 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_MEDIAN_DISTANCE_HPP_
 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_MEDIAN_DISTANCE_HPP_
-
-
-#endif    // PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_MEDIAN_DISTANCE_HPP_
+PCL_DEPRECATED_HEADER(1, 15, "")
+#endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_MEDIAN_DISTANCE_HPP_
diff --git a/registration/include/pcl/registration/impl/correspondence_rejection_one_to_one.hpp b/registration/include/pcl/registration/impl/correspondence_rejection_one_to_one.hpp
index 656b6db8..3f256839 100644
--- a/registration/include/pcl/registration/impl/correspondence_rejection_one_to_one.hpp
+++ b/registration/include/pcl/registration/impl/correspondence_rejection_one_to_one.hpp
@@ -39,5 +39,5 @@
  */
 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_ONE_TO_ONE_HPP_
 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_ONE_TO_ONE_HPP_
-
+PCL_DEPRECATED_HEADER(1, 15, "")
 #endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_ONE_TO_ONE_HPP_ */
diff --git a/registration/include/pcl/registration/impl/correspondence_rejection_organized_boundary.hpp b/registration/include/pcl/registration/impl/correspondence_rejection_organized_boundary.hpp
index a6821e48..c70ea72b 100644
--- a/registration/include/pcl/registration/impl/correspondence_rejection_organized_boundary.hpp
+++ b/registration/include/pcl/registration/impl/correspondence_rejection_organized_boundary.hpp
@@ -36,8 +36,7 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-
 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_ORGANIZED_BOUNDARY_HPP_
 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_ORGANIZED_BOUNDARY_HPP_
-
+PCL_DEPRECATED_HEADER(1, 15, "")
 #endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_ORGANIZED_BOUNDARY_HPP_ */
diff --git a/registration/include/pcl/registration/impl/correspondence_rejection_poly.hpp b/registration/include/pcl/registration/impl/correspondence_rejection_poly.hpp
index 906d8f05..21883483 100644
--- a/registration/include/pcl/registration/impl/correspondence_rejection_poly.hpp
+++ b/registration/include/pcl/registration/impl/correspondence_rejection_poly.hpp
@@ -36,19 +36,16 @@
  *
  */
 
-
 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_
 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_
 
+namespace pcl {
 
-namespace pcl
-{
+namespace registration {
 
-namespace registration
-{
-
-template <typename SourceT, typename TargetT> void
-CorrespondenceRejectorPoly<SourceT, TargetT>::getRemainingCorrespondences (
+template <typename SourceT, typename TargetT>
+void
+CorrespondenceRejectorPoly<SourceT, TargetT>::getRemainingCorrespondences(
     const pcl::Correspondences& original_correspondences,
     pcl::Correspondences& remaining_correspondences)
 {
@@ -56,44 +53,46 @@ CorrespondenceRejectorPoly<SourceT, TargetT>::getRemainingCorrespondences (
   remaining_correspondences = original_correspondences;
 
   // Check source/target
-  if (!input_)
-  {
-    PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No source was input! Returning all input correspondences.\n",
-               getClassName ().c_str ());
+  if (!input_) {
+    PCL_ERROR("[pcl::registration::%s::getRemainingCorrespondences] No source was "
+              "input! Returning all input correspondences.\n",
+              getClassName().c_str());
     return;
   }
 
-  if (!target_)
-  {
-    PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No target was input! Returning all input correspondences.\n",
-               getClassName ().c_str ());
+  if (!target_) {
+    PCL_ERROR("[pcl::registration::%s::getRemainingCorrespondences] No target was "
+              "input! Returning all input correspondences.\n",
+              getClassName().c_str());
     return;
   }
 
   // Check cardinality
-  if (cardinality_ < 2)
-  {
-    PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Polygon cardinality too low!. Returning all input correspondences.\n",
-               getClassName ().c_str() );
+  if (cardinality_ < 2) {
+    PCL_ERROR("[pcl::registration::%s::getRemainingCorrespondences] Polygon "
+              "cardinality too low!. Returning all input correspondences.\n",
+              getClassName().c_str());
     return;
   }
 
   // Number of input correspondences
-  const int nr_correspondences = static_cast<int> (original_correspondences.size ());
+  const int nr_correspondences = static_cast<int>(original_correspondences.size());
 
   // Not enough correspondences for polygonal rejections
-  if (cardinality_ >= nr_correspondences)
-  {
-    PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Number of correspondences smaller than polygon cardinality! Returning all input correspondences.\n",
-               getClassName ().c_str() );
+  if (cardinality_ >= nr_correspondences) {
+    PCL_ERROR("[pcl::registration::%s::getRemainingCorrespondences] Number of "
+              "correspondences smaller than polygon cardinality! Returning all input "
+              "correspondences.\n",
+              getClassName().c_str());
     return;
   }
 
   // Check similarity
-  if (similarity_threshold_ < 0.0f || similarity_threshold_ > 1.0f)
-  {
-    PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Invalid edge length similarity - must be in [0,1]!. Returning all input correspondences.\n",
-               getClassName ().c_str() );
+  if (similarity_threshold_ < 0.0f || similarity_threshold_ > 1.0f) {
+    PCL_ERROR(
+        "[pcl::registration::%s::getRemainingCorrespondences] Invalid edge length "
+        "similarity - must be in [0,1]!. Returning all input correspondences.\n",
+        getClassName().c_str());
     return;
   }
 
@@ -101,100 +100,97 @@ CorrespondenceRejectorPoly<SourceT, TargetT>::getRemainingCorrespondences (
   similarity_threshold_squared_ = similarity_threshold_ * similarity_threshold_;
 
   // Initialization of result
-  remaining_correspondences.clear ();
-  remaining_correspondences.reserve (nr_correspondences);
+  remaining_correspondences.clear();
+  remaining_correspondences.reserve(nr_correspondences);
 
   // Number of times a correspondence is sampled and number of times it was accepted
-  std::vector<int> num_samples (nr_correspondences, 0);
-  std::vector<int> num_accepted (nr_correspondences, 0);
+  std::vector<int> num_samples(nr_correspondences, 0);
+  std::vector<int> num_accepted(nr_correspondences, 0);
 
   // Main loop
-  for (int i = 0; i < iterations_; ++i)
-  {
+  for (int i = 0; i < iterations_; ++i) {
     // Sample cardinality_ correspondences without replacement
-    const std::vector<int> idx = getUniqueRandomIndices (nr_correspondences, cardinality_);
+    const std::vector<int> idx =
+        getUniqueRandomIndices(nr_correspondences, cardinality_);
 
     // Verify the polygon similarity
-    if (thresholdPolygon (original_correspondences, idx))
-    {
+    if (thresholdPolygon(original_correspondences, idx)) {
       // Increment sample counter and accept counter
-      for (int j = 0; j < cardinality_; ++j)
-      {
-        ++num_samples[ idx[j] ];
-        ++num_accepted[ idx[j] ];
+      for (int j = 0; j < cardinality_; ++j) {
+        ++num_samples[idx[j]];
+        ++num_accepted[idx[j]];
       }
     }
-    else
-    {
+    else {
       // Not accepted, only increment sample counter
       for (int j = 0; j < cardinality_; ++j)
-        ++num_samples[ idx[j] ];
+        ++num_samples[idx[j]];
     }
   }
 
   // Now calculate the acceptance rate of each correspondence
-  std::vector<float> accept_rate (nr_correspondences, 0.0f);
-  for (int i = 0; i < nr_correspondences; ++i)
-  {
+  std::vector<float> accept_rate(nr_correspondences, 0.0f);
+  for (int i = 0; i < nr_correspondences; ++i) {
     const int numsi = num_samples[i];
     if (numsi == 0)
       accept_rate[i] = 0.0f;
     else
-      accept_rate[i] = static_cast<float> (num_accepted[i]) / static_cast<float> (numsi);
+      accept_rate[i] = static_cast<float>(num_accepted[i]) / static_cast<float>(numsi);
   }
 
   // Compute a histogram in range [0,1] for acceptance rates
   const int hist_size = nr_correspondences / 2; // TODO: Optimize this
-  const std::vector<int> histogram = computeHistogram (accept_rate, 0.0f, 1.0f, hist_size);
+  const std::vector<int> 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<float> (cut_idx) / static_cast<float> (hist_size);
+  const int cut_idx = findThresholdOtsu(histogram);
+  const float cut = static_cast<float>(cut_idx) / static_cast<float>(hist_size);
 
   // Threshold
   for (int i = 0; i < nr_correspondences; ++i)
     if (accept_rate[i] > cut)
-      remaining_correspondences.push_back (original_correspondences[i]);
+      remaining_correspondences.push_back(original_correspondences[i]);
 }
 
-
-template <typename SourceT, typename TargetT> std::vector<int>
-CorrespondenceRejectorPoly<SourceT, TargetT>::computeHistogram (const std::vector<float>& data,
-                                                                float lower, float upper, int bins)
+template <typename SourceT, typename TargetT>
+std::vector<int>
+CorrespondenceRejectorPoly<SourceT, TargetT>::computeHistogram(
+    const std::vector<float>& data, float lower, float upper, int bins)
 {
   // Result
-  std::vector<int> result (bins, 0);
+  std::vector<int> 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<float> (bins) / (upper - lower);
+  const float idx_per_val = static_cast<float>(bins) / (upper - lower);
 
   // Accumulate
-  for (const float &value : data)
-     ++result[ std::min (last_idx, int (value*idx_per_val)) ];
+  for (const float& value : data)
+    ++result[std::min(last_idx, int(value * idx_per_val))];
 
   return (result);
 }
 
-
-template <typename SourceT, typename TargetT> int
-CorrespondenceRejectorPoly<SourceT, TargetT>::findThresholdOtsu (const std::vector<int>& histogram)
+template <typename SourceT, typename TargetT>
+int
+CorrespondenceRejectorPoly<SourceT, TargetT>::findThresholdOtsu(
+    const std::vector<int>& histogram)
 {
   // Precision
   const double eps = std::numeric_limits<double>::epsilon();
 
   // Histogram dimension
-  const int nbins = static_cast<int> (histogram.size ());
+  const int nbins = static_cast<int>(histogram.size());
 
   // Mean and inverse of the number of data points
   double mean = 0.0;
   double sum_inv = 0.0;
-  for (int i = 0; i < nbins; ++i)
-  {
-    mean += static_cast<double> (i * histogram[i]);
-    sum_inv += static_cast<double> (histogram[i]);
+  for (int i = 0; i < nbins; ++i) {
+    mean += static_cast<double>(i * histogram[i]);
+    sum_inv += static_cast<double>(histogram[i]);
   }
-  sum_inv = 1.0/sum_inv;
+  sum_inv = 1.0 / sum_inv;
   mean *= sum_inv;
 
   // Probability and mean of class 1 (data to the left of threshold)
@@ -207,12 +203,11 @@ CorrespondenceRejectorPoly<SourceT, TargetT>::findThresholdOtsu (const std::vect
   int result = 0;
 
   // Loop over all bin values
-  for (int i = 0; i < nbins; ++i)
-  {
+  for (int i = 0; i < nbins; ++i) {
     class_mean1 *= class_prob1;
 
     // Probability of bin i
-    const double prob_i = static_cast<double> (histogram[i]) * sum_inv;
+    const double prob_i = static_cast<double>(histogram[i]) * sum_inv;
 
     // Class probability 1: sum of probabilities from 0 to i
     class_prob1 += prob_i;
@@ -221,23 +216,23 @@ CorrespondenceRejectorPoly<SourceT, TargetT>::findThresholdOtsu (const std::vect
     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)
+    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<double> (i) * prob_i) / class_prob1;
+    class_mean1 = (class_mean1 + static_cast<double>(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;
+    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);
+    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)
-    {
+    if (between_class_variance > between_class_variance_max) {
       between_class_variance_max = between_class_variance;
       result = i;
     }
@@ -249,5 +244,4 @@ CorrespondenceRejectorPoly<SourceT, TargetT>::findThresholdOtsu (const std::vect
 } // namespace registration
 } // namespace pcl
 
-#endif    // PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_
-
+#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 d460bd29..e2c68aa5 100644
--- a/registration/include/pcl/registration/impl/correspondence_rejection_sample_consensus.hpp
+++ b/registration/include/pcl/registration/impl/correspondence_rejection_sample_consensus.hpp
@@ -41,109 +41,109 @@
 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_HPP_
 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_HPP_
 
-#include <unordered_map>
 #include <pcl/sample_consensus/ransac.h>
 #include <pcl/sample_consensus/sac_model_registration.h>
 
+#include <unordered_map>
 
-namespace pcl
-{
+namespace pcl {
 
-namespace registration
-{
+namespace registration {
 
-template <typename PointT> void
-CorrespondenceRejectorSampleConsensus<PointT>::getRemainingCorrespondences (
+template <typename PointT>
+void
+CorrespondenceRejectorSampleConsensus<PointT>::getRemainingCorrespondences(
     const pcl::Correspondences& original_correspondences,
     pcl::Correspondences& remaining_correspondences)
 {
-  if (!input_)
-  {
-    PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input cloud dataset was given!\n", getClassName ().c_str ());
+  if (!input_) {
+    PCL_ERROR("[pcl::registration::%s::getRemainingCorrespondences] No input cloud "
+              "dataset was given!\n",
+              getClassName().c_str());
     return;
   }
 
-  if (!target_)
-  {
-    PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input target dataset was given!\n", getClassName ().c_str ());
+  if (!target_) {
+    PCL_ERROR("[pcl::registration::%s::getRemainingCorrespondences] No input target "
+              "dataset was given!\n",
+              getClassName().c_str());
     return;
   }
 
   if (save_inliers_)
-     inlier_indices_.clear ();
+    inlier_indices_.clear();
 
-  int nr_correspondences = static_cast<int> (original_correspondences.size ());
-  std::vector<int> source_indices (nr_correspondences);
-  std::vector<int> target_indices (nr_correspondences);
+  int nr_correspondences = static_cast<int>(original_correspondences.size());
+  pcl::Indices source_indices(nr_correspondences);
+  pcl::Indices target_indices(nr_correspondences);
 
   // Copy the query-match indices
-  for (std::size_t i = 0; i < original_correspondences.size (); ++i)
-  {
+  for (std::size_t i = 0; i < original_correspondences.size(); ++i) {
     source_indices[i] = original_correspondences[i].index_query;
     target_indices[i] = original_correspondences[i].index_match;
   }
 
-   {
-     // From the set of correspondences found, attempt to remove outliers
-     // Create the registration model
-     using SampleConsensusModelRegistrationPtr = typename pcl::SampleConsensusModelRegistration<PointT>::Ptr;
-     SampleConsensusModelRegistrationPtr model;
-     model.reset (new pcl::SampleConsensusModelRegistration<PointT> (input_, source_indices));
-     // Pass the target_indices
-     model->setInputTarget (target_, target_indices);
-     // Create a RANSAC model
-     pcl::RandomSampleConsensus<PointT> sac (model, inlier_threshold_);
-     sac.setMaxIterations (max_iterations_);
-
-     // Compute the set of inliers
-     if (!sac.computeModel ())
-     {
-       remaining_correspondences = original_correspondences;
-       best_transformation_.setIdentity ();
-       return;
-     }
-     if (refine_ && !sac.refineModel ())
-     {
-       PCL_ERROR ("[pcl::registration::CorrespondenceRejectorSampleConsensus::getRemainingCorrespondences] Could not refine the model! Returning an empty solution.\n");
-       return;
-     }
-
-     std::vector<int> inliers;
-     sac.getInliers (inliers);
-
-     if (inliers.size () < 3)
-     {
-       remaining_correspondences = original_correspondences;
-       best_transformation_.setIdentity ();
-       return;
-     }
-     std::unordered_map<int, int> index_to_correspondence;
-     for (int i = 0; i < nr_correspondences; ++i)
-       index_to_correspondence[original_correspondences[i].index_query] = i;
-
-     remaining_correspondences.resize (inliers.size ());
-     for (std::size_t i = 0; i < inliers.size (); ++i)
-       remaining_correspondences[i] = original_correspondences[index_to_correspondence[inliers[i]]];
-
-     if (save_inliers_)
-     {
-       inlier_indices_.reserve (inliers.size ());
-       for (const int &inlier : inliers)
-         inlier_indices_.push_back (index_to_correspondence[inlier]);
-     }
-
-     // get best transformation
-     Eigen::VectorXf model_coefficients;
-     sac.getModelCoefficients (model_coefficients);
-     best_transformation_.row (0) = model_coefficients.segment<4>(0);
-     best_transformation_.row (1) = model_coefficients.segment<4>(4);
-     best_transformation_.row (2) = model_coefficients.segment<4>(8);
-     best_transformation_.row (3) = model_coefficients.segment<4>(12);
-   }
+  {
+    // From the set of correspondences found, attempt to remove outliers
+    // Create the registration model
+    using SampleConsensusModelRegistrationPtr =
+        typename pcl::SampleConsensusModelRegistration<PointT>::Ptr;
+    SampleConsensusModelRegistrationPtr model;
+    model.reset(
+        new pcl::SampleConsensusModelRegistration<PointT>(input_, source_indices));
+    // Pass the target_indices
+    model->setInputTarget(target_, target_indices);
+    // Create a RANSAC model
+    pcl::RandomSampleConsensus<PointT> sac(model, inlier_threshold_);
+    sac.setMaxIterations(max_iterations_);
+
+    // Compute the set of inliers
+    if (!sac.computeModel()) {
+      remaining_correspondences = original_correspondences;
+      best_transformation_.setIdentity();
+      return;
+    }
+    if (refine_ && !sac.refineModel()) {
+      PCL_ERROR("[pcl::registration::CorrespondenceRejectorSampleConsensus::"
+                "getRemainingCorrespondences] Could not refine the model! Returning an "
+                "empty solution.\n");
+      return;
+    }
+
+    pcl::Indices inliers;
+    sac.getInliers(inliers);
+
+    if (inliers.size() < 3) {
+      remaining_correspondences = original_correspondences;
+      best_transformation_.setIdentity();
+      return;
+    }
+    std::unordered_map<int, int> index_to_correspondence;
+    for (int i = 0; i < nr_correspondences; ++i)
+      index_to_correspondence[original_correspondences[i].index_query] = i;
+
+    remaining_correspondences.resize(inliers.size());
+    for (std::size_t i = 0; i < inliers.size(); ++i)
+      remaining_correspondences[i] =
+          original_correspondences[index_to_correspondence[inliers[i]]];
+
+    if (save_inliers_) {
+      inlier_indices_.reserve(inliers.size());
+      for (const auto& inlier : inliers)
+        inlier_indices_.push_back(index_to_correspondence[inlier]);
+    }
+
+    // get best transformation
+    Eigen::VectorXf model_coefficients;
+    sac.getModelCoefficients(model_coefficients);
+    best_transformation_.row(0) = model_coefficients.segment<4>(0);
+    best_transformation_.row(1) = model_coefficients.segment<4>(4);
+    best_transformation_.row(2) = model_coefficients.segment<4>(8);
+    best_transformation_.row(3) = model_coefficients.segment<4>(12);
+  }
 }
 
 } // namespace registration
 } // namespace pcl
 
-#endif    // PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_HPP_
-
+#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 173c524a..3579fe7d 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
@@ -39,81 +39,86 @@
 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_2D_HPP_
 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_2D_HPP_
 
-#include <pcl/sample_consensus/sac_model_registration_2d.h>
 #include <pcl/sample_consensus/ransac.h>
+#include <pcl/sample_consensus/sac_model_registration_2d.h>
 
 #include <unordered_map>
 
+namespace pcl {
 
-namespace pcl
-{
-
-namespace registration
-{
+namespace registration {
 
-template <typename PointT> void
-CorrespondenceRejectorSampleConsensus2D<PointT>::getRemainingCorrespondences (
+template <typename PointT>
+void
+CorrespondenceRejectorSampleConsensus2D<PointT>::getRemainingCorrespondences(
     const pcl::Correspondences& original_correspondences,
     pcl::Correspondences& remaining_correspondences)
 {
-  if (!input_)
-  {
-    PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input cloud dataset was given!\n", getClassName ().c_str ());
+  if (!input_) {
+    PCL_ERROR("[pcl::registration::%s::getRemainingCorrespondences] No input cloud "
+              "dataset was given!\n",
+              getClassName().c_str());
     return;
   }
 
-  if (!target_)
-  {
-    PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input target dataset was given!\n", getClassName ().c_str ());
+  if (!target_) {
+    PCL_ERROR("[pcl::registration::%s::getRemainingCorrespondences] No input target "
+              "dataset was given!\n",
+              getClassName().c_str());
     return;
   }
 
-  if (projection_matrix_ == Eigen::Matrix3f::Identity ())
-  {
-    PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Intrinsic camera parameters not given!\n", getClassName ().c_str ());
+  if (projection_matrix_ == Eigen::Matrix3f::Identity()) {
+    PCL_ERROR("[pcl::registration::%s::getRemainingCorrespondences] Intrinsic camera "
+              "parameters not given!\n",
+              getClassName().c_str());
     return;
   }
 
-  int nr_correspondences = static_cast<int> (original_correspondences.size ());
-  std::vector<int> source_indices (nr_correspondences);
-  std::vector<int> target_indices (nr_correspondences);
+  int nr_correspondences = static_cast<int>(original_correspondences.size());
+  pcl::Indices source_indices(nr_correspondences);
+  pcl::Indices target_indices(nr_correspondences);
 
   // Copy the query-match indices
-  for (std::size_t i = 0; i < original_correspondences.size (); ++i)
-  {
+  for (std::size_t i = 0; i < original_correspondences.size(); ++i) {
     source_indices[i] = original_correspondences[i].index_query;
     target_indices[i] = original_correspondences[i].index_match;
   }
 
   // From the set of correspondences found, attempt to remove outliers
-  typename pcl::SampleConsensusModelRegistration2D<PointT>::Ptr model (new pcl::SampleConsensusModelRegistration2D<PointT> (input_, source_indices));
+  typename pcl::SampleConsensusModelRegistration2D<PointT>::Ptr model(
+      new pcl::SampleConsensusModelRegistration2D<PointT>(input_, source_indices));
   // Pass the target_indices
-  model->setInputTarget (target_, target_indices);
-  model->setProjectionMatrix (projection_matrix_);
+  model->setInputTarget(target_, target_indices);
+  model->setProjectionMatrix(projection_matrix_);
 
   // Create a RANSAC model
-  pcl::RandomSampleConsensus<PointT> sac (model, inlier_threshold_);
-  sac.setMaxIterations (max_iterations_);
+  pcl::RandomSampleConsensus<PointT> sac(model, inlier_threshold_);
+  sac.setMaxIterations(max_iterations_);
 
   // Compute the set of inliers
-  if (!sac.computeModel ())
-  {
-    PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Error computing model! Returning the original correspondences...\n", getClassName ().c_str ());
+  if (!sac.computeModel()) {
+    PCL_ERROR("[pcl::registration::%s::getRemainingCorrespondences] Error computing "
+              "model! Returning the original correspondences...\n",
+              getClassName().c_str());
     remaining_correspondences = original_correspondences;
-    best_transformation_.setIdentity ();
+    best_transformation_.setIdentity();
     return;
   }
-  if (refine_ && !sac.refineModel (2.0))
-    PCL_WARN ("[pcl::registration::%s::getRemainingCorrespondences] Error refining model!\n", getClassName ().c_str ());
-
-  std::vector<int> inliers;
-  sac.getInliers (inliers);
-
-  if (inliers.size () < 3)
-  {
-    PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Less than 3 correspondences found!\n", getClassName ().c_str ());
+  if (refine_ && !sac.refineModel(2.0))
+    PCL_WARN(
+        "[pcl::registration::%s::getRemainingCorrespondences] Error refining model!\n",
+        getClassName().c_str());
+
+  pcl::Indices inliers;
+  sac.getInliers(inliers);
+
+  if (inliers.size() < 3) {
+    PCL_ERROR("[pcl::registration::%s::getRemainingCorrespondences] Less than 3 "
+              "correspondences found!\n",
+              getClassName().c_str());
     remaining_correspondences = original_correspondences;
-    best_transformation_.setIdentity ();
+    best_transformation_.setIdentity();
     return;
   }
 
@@ -121,21 +126,21 @@ CorrespondenceRejectorSampleConsensus2D<PointT>::getRemainingCorrespondences (
   for (int i = 0; i < nr_correspondences; ++i)
     index_to_correspondence[original_correspondences[i].index_query] = i;
 
-  remaining_correspondences.resize (inliers.size ());
-  for (std::size_t i = 0; i < inliers.size (); ++i)
-    remaining_correspondences[i] = original_correspondences[index_to_correspondence[inliers[i]]];
+  remaining_correspondences.resize(inliers.size());
+  for (std::size_t i = 0; i < inliers.size(); ++i)
+    remaining_correspondences[i] =
+        original_correspondences[index_to_correspondence[inliers[i]]];
 
   // get best transformation
   Eigen::VectorXf model_coefficients;
-  sac.getModelCoefficients (model_coefficients);
-  best_transformation_.row (0) = model_coefficients.segment<4>(0);
-  best_transformation_.row (1) = model_coefficients.segment<4>(4);
-  best_transformation_.row (2) = model_coefficients.segment<4>(8);
-  best_transformation_.row (3) = model_coefficients.segment<4>(12);
+  sac.getModelCoefficients(model_coefficients);
+  best_transformation_.row(0) = model_coefficients.segment<4>(0);
+  best_transformation_.row(1) = model_coefficients.segment<4>(4);
+  best_transformation_.row(2) = model_coefficients.segment<4>(8);
+  best_transformation_.row(3) = model_coefficients.segment<4>(12);
 }
 
 } // namespace registration
 } // namespace pcl
 
-#endif    // PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_2D_HPP_
-
+#endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_2D_HPP_
diff --git a/registration/include/pcl/registration/impl/correspondence_rejection_surface_normal.hpp b/registration/include/pcl/registration/impl/correspondence_rejection_surface_normal.hpp
index 6509909b..65e8bbc1 100644
--- a/registration/include/pcl/registration/impl/correspondence_rejection_surface_normal.hpp
+++ b/registration/include/pcl/registration/impl/correspondence_rejection_surface_normal.hpp
@@ -38,5 +38,5 @@
  */
 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SURFACE_NORMAL_HPP_
 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SURFACE_NORMAL_HPP_
-
+PCL_DEPRECATED_HEADER(1, 15, "")
 #endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SURFACE_NORMAL_HPP_ */
diff --git a/registration/include/pcl/registration/impl/correspondence_rejection_trimmed.hpp b/registration/include/pcl/registration/impl/correspondence_rejection_trimmed.hpp
index 0690e1dc..9aee8cda 100644
--- a/registration/include/pcl/registration/impl/correspondence_rejection_trimmed.hpp
+++ b/registration/include/pcl/registration/impl/correspondence_rejection_trimmed.hpp
@@ -39,5 +39,5 @@
  */
 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_TRIMMED_HPP_
 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_TRIMMED_HPP_
-
+PCL_DEPRECATED_HEADER(1, 15, "")
 #endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_TRIMMED_HPP_ */
diff --git a/registration/include/pcl/registration/impl/correspondence_rejection_var_trimmed.hpp b/registration/include/pcl/registration/impl/correspondence_rejection_var_trimmed.hpp
index 53cd52fa..201c6d4a 100644
--- a/registration/include/pcl/registration/impl/correspondence_rejection_var_trimmed.hpp
+++ b/registration/include/pcl/registration/impl/correspondence_rejection_var_trimmed.hpp
@@ -38,5 +38,5 @@
  */
 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_VAR_TRIMMED_HPP_
 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_VAR_TRIMMED_HPP_
-
+PCL_DEPRECATED_HEADER(1, 15, "")
 #endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_VAR_TRIMMED_HPP_ */
diff --git a/registration/include/pcl/registration/impl/correspondence_types.hpp b/registration/include/pcl/registration/impl/correspondence_types.hpp
index 2a2eb88d..c5cefbde 100644
--- a/registration/include/pcl/registration/impl/correspondence_types.hpp
+++ b/registration/include/pcl/registration/impl/correspondence_types.hpp
@@ -40,53 +40,48 @@
 
 #pragma once
 
-#include <pcl/registration/eigen.h>
-
 #include <cstddef>
 #include <vector>
 
+namespace pcl {
 
-namespace pcl
-{
-
-namespace registration
-{
+namespace registration {
 
 inline void
-getCorDistMeanStd (const pcl::Correspondences &correspondences, double &mean, double &stddev)
+getCorDistMeanStd(const pcl::Correspondences& correspondences,
+                  double& mean,
+                  double& stddev)
 {
-  if (correspondences.empty ())
+  if (correspondences.empty())
     return;
 
   double sum = 0, sq_sum = 0;
 
-  for (const auto &correspondence : correspondences)
-  {
+  for (const auto& correspondence : correspondences) {
     sum += correspondence.distance;
     sq_sum += correspondence.distance * correspondence.distance;
   }
-  mean = sum / static_cast<double> (correspondences.size ());
-  double variance = (sq_sum - sum * sum / static_cast<double> (correspondences.size ())) / static_cast<double> (correspondences.size () - 1);
-  stddev = sqrt (variance);
+  mean = sum / static_cast<double>(correspondences.size());
+  double variance = (sq_sum - sum * sum / static_cast<double>(correspondences.size())) /
+                    static_cast<double>(correspondences.size() - 1);
+  stddev = sqrt(variance);
 }
 
 inline void
-getQueryIndices (const pcl::Correspondences& correspondences, std::vector<int>& indices)
+getQueryIndices(const pcl::Correspondences& correspondences, pcl::Indices& indices)
 {
-  indices.resize (correspondences.size ());
-  for (std::size_t i = 0; i < correspondences.size (); ++i)
+  indices.resize(correspondences.size());
+  for (std::size_t i = 0; i < correspondences.size(); ++i)
     indices[i] = correspondences[i].index_query;
 }
 
-
 inline void
-getMatchIndices (const pcl::Correspondences& correspondences, std::vector<int>& indices)
+getMatchIndices(const pcl::Correspondences& correspondences, pcl::Indices& indices)
 {
-  indices.resize (correspondences.size ());
-  for (std::size_t i = 0; i < correspondences.size (); ++i)
+  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 b6743997..9b4c940b 100644
--- a/registration/include/pcl/registration/impl/default_convergence_criteria.hpp
+++ b/registration/include/pcl/registration/impl/default_convergence_criteria.hpp
@@ -42,63 +42,65 @@
 
 #include <pcl/console/print.h>
 
+namespace pcl {
 
-namespace pcl
-{
-
-namespace registration
-{
+namespace registration {
 
-template <typename Scalar> bool
-DefaultConvergenceCriteria<Scalar>::hasConverged ()
+template <typename Scalar>
+bool
+DefaultConvergenceCriteria<Scalar>::hasConverged()
 {
-  if (convergence_state_ != CONVERGENCE_CRITERIA_NOT_CONVERGED)
-  {
-    //If it already converged or failed before, reset.
+  if (convergence_state_ != CONVERGENCE_CRITERIA_NOT_CONVERGED) {
+    // If it already converged or failed before, reset.
     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_);
+  PCL_DEBUG("[pcl::DefaultConvergenceCriteria::hasConverged] Iteration %d out of %d.\n",
+            iterations_,
+            max_iterations_);
   // 1. Number of iterations has reached the maximum user imposed number of iterations
-  if (iterations_ >= max_iterations_)
-  {
-    if (!failure_after_max_iter_)
-    {
+  if (iterations_ >= max_iterations_) {
+    if (!failure_after_max_iter_) {
       convergence_state_ = CONVERGENCE_CRITERIA_ITERATIONS;
       return (true);
     }
     convergence_state_ = CONVERGENCE_CRITERIA_FAILURE_AFTER_MAX_ITERATIONS;
   }
 
-  // 2. The epsilon (difference) between the previous transformation and the current estimated transformation
-  double cos_angle = 0.5 * (transformation_.coeff (0, 0) + transformation_.coeff (1, 1) + transformation_.coeff (2, 2) - 1);
-  double translation_sqr = transformation_.coeff (0, 3) * transformation_.coeff (0, 3) +
-                           transformation_.coeff (1, 3) * transformation_.coeff (1, 3) +
-                           transformation_.coeff (2, 3) * transformation_.coeff (2, 3);
-  PCL_DEBUG ("[pcl::DefaultConvergenceCriteria::hasConverged] Current transformation gave %f rotation (cosine) and %f translation.\n", cos_angle, translation_sqr);
-
-  if (cos_angle >= rotation_threshold_ && translation_sqr <= translation_threshold_)
-  {
-    if (iterations_similar_transforms_ >= max_iterations_similar_transforms_)
-    {
+  // 2. The epsilon (difference) between the previous transformation and the current
+  // estimated transformation
+  double cos_angle = 0.5 * (transformation_.coeff(0, 0) + transformation_.coeff(1, 1) +
+                            transformation_.coeff(2, 2) - 1);
+  double translation_sqr = transformation_.coeff(0, 3) * transformation_.coeff(0, 3) +
+                           transformation_.coeff(1, 3) * transformation_.coeff(1, 3) +
+                           transformation_.coeff(2, 3) * transformation_.coeff(2, 3);
+  PCL_DEBUG("[pcl::DefaultConvergenceCriteria::hasConverged] Current transformation "
+            "gave %f rotation (cosine) and %f translation.\n",
+            cos_angle,
+            translation_sqr);
+
+  if (cos_angle >= rotation_threshold_ && translation_sqr <= translation_threshold_) {
+    if (iterations_similar_transforms_ >= max_iterations_similar_transforms_) {
       convergence_state_ = CONVERGENCE_CRITERIA_TRANSFORM;
       return (true);
     }
     is_similar = true;
   }
 
-  correspondences_cur_mse_ = calculateMSE (correspondences_);
-  PCL_DEBUG ("[pcl::DefaultConvergenceCriteria::hasConverged] Previous / Current MSE for correspondences distances is: %f / %f.\n", correspondences_prev_mse_, correspondences_cur_mse_);
-
-  // 3. The relative sum of Euclidean squared errors is smaller than a user defined threshold
-  // Absolute
-  if (std::abs (correspondences_cur_mse_ - correspondences_prev_mse_) < mse_threshold_absolute_)
-  {
-    if (iterations_similar_transforms_ >= max_iterations_similar_transforms_)
-    {
+  correspondences_cur_mse_ = calculateMSE(correspondences_);
+  PCL_DEBUG("[pcl::DefaultConvergenceCriteria::hasConverged] Previous / Current MSE "
+            "for correspondences distances is: %f / %f.\n",
+            correspondences_prev_mse_,
+            correspondences_cur_mse_);
+
+  // 3. The relative sum of Euclidean squared errors is smaller than a user defined
+  // threshold Absolute
+  if (std::abs(correspondences_cur_mse_ - correspondences_prev_mse_) <
+      mse_threshold_absolute_) {
+    if (iterations_similar_transforms_ >= max_iterations_similar_transforms_) {
       convergence_state_ = CONVERGENCE_CRITERIA_ABS_MSE;
       return (true);
     }
@@ -106,23 +108,21 @@ DefaultConvergenceCriteria<Scalar>::hasConverged ()
   }
 
   // Relative
-  if (std::abs (correspondences_cur_mse_ - correspondences_prev_mse_) / correspondences_prev_mse_ < mse_threshold_relative_)
-  {
-    if (iterations_similar_transforms_ >= max_iterations_similar_transforms_)
-    {
+  if (std::abs(correspondences_cur_mse_ - correspondences_prev_mse_) /
+          correspondences_prev_mse_ <
+      mse_threshold_relative_) {
+    if (iterations_similar_transforms_ >= max_iterations_similar_transforms_) {
       convergence_state_ = CONVERGENCE_CRITERIA_REL_MSE;
       return (true);
     }
     is_similar = true;
   }
 
-  if (is_similar)
-  {
+  if (is_similar) {
     // Increment the number of transforms that the thresholds are allowed to be similar
     ++iterations_similar_transforms_;
   }
-  else
-  {
+  else {
     // When the transform becomes large, reset.
     iterations_similar_transforms_ = 0;
   }
@@ -135,5 +135,4 @@ DefaultConvergenceCriteria<Scalar>::hasConverged ()
 } // namespace registration
 } // namespace pcl
 
-#endif    // PCL_REGISTRATION_DEFAULT_CONVERGENCE_CRITERIA_HPP_
-
+#endif // PCL_REGISTRATION_DEFAULT_CONVERGENCE_CRITERIA_HPP_
diff --git a/registration/include/pcl/registration/impl/elch.hpp b/registration/include/pcl/registration/impl/elch.hpp
index 4b234f5f..647386cf 100644
--- a/registration/include/pcl/registration/impl/elch.hpp
+++ b/registration/include/pcl/registration/impl/elch.hpp
@@ -41,91 +41,87 @@
 #ifndef PCL_REGISTRATION_IMPL_ELCH_H_
 #define PCL_REGISTRATION_IMPL_ELCH_H_
 
+#include <pcl/common/transforms.h>
+#include <pcl/registration/registration.h>
+
+#include <boost/graph/dijkstra_shortest_paths.hpp> // for dijkstra_shortest_paths
+
 #include <algorithm>
 #include <list>
 #include <tuple>
 
-#include <pcl/common/transforms.h>
-#include <pcl/registration/eigen.h>
-#include <pcl/registration/boost.h>
-#include <pcl/registration/registration.h>
-
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> void
-pcl::registration::ELCH<PointT>::loopOptimizerAlgorithm (LOAGraph &g, double *weights)
+template <typename PointT>
+void
+pcl::registration::ELCH<PointT>::loopOptimizerAlgorithm(LOAGraph& g, double* weights)
 {
   std::list<int> crossings, branches;
-  crossings.push_back (static_cast<int> (loop_start_));
-  crossings.push_back (static_cast<int> (loop_end_));
+  crossings.push_back(static_cast<int>(loop_start_));
+  crossings.push_back(static_cast<int>(loop_end_));
   weights[loop_start_] = 0;
   weights[loop_end_] = 1;
 
-  int *p = new int[num_vertices (g)];
-  int *p_min = new int[num_vertices (g)];
-  double *d = new double[num_vertices (g)];
-  double *d_min = new double[num_vertices (g)];
+  int* p = new int[num_vertices(g)];
+  int* p_min = new int[num_vertices(g)];
+  double* d = new double[num_vertices(g)];
+  double* d_min = new double[num_vertices(g)];
   bool do_swap = false;
   std::list<int>::iterator start_min, end_min;
 
   // process all junctions
-  while (!crossings.empty ())
-  {
+  while (!crossings.empty()) {
     double dist = -1;
     // find shortest crossing for all vertices on the loop
-    for (auto crossings_it = crossings.begin (); crossings_it != crossings.end (); )
-    {
-      dijkstra_shortest_paths (g, *crossings_it,
-          predecessor_map(boost::make_iterator_property_map(p, get(boost::vertex_index, g))).
-          distance_map(boost::make_iterator_property_map(d, get(boost::vertex_index, g))));
+    for (auto crossings_it = crossings.begin(); crossings_it != crossings.end();) {
+      dijkstra_shortest_paths(g,
+                              *crossings_it,
+                              predecessor_map(boost::make_iterator_property_map(
+                                                  p, get(boost::vertex_index, g)))
+                                  .distance_map(boost::make_iterator_property_map(
+                                      d, get(boost::vertex_index, g))));
 
       auto end_it = crossings_it;
       end_it++;
       // find shortest crossing for one vertex
-      for (; end_it != crossings.end (); end_it++)
-      {
-        if (*end_it != p[*end_it] && (dist < 0 || d[*end_it] < dist))
-        {
+      for (; end_it != crossings.end(); end_it++) {
+        if (*end_it != p[*end_it] && (dist < 0 || d[*end_it] < dist)) {
           dist = d[*end_it];
           start_min = crossings_it;
           end_min = end_it;
           do_swap = true;
         }
       }
-      if (do_swap)
-      {
-        std::swap (p, p_min);
-        std::swap (d, d_min);
+      if (do_swap) {
+        std::swap(p, p_min);
+        std::swap(d, d_min);
         do_swap = false;
       }
       // vertex starts a branch
-      if (dist < 0)
-      {
-        branches.push_back (static_cast<int> (*crossings_it));
-        crossings_it = crossings.erase (crossings_it);
+      if (dist < 0) {
+        branches.push_back(static_cast<int>(*crossings_it));
+        crossings_it = crossings.erase(crossings_it);
       }
       else
         crossings_it++;
     }
 
-    if (dist > -1)
-    {
-      remove_edge (*end_min, p_min[*end_min], g);
-      for (int i = p_min[*end_min]; i != *start_min; i = p_min[i])
-      {
-        //even right with weights[*start_min] > weights[*end_min]! (math works)
-        weights[i] = weights[*start_min] + (weights[*end_min] - weights[*start_min]) * d_min[i] / d_min[*end_min];
-        remove_edge (i, p_min[i], g);
-        if (degree (i, g) > 0)
-        {
-          crossings.push_back (i);
+    if (dist > -1) {
+      remove_edge(*end_min, p_min[*end_min], g);
+      for (int i = p_min[*end_min]; i != *start_min; i = p_min[i]) {
+        // even right with weights[*start_min] > weights[*end_min]! (math works)
+        weights[i] = weights[*start_min] + (weights[*end_min] - weights[*start_min]) *
+                                               d_min[i] / d_min[*end_min];
+        remove_edge(i, p_min[i], g);
+        if (degree(i, g) > 0) {
+          crossings.push_back(i);
         }
       }
 
-      if (degree (*start_min, g) == 0)
-        crossings.erase (start_min);
+      if (degree(*start_min, g) == 0)
+        crossings.erase(start_min);
 
-      if (degree (*end_min, g) == 0)
-        crossings.erase (end_min);
+      if (degree(*end_min, g) == 0)
+        crossings.erase(end_min);
     }
   }
 
@@ -137,24 +133,25 @@ pcl::registration::ELCH<PointT>::loopOptimizerAlgorithm (LOAGraph &g, double *we
   boost::graph_traits<LOAGraph>::adjacency_iterator adjacent_it, adjacent_it_end;
 
   // error propagation
-  while (!branches.empty ())
-  {
-    int s = branches.front ();
-    branches.pop_front ();
+  while (!branches.empty()) {
+    int s = branches.front();
+    branches.pop_front();
 
-    for (std::tie (adjacent_it, adjacent_it_end) = adjacent_vertices (s, g); adjacent_it != adjacent_it_end; ++adjacent_it)
-    {
+    for (std::tie(adjacent_it, adjacent_it_end) = adjacent_vertices(s, g);
+         adjacent_it != adjacent_it_end;
+         ++adjacent_it) {
       weights[*adjacent_it] = weights[s];
-      if (degree (*adjacent_it, g) > 1)
-        branches.push_back (static_cast<int> (*adjacent_it));
+      if (degree(*adjacent_it, g) > 1)
+        branches.push_back(static_cast<int>(*adjacent_it));
     }
-    clear_vertex (s, g);
+    clear_vertex(s, g);
   }
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> bool
-pcl::registration::ELCH<PointT>::initCompute ()
+template <typename PointT>
+bool
+pcl::registration::ELCH<PointT>::initCompute()
 {
   /*if (!PCLBase<PointT>::initCompute ())
   {
@@ -162,115 +159,118 @@ pcl::registration::ELCH<PointT>::initCompute ()
     return (false);
   }*/ //TODO
 
-  if (loop_end_ == 0)
-  {
-    PCL_ERROR ("[pcl::registration::ELCH::initCompute] no end of loop defined!\n");
-    deinitCompute ();
+  if (loop_end_ == 0) {
+    PCL_ERROR("[pcl::registration::ELCH::initCompute] no end of loop defined!\n");
+    deinitCompute();
     return (false);
   }
 
-  //compute transformation if it's not given
-  if (compute_loop_)
-  {
-    PointCloudPtr meta_start (new PointCloud);
-    PointCloudPtr meta_end (new PointCloud);
+  // compute transformation if it's not given
+  if (compute_loop_) {
+    PointCloudPtr meta_start(new PointCloud);
+    PointCloudPtr meta_end(new PointCloud);
     *meta_start = *(*loop_graph_)[loop_start_].cloud;
     *meta_end = *(*loop_graph_)[loop_end_].cloud;
 
     typename boost::graph_traits<LoopGraph>::adjacency_iterator si, si_end;
-    for (std::tie (si, si_end) = adjacent_vertices (loop_start_, *loop_graph_); si != si_end; si++)
+    for (std::tie(si, si_end) = adjacent_vertices(loop_start_, *loop_graph_);
+         si != si_end;
+         si++)
       *meta_start += *(*loop_graph_)[*si].cloud;
 
-    for (std::tie (si, si_end) = adjacent_vertices (loop_end_, *loop_graph_); si != si_end; si++)
+    for (std::tie(si, si_end) = adjacent_vertices(loop_end_, *loop_graph_);
+         si != si_end;
+         si++)
       *meta_end += *(*loop_graph_)[*si].cloud;
 
-    //TODO use real pose instead of centroid
-    //Eigen::Vector4f pose_start;
-    //pcl::compute3DCentroid (*(*loop_graph_)[loop_start_].cloud, pose_start);
-
-    //Eigen::Vector4f pose_end;
-    //pcl::compute3DCentroid (*(*loop_graph_)[loop_end_].cloud, pose_end);
+    // TODO use real pose instead of centroid
+    // Eigen::Vector4f pose_start;
+    // pcl::compute3DCentroid (*(*loop_graph_)[loop_start_].cloud, pose_start);
 
-    PointCloudPtr tmp (new PointCloud);
-    //Eigen::Vector4f diff = pose_start - pose_end;
-    //Eigen::Translation3f translation (diff.head (3));
-    //Eigen::Affine3f trans = translation * Eigen::Quaternionf::Identity ();
-    //pcl::transformPointCloud (*(*loop_graph_)[loop_end_].cloud, *tmp, trans);
+    // Eigen::Vector4f pose_end;
+    // pcl::compute3DCentroid (*(*loop_graph_)[loop_end_].cloud, pose_end);
 
-    reg_->setInputTarget (meta_start);
+    PointCloudPtr tmp(new PointCloud);
+    // Eigen::Vector4f diff = pose_start - pose_end;
+    // Eigen::Translation3f translation (diff.head (3));
+    // Eigen::Affine3f trans = translation * Eigen::Quaternionf::Identity ();
+    // pcl::transformPointCloud (*(*loop_graph_)[loop_end_].cloud, *tmp, trans);
 
-    reg_->setInputSource (meta_end);
+    reg_->setInputTarget(meta_start);
 
-    reg_->align (*tmp);
+    reg_->setInputSource(meta_end);
 
-    loop_transform_ = reg_->getFinalTransformation ();
-    //TODO hack
-    //loop_transform_ *= trans.matrix ();
+    reg_->align(*tmp);
 
+    loop_transform_ = reg_->getFinalTransformation();
+    // TODO hack
+    // loop_transform_ *= trans.matrix ();
   }
 
   return (true);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> void
-pcl::registration::ELCH<PointT>::compute ()
+template <typename PointT>
+void
+pcl::registration::ELCH<PointT>::compute()
 {
-  if (!initCompute ())
-  {
+  if (!initCompute()) {
     return;
   }
 
   LOAGraph grb[4];
 
   typename boost::graph_traits<LoopGraph>::edge_iterator edge_it, edge_it_end;
-  for (std::tie (edge_it, edge_it_end) = edges (*loop_graph_); edge_it != edge_it_end; edge_it++)
-  {
-    for (auto &j : grb)
-      add_edge (source (*edge_it, *loop_graph_), target (*edge_it, *loop_graph_), 1, j);  //TODO add variance
+  for (std::tie(edge_it, edge_it_end) = edges(*loop_graph_); edge_it != edge_it_end;
+       edge_it++) {
+    for (auto& j : grb)
+      add_edge(source(*edge_it, *loop_graph_),
+               target(*edge_it, *loop_graph_),
+               1,
+               j); // TODO add variance
   }
 
-  double *weights[4];
-  for (int i = 0; i < 4; i++)
-  {
-    weights[i] = new double[num_vertices (*loop_graph_)];
-    loopOptimizerAlgorithm (grb[i], weights[i]);
+  double* weights[4];
+  for (int i = 0; i < 4; i++) {
+    weights[i] = new double[num_vertices(*loop_graph_)];
+    loopOptimizerAlgorithm(grb[i], weights[i]);
   }
 
-  //TODO use pose
-  //Eigen::Vector4f cend;
-  //pcl::compute3DCentroid (*((*loop_graph_)[loop_end_].cloud), cend);
-  //Eigen::Translation3f tend (cend.head (3));
-  //Eigen::Affine3f aend (tend);
-  //Eigen::Affine3f aendI = aend.inverse ();
-
-  //TODO iterate ovr loop_graph_
-  //typename boost::graph_traits<LoopGraph>::vertex_iterator vertex_it, vertex_it_end;
-  //for (std::tie (vertex_it, vertex_it_end) = vertices (*loop_graph_); vertex_it != vertex_it_end; vertex_it++)
-  for (std::size_t i = 0; i < num_vertices (*loop_graph_); i++)
-  {
+  // TODO use pose
+  // Eigen::Vector4f cend;
+  // pcl::compute3DCentroid (*((*loop_graph_)[loop_end_].cloud), cend);
+  // Eigen::Translation3f tend (cend.head (3));
+  // Eigen::Affine3f aend (tend);
+  // Eigen::Affine3f aendI = aend.inverse ();
+
+  // TODO iterate ovr loop_graph_
+  // typename boost::graph_traits<LoopGraph>::vertex_iterator vertex_it, vertex_it_end;
+  // for (std::tie (vertex_it, vertex_it_end) = vertices (*loop_graph_); vertex_it !=
+  // vertex_it_end; vertex_it++)
+  for (std::size_t i = 0; i < num_vertices(*loop_graph_); i++) {
     Eigen::Vector3f t2;
-    t2[0] = loop_transform_ (0, 3) * static_cast<float> (weights[0][i]);
-    t2[1] = loop_transform_ (1, 3) * static_cast<float> (weights[1][i]);
-    t2[2] = loop_transform_ (2, 3) * static_cast<float> (weights[2][i]);
+    t2[0] = loop_transform_(0, 3) * static_cast<float>(weights[0][i]);
+    t2[1] = loop_transform_(1, 3) * static_cast<float>(weights[1][i]);
+    t2[2] = loop_transform_(2, 3) * static_cast<float>(weights[2][i]);
 
-    Eigen::Affine3f bl (loop_transform_);
-    Eigen::Quaternionf q (bl.rotation ());
+    Eigen::Affine3f bl(loop_transform_);
+    Eigen::Quaternionf q(bl.rotation());
     Eigen::Quaternionf q2;
-    q2 = Eigen::Quaternionf::Identity ().slerp (static_cast<float> (weights[3][i]), q);
+    q2 = Eigen::Quaternionf::Identity().slerp(static_cast<float>(weights[3][i]), q);
 
-    //TODO use rotation from branch start
-    Eigen::Translation3f t3 (t2);
-    Eigen::Affine3f a (t3 * q2);
-    //a = aend * a * aendI;
+    // TODO use rotation from branch start
+    Eigen::Translation3f t3(t2);
+    Eigen::Affine3f a(t3 * q2);
+    // a = aend * a * aendI;
 
-    pcl::transformPointCloud (*(*loop_graph_)[i].cloud, *(*loop_graph_)[i].cloud, a);
+    pcl::transformPointCloud(*(*loop_graph_)[i].cloud, *(*loop_graph_)[i].cloud, a);
     (*loop_graph_)[i].transform = a;
   }
 
-  add_edge (loop_start_, loop_end_, *loop_graph_);
+  add_edge(loop_start_, loop_end_, *loop_graph_);
 
-  deinitCompute ();
+  deinitCompute();
 }
 
 #endif // PCL_REGISTRATION_IMPL_ELCH_H_
diff --git a/registration/include/pcl/registration/impl/gicp.hpp b/registration/include/pcl/registration/impl/gicp.hpp
index 85853750..c6f12bee 100644
--- a/registration/include/pcl/registration/impl/gicp.hpp
+++ b/registration/include/pcl/registration/impl/gicp.hpp
@@ -41,93 +41,95 @@
 #ifndef PCL_REGISTRATION_IMPL_GICP_HPP_
 #define PCL_REGISTRATION_IMPL_GICP_HPP_
 
-#include <pcl/registration/boost.h>
 #include <pcl/registration/exceptions.h>
 
-
-namespace pcl
-{
+namespace pcl {
 
 template <typename PointSource, typename PointTarget>
-template<typename PointT> void
-GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud,
-                                                                               const typename pcl::search::KdTree<PointT>::Ptr kdtree,
-                                                                               MatricesVector& cloud_covariances)
+template <typename PointT>
+void
+GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeCovariances(
+    typename pcl::PointCloud<PointT>::ConstPtr cloud,
+    const typename pcl::search::KdTree<PointT>::Ptr kdtree,
+    MatricesVector& cloud_covariances)
 {
-  if (k_correspondences_ > int (cloud->size ()))
-  {
-    PCL_ERROR ("[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or points in cloud (%lu) is less than k_correspondences_ (%lu)!\n", cloud->size (), k_correspondences_);
+  if (k_correspondences_ > int(cloud->size())) {
+    PCL_ERROR("[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or "
+              "points in cloud (%lu) is less than k_correspondences_ (%lu)!\n",
+              cloud->size(),
+              k_correspondences_);
     return;
   }
 
   Eigen::Vector3d mean;
-  std::vector<int> nn_indecies; nn_indecies.reserve (k_correspondences_);
-  std::vector<float> nn_dist_sq; nn_dist_sq.reserve (k_correspondences_);
+  pcl::Indices nn_indecies;
+  nn_indecies.reserve(k_correspondences_);
+  std::vector<float> nn_dist_sq;
+  nn_dist_sq.reserve(k_correspondences_);
 
   // We should never get there but who knows
-  if(cloud_covariances.size () < cloud->size ())
-    cloud_covariances.resize (cloud->size ());
-
-  MatricesVector::iterator matrices_iterator = cloud_covariances.begin ();
-  for(auto points_iterator = cloud->begin ();
-      points_iterator != cloud->end ();
-      ++points_iterator, ++matrices_iterator)
-  {
-    const PointT &query_point = *points_iterator;
-    Eigen::Matrix3d &cov = *matrices_iterator;
+  if (cloud_covariances.size() < cloud->size())
+    cloud_covariances.resize(cloud->size());
+
+  MatricesVector::iterator matrices_iterator = cloud_covariances.begin();
+  for (auto points_iterator = cloud->begin(); points_iterator != cloud->end();
+       ++points_iterator, ++matrices_iterator) {
+    const PointT& query_point = *points_iterator;
+    Eigen::Matrix3d& cov = *matrices_iterator;
     // Zero out the cov and mean
-    cov.setZero ();
-    mean.setZero ();
+    cov.setZero();
+    mean.setZero();
 
     // Search for the K nearest neighbours
     kdtree->nearestKSearch(query_point, k_correspondences_, nn_indecies, nn_dist_sq);
 
     // Find the covariance matrix
-    for(int j = 0; j < k_correspondences_; j++) {
-      const PointT &pt = (*cloud)[nn_indecies[j]];
+    for (int j = 0; j < k_correspondences_; j++) {
+      const PointT& pt = (*cloud)[nn_indecies[j]];
 
       mean[0] += pt.x;
       mean[1] += pt.y;
       mean[2] += pt.z;
 
-      cov(0,0) += pt.x*pt.x;
+      cov(0, 0) += pt.x * pt.x;
 
-      cov(1,0) += pt.y*pt.x;
-      cov(1,1) += pt.y*pt.y;
+      cov(1, 0) += pt.y * pt.x;
+      cov(1, 1) += pt.y * pt.y;
 
-      cov(2,0) += pt.z*pt.x;
-      cov(2,1) += pt.z*pt.y;
-      cov(2,2) += pt.z*pt.z;
+      cov(2, 0) += pt.z * pt.x;
+      cov(2, 1) += pt.z * pt.y;
+      cov(2, 2) += pt.z * pt.z;
     }
 
-    mean /= static_cast<double> (k_correspondences_);
+    mean /= static_cast<double>(k_correspondences_);
     // Get the actual covariance
     for (int k = 0; k < 3; k++)
-      for (int l = 0; l <= k; l++)
-      {
-        cov(k,l) /= static_cast<double> (k_correspondences_);
-        cov(k,l) -= mean[k]*mean[l];
-        cov(l,k) = cov(k,l);
+      for (int l = 0; l <= k; l++) {
+        cov(k, l) /= static_cast<double>(k_correspondences_);
+        cov(k, l) -= mean[k] * mean[l];
+        cov(l, k) = cov(k, l);
       }
 
     // Compute the SVD (covariance matrix is symmetric so U = V')
     Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov, Eigen::ComputeFullU);
-    cov.setZero ();
-    Eigen::Matrix3d U = svd.matrixU ();
-    // Reconstitute the covariance matrix with modified singular values using the column     // vectors in V.
-    for(int k = 0; k < 3; k++) {
+    cov.setZero();
+    Eigen::Matrix3d U = svd.matrixU();
+    // Reconstitute the covariance matrix with modified singular values using the column
+    // // vectors in V.
+    for (int k = 0; k < 3; k++) {
       Eigen::Vector3d col = U.col(k);
       double v = 1.; // biggest 2 singular values replaced by 1
-      if(k == 2)   // smallest singular value replaced by gicp_epsilon
+      if (k == 2)    // smallest singular value replaced by gicp_epsilon
         v = gicp_epsilon_;
-      cov+= v * col * col.transpose();
+      cov += v * col * col.transpose();
     }
   }
 }
 
-
-template <typename PointSource, typename PointTarget> void
-GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d& g) const
+template <typename PointSource, typename PointTarget>
+void
+GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeRDerivative(
+    const Vector6d& x, const Eigen::Matrix3d& R, Vector6d& g) const
 {
   Eigen::Matrix3d dR_dPhi;
   Eigen::Matrix3d dR_dTheta;
@@ -139,72 +141,76 @@ GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeRDerivative(c
   double ctheta = std::cos(theta), stheta = sin(theta);
   double cpsi = std::cos(psi), spsi = sin(psi);
 
-  dR_dPhi(0,0) = 0.;
-  dR_dPhi(1,0) = 0.;
-  dR_dPhi(2,0) = 0.;
+  dR_dPhi(0, 0) = 0.;
+  dR_dPhi(1, 0) = 0.;
+  dR_dPhi(2, 0) = 0.;
 
-  dR_dPhi(0,1) = sphi*spsi + cphi*cpsi*stheta;
-  dR_dPhi(1,1) = -cpsi*sphi + cphi*spsi*stheta;
-  dR_dPhi(2,1) = cphi*ctheta;
+  dR_dPhi(0, 1) = sphi * spsi + cphi * cpsi * stheta;
+  dR_dPhi(1, 1) = -cpsi * sphi + cphi * spsi * stheta;
+  dR_dPhi(2, 1) = cphi * ctheta;
 
-  dR_dPhi(0,2) = cphi*spsi - cpsi*sphi*stheta;
-  dR_dPhi(1,2) = -cphi*cpsi - sphi*spsi*stheta;
-  dR_dPhi(2,2) = -ctheta*sphi;
+  dR_dPhi(0, 2) = cphi * spsi - cpsi * sphi * stheta;
+  dR_dPhi(1, 2) = -cphi * cpsi - sphi * spsi * stheta;
+  dR_dPhi(2, 2) = -ctheta * sphi;
 
-  dR_dTheta(0,0) = -cpsi*stheta;
-  dR_dTheta(1,0) = -spsi*stheta;
-  dR_dTheta(2,0) = -ctheta;
+  dR_dTheta(0, 0) = -cpsi * stheta;
+  dR_dTheta(1, 0) = -spsi * stheta;
+  dR_dTheta(2, 0) = -ctheta;
 
-  dR_dTheta(0,1) = cpsi*ctheta*sphi;
-  dR_dTheta(1,1) = ctheta*sphi*spsi;
-  dR_dTheta(2,1) = -sphi*stheta;
+  dR_dTheta(0, 1) = cpsi * ctheta * sphi;
+  dR_dTheta(1, 1) = ctheta * sphi * spsi;
+  dR_dTheta(2, 1) = -sphi * stheta;
 
-  dR_dTheta(0,2) = cphi*cpsi*ctheta;
-  dR_dTheta(1,2) = cphi*ctheta*spsi;
-  dR_dTheta(2,2) = -cphi*stheta;
+  dR_dTheta(0, 2) = cphi * cpsi * ctheta;
+  dR_dTheta(1, 2) = cphi * ctheta * spsi;
+  dR_dTheta(2, 2) = -cphi * stheta;
 
-  dR_dPsi(0,0) = -ctheta*spsi;
-  dR_dPsi(1,0) = cpsi*ctheta;
-  dR_dPsi(2,0) = 0.;
+  dR_dPsi(0, 0) = -ctheta * spsi;
+  dR_dPsi(1, 0) = cpsi * ctheta;
+  dR_dPsi(2, 0) = 0.;
 
-  dR_dPsi(0,1) = -cphi*cpsi - sphi*spsi*stheta;
-  dR_dPsi(1,1) = -cphi*spsi + cpsi*sphi*stheta;
-  dR_dPsi(2,1) = 0.;
+  dR_dPsi(0, 1) = -cphi * cpsi - sphi * spsi * stheta;
+  dR_dPsi(1, 1) = -cphi * spsi + cpsi * sphi * stheta;
+  dR_dPsi(2, 1) = 0.;
 
-  dR_dPsi(0,2) = cpsi*sphi - cphi*spsi*stheta;
-  dR_dPsi(1,2) = sphi*spsi + cphi*cpsi*stheta;
-  dR_dPsi(2,2) = 0.;
+  dR_dPsi(0, 2) = cpsi * sphi - cphi * spsi * stheta;
+  dR_dPsi(1, 2) = sphi * spsi + cphi * cpsi * stheta;
+  dR_dPsi(2, 2) = 0.;
 
   g[3] = matricesInnerProd(dR_dPhi, R);
   g[4] = matricesInnerProd(dR_dTheta, R);
   g[5] = matricesInnerProd(dR_dPsi, R);
 }
 
-
-template <typename PointSource, typename PointTarget> void
-GeneralizedIterativeClosestPoint<PointSource, PointTarget>::estimateRigidTransformationBFGS (const PointCloudSource &cloud_src,
-                                                                                             const std::vector<int> &indices_src,
-                                                                                             const PointCloudTarget &cloud_tgt,
-                                                                                             const std::vector<int> &indices_tgt,
-                                                                                             Eigen::Matrix4f &transformation_matrix)
+template <typename PointSource, typename PointTarget>
+void
+GeneralizedIterativeClosestPoint<PointSource, PointTarget>::
+    estimateRigidTransformationBFGS(const PointCloudSource& cloud_src,
+                                    const pcl::Indices& indices_src,
+                                    const PointCloudTarget& cloud_tgt,
+                                    const pcl::Indices& indices_tgt,
+                                    Eigen::Matrix4f& transformation_matrix)
 {
-  if (indices_src.size () < 4)     // need at least 4 samples
+  if (indices_src.size() < 4) // need at least 4 samples
   {
-    PCL_THROW_EXCEPTION (NotEnoughPointsException,
-                         "[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS] Need at least 4 points to estimate a transform! Source and target have " << indices_src.size () << " points!");
+    PCL_THROW_EXCEPTION(
+        NotEnoughPointsException,
+        "[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS] Need "
+        "at least 4 points to estimate a transform! Source and target have "
+            << indices_src.size() << " points!");
     return;
   }
   // Set the initial solution
-  Vector6d x = Vector6d::Zero ();
+  Vector6d x = Vector6d::Zero();
   // translation part
-  x[0] = transformation_matrix (0,3);
-  x[1] = transformation_matrix (1,3);
-  x[2] = transformation_matrix (2,3);
+  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));
+  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));
 
   // Set temporary pointers
   tmp_src_ = &cloud_src;
@@ -214,7 +220,7 @@ GeneralizedIterativeClosestPoint<PointSource, PointTarget>::estimateRigidTransfo
 
   // Optimize using forward-difference approximation LM
   OptimizationFunctorWithIndices functor(this);
-  BFGS<OptimizationFunctorWithIndices> bfgs (functor);
+  BFGS<OptimizationFunctorWithIndices> bfgs(functor);
   bfgs.parameters.sigma = 0.01;
   bfgs.parameters.rho = 0.01;
   bfgs.parameters.tau1 = 9;
@@ -223,130 +229,143 @@ GeneralizedIterativeClosestPoint<PointSource, PointTarget>::estimateRigidTransfo
   bfgs.parameters.order = 3;
 
   int inner_iterations_ = 0;
-  int result = bfgs.minimizeInit (x);
+  int result = bfgs.minimizeInit(x);
   result = BFGSSpace::Running;
-  do
-  {
+  do {
     inner_iterations_++;
-    result = bfgs.minimizeOneStep (x);
-    if(result)
-    {
+    result = bfgs.minimizeOneStep(x);
+    if (result) {
       break;
     }
     result = bfgs.testGradient();
-  } while(result == BFGSSpace::Running && inner_iterations_ < max_inner_iterations_);
-  if(result == BFGSSpace::NoProgress || result == BFGSSpace::Success || inner_iterations_ == max_inner_iterations_)
-  {
-    PCL_DEBUG ("[pcl::registration::TransformationEstimationBFGS::estimateRigidTransformation]");
-    PCL_DEBUG ("BFGS solver finished with exit code %i \n", result);
+  } while (result == BFGSSpace::Running && inner_iterations_ < max_inner_iterations_);
+  if (result == BFGSSpace::NoProgress || result == BFGSSpace::Success ||
+      inner_iterations_ == max_inner_iterations_) {
+    PCL_DEBUG("[pcl::registration::TransformationEstimationBFGS::"
+              "estimateRigidTransformation]");
+    PCL_DEBUG("BFGS solver finished with exit code %i \n", result);
     transformation_matrix.setIdentity();
     applyState(transformation_matrix, x);
   }
   else
-    PCL_THROW_EXCEPTION(SolverDidntConvergeException,
-                        "[pcl::" << getClassName () << "::TransformationEstimationBFGS::estimateRigidTransformation] BFGS solver didn't converge!");
+    PCL_THROW_EXCEPTION(
+        SolverDidntConvergeException,
+        "[pcl::" << getClassName()
+                 << "::TransformationEstimationBFGS::estimateRigidTransformation] BFGS "
+                    "solver didn't converge!");
 }
 
-
-template <typename PointSource, typename PointTarget> inline double
-GeneralizedIterativeClosestPoint<PointSource, PointTarget>::OptimizationFunctorWithIndices::operator() (const Vector6d& x)
+template <typename PointSource, typename PointTarget>
+inline double
+GeneralizedIterativeClosestPoint<PointSource, PointTarget>::
+    OptimizationFunctorWithIndices::operator()(const Vector6d& x)
 {
   Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
   gicp_->applyState(transformation_matrix, x);
   double f = 0;
-  int m = static_cast<int> (gicp_->tmp_idx_src_->size ());
-  for (int i = 0; i < m; ++i)
-  {
+  int m = static_cast<int>(gicp_->tmp_idx_src_->size());
+  for (int i = 0; i < m; ++i) {
     // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
-    Vector4fMapConst p_src = (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
+    Vector4fMapConst p_src =
+        (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap();
     // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
-    Vector4fMapConst p_tgt = (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
-    Eigen::Vector4f pp (transformation_matrix * p_src);
+    Vector4fMapConst p_tgt =
+        (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
+    Eigen::Vector4f pp(transformation_matrix * p_src);
     // Estimate the distance (cost function)
     // The last coordinate is still guaranteed to be set to 1.0
     Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
-    Eigen::Vector3d temp (gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
-    //increment= res'*temp/num_matches = temp'*M*temp/num_matches (we postpone 1/num_matches after the loop closes)
-    f+= double(res.transpose() * temp);
+    Eigen::Vector3d temp(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
+    // increment= res'*temp/num_matches = temp'*M*temp/num_matches (we postpone
+    // 1/num_matches after the loop closes)
+    f += double(res.transpose() * temp);
   }
-  return f/m;
+  return f / m;
 }
 
-
-template <typename PointSource, typename PointTarget> inline void
-GeneralizedIterativeClosestPoint<PointSource, PointTarget>::OptimizationFunctorWithIndices::df (const Vector6d& x, Vector6d& g)
+template <typename PointSource, typename PointTarget>
+inline void
+GeneralizedIterativeClosestPoint<PointSource, PointTarget>::
+    OptimizationFunctorWithIndices::df(const Vector6d& x, Vector6d& g)
 {
   Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
   gicp_->applyState(transformation_matrix, x);
-  //Zero out g
-  g.setZero ();
-  //Eigen::Vector3d g_t = g.head<3> ();
-  Eigen::Matrix3d R = Eigen::Matrix3d::Zero ();
-  int m = static_cast<int> (gicp_->tmp_idx_src_->size ());
-  for (int i = 0; i < m; ++i)
-  {
+  // Zero out g
+  g.setZero();
+  // Eigen::Vector3d g_t = g.head<3> ();
+  Eigen::Matrix3d R = Eigen::Matrix3d::Zero();
+  int m = static_cast<int>(gicp_->tmp_idx_src_->size());
+  for (int i = 0; i < m; ++i) {
     // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
-    Vector4fMapConst p_src = (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
+    Vector4fMapConst p_src =
+        (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap();
     // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
-    Vector4fMapConst p_tgt = (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
+    Vector4fMapConst p_tgt =
+        (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
 
-    Eigen::Vector4f pp (transformation_matrix * p_src);
+    Eigen::Vector4f pp(transformation_matrix * p_src);
     // The last coordinate is still guaranteed to be set to 1.0
-    Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
+    Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
     // temp = M*res
-    Eigen::Vector3d temp (gicp_->mahalanobis ((*gicp_->tmp_idx_src_)[i]) * res);
+    Eigen::Vector3d temp(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
     // Increment translation gradient
-    // g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the loop closes)
-    g.head<3> ()+= temp;
+    // g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the loop
+    // closes)
+    g.head<3>() += temp;
     // Increment rotation gradient
     pp = gicp_->base_transformation_ * p_src;
-    Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]);
-    R+= p_src3 * temp.transpose();
+    Eigen::Vector3d p_src3(pp[0], pp[1], pp[2]);
+    R += p_src3 * temp.transpose();
   }
-  g.head<3> ()*= 2.0/m;
-  R*= 2.0/m;
+  g.head<3>() *= 2.0 / m;
+  R *= 2.0 / m;
   gicp_->computeRDerivative(x, R, g);
 }
 
-
-template <typename PointSource, typename PointTarget> inline void
-GeneralizedIterativeClosestPoint<PointSource, PointTarget>::OptimizationFunctorWithIndices::fdf (const Vector6d& x, double& f, Vector6d& g)
+template <typename PointSource, typename PointTarget>
+inline void
+GeneralizedIterativeClosestPoint<PointSource, PointTarget>::
+    OptimizationFunctorWithIndices::fdf(const Vector6d& x, double& f, Vector6d& g)
 {
   Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
   gicp_->applyState(transformation_matrix, x);
   f = 0;
-  g.setZero ();
-  Eigen::Matrix3d R = Eigen::Matrix3d::Zero ();
-  const int m = static_cast<int> (gicp_->tmp_idx_src_->size ());
-  for (int i = 0; i < m; ++i)
-  {
+  g.setZero();
+  Eigen::Matrix3d R = Eigen::Matrix3d::Zero();
+  const int m = static_cast<int>(gicp_->tmp_idx_src_->size());
+  for (int i = 0; i < m; ++i) {
     // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
-    Vector4fMapConst p_src = (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
+    Vector4fMapConst p_src =
+        (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap();
     // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
-    Vector4fMapConst p_tgt = (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
-    Eigen::Vector4f pp (transformation_matrix * p_src);
+    Vector4fMapConst p_tgt =
+        (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
+    Eigen::Vector4f pp(transformation_matrix * p_src);
     // The last coordinate is still guaranteed to be set to 1.0
-    Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
+    Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
     // temp = M*res
-    Eigen::Vector3d temp (gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
+    Eigen::Vector3d temp(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
     // Increment total error
-    f+= double(res.transpose() * temp);
+    f += double(res.transpose() * temp);
     // Increment translation gradient
-    // g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the loop closes)
-    g.head<3> ()+= temp;
+    // g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the loop
+    // closes)
+    g.head<3>() += temp;
     pp = gicp_->base_transformation_ * p_src;
-    Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]);
+    Eigen::Vector3d p_src3(pp[0], pp[1], pp[2]);
     // Increment rotation gradient
-    R+= p_src3 * temp.transpose();
+    R += p_src3 * temp.transpose();
   }
-  f/= double(m);
-  g.head<3> ()*= double(2.0/m);
-  R*= 2.0/m;
+  f /= double(m);
+  g.head<3>() *= double(2.0 / m);
+  R *= 2.0 / m;
   gicp_->computeRDerivative(x, R, g);
 }
 
-template <typename PointSource, typename PointTarget> inline BFGSSpace::Status
-GeneralizedIterativeClosestPoint<PointSource, PointTarget>::OptimizationFunctorWithIndices::checkGradient(const Vector6d& g)
+template <typename PointSource, typename PointTarget>
+inline BFGSSpace::Status
+GeneralizedIterativeClosestPoint<PointSource, PointTarget>::
+    OptimizationFunctorWithIndices::checkGradient(const Vector6d& g)
 {
   auto translation_epsilon = gicp_->translation_gradient_tolerance_;
   auto rotation_epsilon = gicp_->rotation_gradient_tolerance_;
@@ -361,148 +380,157 @@ GeneralizedIterativeClosestPoint<PointSource, PointTarget>::OptimizationFunctorW
   auto rotation_grad = g.tail<3>().norm();
 
   if ((translation_grad < translation_epsilon) && (rotation_grad < rotation_epsilon))
-	return BFGSSpace::Success;
+    return BFGSSpace::Success;
 
   return BFGSSpace::Running;
 }
 
-template <typename PointSource, typename PointTarget> inline void
-GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
+template <typename PointSource, typename PointTarget>
+inline void
+GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeTransformation(
+    PointCloudSource& output, const Eigen::Matrix4f& guess)
 {
-  pcl::IterativeClosestPoint<PointSource, PointTarget>::initComputeReciprocal ();
+  pcl::IterativeClosestPoint<PointSource, PointTarget>::initComputeReciprocal();
   // Difference between consecutive transforms
   double delta = 0;
   // Get the size of the target
-  const std::size_t N = indices_->size ();
+  const std::size_t N = indices_->size();
   // Set the mahalanobis matrices to identity
-  mahalanobis_.resize (N, Eigen::Matrix3d::Identity ());
+  mahalanobis_.resize(N, Eigen::Matrix3d::Identity());
   // Compute target cloud covariance matrices
-  if ((!target_covariances_) || (target_covariances_->empty ()))
-  {
-    target_covariances_.reset (new MatricesVector);
-    computeCovariances<PointTarget> (target_, tree_, *target_covariances_);
+  if ((!target_covariances_) || (target_covariances_->empty())) {
+    target_covariances_.reset(new MatricesVector);
+    computeCovariances<PointTarget>(target_, tree_, *target_covariances_);
   }
   // Compute input cloud covariance matrices
-  if ((!input_covariances_) || (input_covariances_->empty ()))
-  {
-    input_covariances_.reset (new MatricesVector);
-    computeCovariances<PointSource> (input_, tree_reciprocal_, *input_covariances_);
+  if ((!input_covariances_) || (input_covariances_->empty())) {
+    input_covariances_.reset(new MatricesVector);
+    computeCovariances<PointSource>(input_, tree_reciprocal_, *input_covariances_);
   }
 
   base_transformation_ = Eigen::Matrix4f::Identity();
   nr_iterations_ = 0;
   converged_ = false;
   double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
-  std::vector<int> nn_indices (1);
-  std::vector<float> nn_dists (1);
+  pcl::Indices nn_indices(1);
+  std::vector<float> nn_dists(1);
 
   pcl::transformPointCloud(output, output, guess);
 
-  while(!converged_)
-  {
+  while (!converged_) {
     std::size_t cnt = 0;
-    std::vector<int> source_indices (indices_->size ());
-    std::vector<int> target_indices (indices_->size ());
+    pcl::Indices source_indices(indices_->size());
+    pcl::Indices target_indices(indices_->size());
 
     // guess corresponds to base_t and transformation_ to t
-    Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero ();
-    for(std::size_t i = 0; i < 4; i++)
-      for(std::size_t j = 0; j < 4; j++)
-        for(std::size_t k = 0; k < 4; k++)
-          transform_R(i,j)+= double(transformation_(i,k)) * double(guess(k,j));
+    Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero();
+    for (std::size_t i = 0; i < 4; i++)
+      for (std::size_t j = 0; j < 4; j++)
+        for (std::size_t k = 0; k < 4; k++)
+          transform_R(i, j) += double(transformation_(i, k)) * double(guess(k, j));
 
-    Eigen::Matrix3d R = transform_R.topLeftCorner<3,3> ();
+    Eigen::Matrix3d R = transform_R.topLeftCorner<3, 3>();
 
-    for (std::size_t i = 0; i < N; i++)
-    {
+    for (std::size_t i = 0; i < N; i++) {
       PointSource query = output[i];
-      query.getVector4fMap () = transformation_ * query.getVector4fMap ();
+      query.getVector4fMap() = transformation_ * query.getVector4fMap();
 
-      if (!searchForNeighbors (query, nn_indices, nn_dists))
-      {
-        PCL_ERROR ("[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n", getClassName ().c_str (), (*indices_)[i]);
+      if (!searchForNeighbors(query, nn_indices, nn_dists)) {
+        PCL_ERROR("[pcl::%s::computeTransformation] Unable to find a nearest neighbor "
+                  "in the target dataset for point %d in the source!\n",
+                  getClassName().c_str(),
+                  (*indices_)[i]);
         return;
       }
 
-      // Check if the distance to the nearest neighbor is smaller than the user imposed threshold
-      if (nn_dists[0] < dist_threshold)
-      {
-        Eigen::Matrix3d &C1 = (*input_covariances_)[i];
-        Eigen::Matrix3d &C2 = (*target_covariances_)[nn_indices[0]];
-        Eigen::Matrix3d &M = mahalanobis_[i];
+      // Check if the distance to the nearest neighbor is smaller than the user imposed
+      // threshold
+      if (nn_dists[0] < dist_threshold) {
+        Eigen::Matrix3d& C1 = (*input_covariances_)[i];
+        Eigen::Matrix3d& C2 = (*target_covariances_)[nn_indices[0]];
+        Eigen::Matrix3d& M = mahalanobis_[i];
         // M = R*C1
         M = R * C1;
         // temp = M*R' + C2 = R*C1*R' + C2
         Eigen::Matrix3d temp = M * R.transpose();
-        temp+= C2;
+        temp += C2;
         // M = temp^-1
-        M = temp.inverse ();
-        source_indices[cnt] = static_cast<int> (i);
+        M = temp.inverse();
+        source_indices[cnt] = static_cast<int>(i);
         target_indices[cnt] = nn_indices[0];
         cnt++;
       }
     }
     // Resize to the actual number of valid correspondences
-    source_indices.resize(cnt); target_indices.resize(cnt);
+    source_indices.resize(cnt);
+    target_indices.resize(cnt);
     /* optimize transformation using the current assignment and Mahalanobis metrics*/
     previous_transformation_ = transformation_;
-    //optimization right here
-    try
-    {
-      rigid_transformation_estimation_(output, source_indices, *target_, target_indices, transformation_);
+    // optimization right here
+    try {
+      rigid_transformation_estimation_(
+          output, source_indices, *target_, target_indices, transformation_);
       /* compute the delta from this iteration */
       delta = 0.;
-      for(int k = 0; k < 4; k++) {
-        for(int l = 0; l < 4; l++) {
+      for (int k = 0; k < 4; k++) {
+        for (int l = 0; l < 4; l++) {
           double ratio = 1;
-          if(k < 3 && l < 3) // rotation part of the transform
-            ratio = 1./rotation_epsilon_;
+          if (k < 3 && l < 3) // rotation part of the transform
+            ratio = 1. / rotation_epsilon_;
           else
-            ratio = 1./transformation_epsilon_;
-          double c_delta = ratio*std::abs(previous_transformation_(k,l) - transformation_(k,l));
-          if(c_delta > delta)
+            ratio = 1. / transformation_epsilon_;
+          double c_delta =
+              ratio * std::abs(previous_transformation_(k, l) - transformation_(k, l));
+          if (c_delta > delta)
             delta = c_delta;
         }
       }
-    }
-    catch (PCLException &e)
-    {
-      PCL_DEBUG ("[pcl::%s::computeTransformation] Optimization issue %s\n", getClassName ().c_str (), e.what ());
+    } catch (PCLException& e) {
+      PCL_DEBUG("[pcl::%s::computeTransformation] Optimization issue %s\n",
+                getClassName().c_str(),
+                e.what());
       break;
     }
     nr_iterations_++;
     // Check for convergence
-    if (nr_iterations_ >= max_iterations_ || delta < 1)
-    {
+    if (nr_iterations_ >= max_iterations_ || delta < 1) {
       converged_ = true;
-      PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n",
-                 getClassName ().c_str (), nr_iterations_, max_iterations_, (transformation_ - previous_transformation_).array ().abs ().sum ());
+      PCL_DEBUG("[pcl::%s::computeTransformation] Convergence reached. Number of "
+                "iterations: %d out of %d. Transformation difference: %f\n",
+                getClassName().c_str(),
+                nr_iterations_,
+                max_iterations_,
+                (transformation_ - previous_transformation_).array().abs().sum());
       previous_transformation_ = transformation_;
     }
     else
-      PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence failed\n", getClassName ().c_str ());
+      PCL_DEBUG("[pcl::%s::computeTransformation] Convergence failed\n",
+                getClassName().c_str());
   }
   final_transformation_ = previous_transformation_ * guess;
 
   // Transform the point cloud
-  pcl::transformPointCloud (*input_, output, final_transformation_);
+  pcl::transformPointCloud(*input_, output, final_transformation_);
 }
 
-
-template <typename PointSource, typename PointTarget> void
-GeneralizedIterativeClosestPoint<PointSource, PointTarget>::applyState(Eigen::Matrix4f &t, const Vector6d& x) const
+template <typename PointSource, typename PointTarget>
+void
+GeneralizedIterativeClosestPoint<PointSource, PointTarget>::applyState(
+    Eigen::Matrix4f& t, const Vector6d& x) const
 {
   // Z Y X euler angles convention
   Eigen::Matrix3f R;
-  R = Eigen::AngleAxisf (static_cast<float> (x[5]), Eigen::Vector3f::UnitZ ())
-    * Eigen::AngleAxisf (static_cast<float> (x[4]), Eigen::Vector3f::UnitY ())
-    * Eigen::AngleAxisf (static_cast<float> (x[3]), Eigen::Vector3f::UnitX ());
-  t.topLeftCorner<3,3> ().matrix () = R * t.topLeftCorner<3,3> ().matrix ();
-  Eigen::Vector4f T (static_cast<float> (x[0]), static_cast<float> (x[1]), static_cast<float> (x[2]), 0.0f);
-  t.col (3) += T;
+  R = Eigen::AngleAxisf(static_cast<float>(x[5]), Eigen::Vector3f::UnitZ()) *
+      Eigen::AngleAxisf(static_cast<float>(x[4]), Eigen::Vector3f::UnitY()) *
+      Eigen::AngleAxisf(static_cast<float>(x[3]), Eigen::Vector3f::UnitX());
+  t.topLeftCorner<3, 3>().matrix() = R * t.topLeftCorner<3, 3>().matrix();
+  Eigen::Vector4f T(static_cast<float>(x[0]),
+                    static_cast<float>(x[1]),
+                    static_cast<float>(x[2]),
+                    0.0f);
+  t.col(3) += T;
 }
 
 } // namespace pcl
 
-#endif //PCL_REGISTRATION_IMPL_GICP_HPP_
-
+#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 44e0f898..07f85579 100644
--- a/registration/include/pcl/registration/impl/ia_fpcs.hpp
+++ b/registration/include/pcl/registration/impl/ia_fpcs.hpp
@@ -20,7 +20,7 @@
  *     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
+ *  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
@@ -38,27 +38,30 @@
 #ifndef PCL_REGISTRATION_IMPL_IA_FPCS_H_
 #define PCL_REGISTRATION_IMPL_IA_FPCS_H_
 
-#include <pcl/registration/ia_fpcs.h>
-#include <pcl/common/time.h>
 #include <pcl/common/distances.h>
+#include <pcl/common/time.h>
 #include <pcl/common/utils.h>
-#include <pcl/sample_consensus/sac_model_plane.h>
+#include <pcl/registration/ia_fpcs.h>
 #include <pcl/registration/transformation_estimation_3point.h>
+#include <pcl/sample_consensus/sac_model_plane.h>
 
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> inline float
-pcl::getMeanPointDensity (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, float max_dist, int nr_threads)
+template <typename PointT>
+inline float
+pcl::getMeanPointDensity(const typename pcl::PointCloud<PointT>::ConstPtr& cloud,
+                         float max_dist,
+                         int nr_threads)
 {
   const float max_dist_sqr = max_dist * max_dist;
-  const std::size_t s = cloud.size ();
+  const std::size_t s = cloud.size();
 
-  pcl::search::KdTree <PointT> tree;
-  tree.setInputCloud (cloud);
+  pcl::search::KdTree<PointT> tree;
+  tree.setInputCloud(cloud);
 
   float mean_dist = 0.f;
   int num = 0;
-  std::vector <int> ids (2);
-  std::vector <float> dists_sqr (2);
+  pcl::Indices ids(2);
+  std::vector<float> dists_sqr(2);
 
   pcl::utils::ignore(nr_threads);
 #pragma omp parallel for \
@@ -68,12 +71,10 @@ pcl::getMeanPointDensity (const typename pcl::PointCloud<PointT>::ConstPtr &clou
   reduction(+:mean_dist, num) \
   firstprivate(s, max_dist_sqr) \
   num_threads(nr_threads)
-  for (int i = 0; i < 1000; i++)
-  {
-    tree.nearestKSearch ((*cloud)[rand () % s], 2, ids, dists_sqr);
-    if (dists_sqr[1] < max_dist_sqr)
-    {
-      mean_dist += std::sqrt (dists_sqr[1]);
+  for (int i = 0; i < 1000; i++) {
+    tree.nearestKSearch((*cloud)[rand() % s], 2, ids, dists_sqr);
+    if (dists_sqr[1] < max_dist_sqr) {
+      mean_dist += std::sqrt(dists_sqr[1]);
       num++;
     }
   }
@@ -81,22 +82,24 @@ pcl::getMeanPointDensity (const typename pcl::PointCloud<PointT>::ConstPtr &clou
   return (mean_dist / num);
 };
 
-
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> inline float
-pcl::getMeanPointDensity (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, const std::vector <int> &indices,
-  float max_dist, int nr_threads)
+template <typename PointT>
+inline float
+pcl::getMeanPointDensity(const typename pcl::PointCloud<PointT>::ConstPtr& cloud,
+                         const pcl::Indices& indices,
+                         float max_dist,
+                         int nr_threads)
 {
   const float max_dist_sqr = max_dist * max_dist;
-  const std::size_t s = indices.size ();
+  const std::size_t s = indices.size();
 
-  pcl::search::KdTree <PointT> tree;
-  tree.setInputCloud (cloud);
+  pcl::search::KdTree<PointT> tree;
+  tree.setInputCloud(cloud);
 
   float mean_dist = 0.f;
   int num = 0;
-  std::vector <int> ids (2);
-  std::vector <float> dists_sqr (2);
+  pcl::Indices ids(2);
+  std::vector<float> dists_sqr(2);
 
   pcl::utils::ignore(nr_threads);
 #if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
@@ -114,12 +117,10 @@ pcl::getMeanPointDensity (const typename pcl::PointCloud<PointT>::ConstPtr &clou
   reduction(+:mean_dist, num) \
   num_threads(nr_threads)
 #endif
-  for (int i = 0; i < 1000; i++)
-  {
-    tree.nearestKSearch ((*cloud)[indices[rand () % s]], 2, ids, dists_sqr);
-    if (dists_sqr[1] < max_dist_sqr)
-    {
-      mean_dist += std::sqrt (dists_sqr[1]);
+  for (int i = 0; i < 1000; i++) {
+    tree.nearestKSearch((*cloud)[indices[rand() % s]], 2, ids, dists_sqr);
+    if (dists_sqr[1] < max_dist_sqr) {
+      mean_dist += std::sqrt(dists_sqr[1]);
       num++;
     }
   }
@@ -127,161 +128,162 @@ pcl::getMeanPointDensity (const typename pcl::PointCloud<PointT>::ConstPtr &clou
   return (mean_dist / num);
 };
 
-
 ///////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
-pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::FPCSInitialAlignment () :
-  source_normals_ (),
-  target_normals_ (),
-  nr_threads_ (1),
-  approx_overlap_ (0.5f),
-  delta_ (1.f),
-  score_threshold_ (FLT_MAX),
-  nr_samples_ (0),
-  max_norm_diff_ (90.f),
-  max_runtime_ (0),
-  fitness_score_ (FLT_MAX),
-  diameter_ (),
-  max_base_diameter_sqr_ (),
-  use_normals_ (false),
-  normalize_delta_ (true),
-  max_pair_diff_ (),
-  max_edge_diff_ (),
-  coincidation_limit_ (),
-  max_mse_ (),
-  max_inlier_dist_sqr_ (),
-  small_error_ (0.00001f)
+pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
+    FPCSInitialAlignment()
+: source_normals_()
+, target_normals_()
+, nr_threads_(1)
+, approx_overlap_(0.5f)
+, delta_(1.f)
+, score_threshold_(FLT_MAX)
+, nr_samples_(0)
+, max_norm_diff_(90.f)
+, max_runtime_(0)
+, fitness_score_(FLT_MAX)
+, diameter_()
+, max_base_diameter_sqr_()
+, use_normals_(false)
+, normalize_delta_(true)
+, max_pair_diff_()
+, max_edge_diff_()
+, coincidation_limit_()
+, max_mse_()
+, max_inlier_dist_sqr_()
+, small_error_(0.00001f)
 {
   reg_name_ = "pcl::registration::FPCSInitialAlignment";
   max_iterations_ = 0;
   ransac_iterations_ = 1000;
-  transformation_estimation_.reset (new pcl::registration::TransformationEstimation3Point <PointSource, PointTarget>);
+  transformation_estimation_.reset(
+      new pcl::registration::TransformationEstimation3Point<PointSource, PointTarget>);
 }
 
-
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
-pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::computeTransformation (
-  PointCloudSource &output,
-  const Eigen::Matrix4f &guess)
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+void
+pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
+    computeTransformation(PointCloudSource& output, const Eigen::Matrix4f& guess)
 {
-  if (!initCompute ())
+  if (!initCompute())
     return;
 
   final_transformation_ = guess;
   bool abort = false;
-  std::vector <MatchingCandidates> all_candidates (max_iterations_);
+  std::vector<MatchingCandidates> all_candidates(max_iterations_);
   pcl::StopWatch timer;
 
-  #pragma omp parallel \
-    default(none) \
-    shared(abort, all_candidates, timer) \
+#pragma omp parallel default(none) shared(abort, all_candidates, timer)                \
     num_threads(nr_threads_)
   {
-    #ifdef _OPENMP
-    std::srand (static_cast <unsigned int> (std::time (NULL)) ^ omp_get_thread_num ());    
-    #pragma omp for schedule(dynamic)
-    #endif
-    for (int i = 0; i < max_iterations_; i++)
-    {
-      #pragma omp flush (abort)
-
-      MatchingCandidates candidates (1);
-      std::vector <int> base_indices (4);
+#ifdef _OPENMP
+    const unsigned int seed =
+        static_cast<unsigned int>(std::time(NULL)) ^ omp_get_thread_num();
+    std::srand(seed);
+    PCL_DEBUG("[%s::computeTransformation] Using seed=%u\n", reg_name_.c_str(), seed);
+#pragma omp for schedule(dynamic)
+#endif
+    for (int i = 0; i < max_iterations_; i++) {
+#pragma omp flush(abort)
+
+      MatchingCandidates candidates(1);
+      pcl::Indices base_indices(4);
       all_candidates[i] = candidates;
 
-      if (!abort)
-      {
+      if (!abort) {
         float ratio[2];
         // select four coplanar point base
-        if (selectBase (base_indices, ratio) == 0)
-        {
+        if (selectBase(base_indices, ratio) == 0) {
           // calculate candidate pair correspondences using diagonal lengths of base
           pcl::Correspondences pairs_a, pairs_b;
-          if (bruteForceCorrespondences (base_indices[0], base_indices[1], pairs_a) == 0 &&
-            bruteForceCorrespondences (base_indices[2], base_indices[3], pairs_b) == 0)
-          {
-            // determine candidate matches by combining pair correspondences based on segment distances
-            std::vector <std::vector <int> > matches;
-            if (determineBaseMatches (base_indices, matches, pairs_a, pairs_b, ratio) == 0)
-            {
+          if (bruteForceCorrespondences(base_indices[0], base_indices[1], pairs_a) ==
+                  0 &&
+              bruteForceCorrespondences(base_indices[2], base_indices[3], pairs_b) ==
+                  0) {
+            // determine candidate matches by combining pair correspondences based on
+            // segment distances
+            std::vector<pcl::Indices> matches;
+            if (determineBaseMatches(base_indices, matches, pairs_a, pairs_b, ratio) ==
+                0) {
               // check and evaluate candidate matches and store them
-              handleMatches (base_indices, matches, candidates);
-              if (!candidates.empty ())
+              handleMatches(base_indices, matches, candidates);
+              if (!candidates.empty())
                 all_candidates[i] = candidates;
             }
           }
         }
 
         // check terminate early (time or fitness_score threshold reached)
-        abort = (!candidates.empty () ? candidates[0].fitness_score < score_threshold_ : abort);
-        abort = (abort ? abort : timer.getTimeSeconds () > max_runtime_);
+        abort = (!candidates.empty() ? candidates[0].fitness_score < score_threshold_
+                                     : abort);
+        abort = (abort ? abort : timer.getTimeSeconds() > max_runtime_);
 
-
-        #pragma omp flush (abort)
+#pragma omp flush(abort)
       }
     }
   }
-  
 
   // determine best match over all tries
-  finalCompute (all_candidates);
+  finalCompute(all_candidates);
 
   // apply the final transformation
-  pcl::transformPointCloud (*input_, output, final_transformation_);
+  pcl::transformPointCloud(*input_, output, final_transformation_);
 
-  deinitCompute ();
+  deinitCompute();
 }
 
-
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> bool
-pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::initCompute ()
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+bool
+pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
+    initCompute()
 {
-  std::srand (static_cast <unsigned int> (std::time (nullptr)));
+  const unsigned int seed = std::time(nullptr);
+  std::srand(seed);
+  PCL_DEBUG("[%s::initCompute] Using seed=%u\n", reg_name_.c_str(), seed);
 
   // basic pcl initialization
-  if (!pcl::PCLBase <PointSource>::initCompute ())
+  if (!pcl::PCLBase<PointSource>::initCompute())
     return (false);
 
   // check if source and target are given
-  if (!input_ || !target_)
-  {
-    PCL_ERROR ("[%s::initCompute] Source or target dataset not given!\n", reg_name_.c_str ());
+  if (!input_ || !target_) {
+    PCL_ERROR("[%s::initCompute] Source or target dataset not given!\n",
+              reg_name_.c_str());
     return (false);
   }
 
-  if (!target_indices_ || target_indices_->empty ())
-  {
-    target_indices_.reset (new std::vector <int> (static_cast <int> (target_->size ())));
+  if (!target_indices_ || target_indices_->empty()) {
+    target_indices_.reset(new pcl::Indices(target_->size()));
     int index = 0;
-    for (int &target_index : *target_indices_)
+    for (auto& target_index : *target_indices_)
       target_index = index++;
     target_cloud_updated_ = true;
   }
 
-  // if a sample size for the point clouds is given; prefarably no sampling of target cloud
-  if (nr_samples_ != 0)
-  {
-    const int ss = static_cast <int> (indices_->size ());
-    const int sample_fraction_src = std::max (1, static_cast <int> (ss / nr_samples_));
+  // if a sample size for the point clouds is given; prefarably no sampling of target
+  // cloud
+  if (nr_samples_ != 0) {
+    const int ss = static_cast<int>(indices_->size());
+    const int sample_fraction_src = std::max(1, static_cast<int>(ss / nr_samples_));
 
-    source_indices_ = pcl::IndicesPtr (new std::vector <int>);
+    source_indices_ = pcl::IndicesPtr(new pcl::Indices);
     for (int i = 0; i < ss; i++)
-    if (rand () % sample_fraction_src == 0)
-      source_indices_->push_back ((*indices_) [i]);
+      if (rand() % sample_fraction_src == 0)
+        source_indices_->push_back((*indices_)[i]);
   }
   else
     source_indices_ = indices_;
 
   // check usage of normals
-  if (source_normals_ && target_normals_  && source_normals_->size () == input_->size () && target_normals_->size () == target_->size ())
+  if (source_normals_ && target_normals_ && source_normals_->size() == input_->size() &&
+      target_normals_->size() == target_->size())
     use_normals_ = true;
 
   // set up tree structures
-  if (target_cloud_updated_)
-  {
-    tree_->setInputCloud (target_, target_indices_);
+  if (target_cloud_updated_) {
+    tree_->setInputCloud(target_, target_indices_);
     target_cloud_updated_ = false;
   }
 
@@ -291,25 +293,28 @@ pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scal
 
   // get diameter of input cloud (distance between farthest points)
   Eigen::Vector4f pt_min, pt_max;
-  pcl::getMinMax3D (*target_, *target_indices_, pt_min, pt_max);
-  diameter_ = (pt_max - pt_min).norm ();
+  pcl::getMinMax3D(*target_, *target_indices_, pt_min, pt_max);
+  diameter_ = (pt_max - pt_min).norm();
 
   // derive the limits for the random base selection
-  float max_base_diameter = diameter_* approx_overlap_ * 2.f;
+  float max_base_diameter = diameter_ * approx_overlap_ * 2.f;
   max_base_diameter_sqr_ = max_base_diameter * max_base_diameter;
 
   // normalize the delta
-  if (normalize_delta_)
-  {
-    float mean_dist = getMeanPointDensity <PointTarget> (target_, *target_indices_, 0.05f * diameter_, nr_threads_);
+  if (normalize_delta_) {
+    float mean_dist = getMeanPointDensity<PointTarget>(
+        target_, *target_indices_, 0.05f * diameter_, nr_threads_);
     delta_ *= mean_dist;
   }
 
-  // heuristic determination of number of trials to have high probability of finding a good solution
-  if (max_iterations_ == 0)
-  {
-    float first_est = std::log (small_error_) / std::log (1.0 - std::pow ((double) approx_overlap_, (double) min_iterations));
-    max_iterations_ = static_cast <int> (first_est / (diameter_fraction * approx_overlap_ * 2.f));
+  // heuristic determination of number of trials to have high probability of finding a
+  // good solution
+  if (max_iterations_ == 0) {
+    float first_est =
+        std::log(small_error_) /
+        std::log(1.0 - std::pow((double)approx_overlap_, (double)min_iterations));
+    max_iterations_ =
+        static_cast<int>(first_est / (diameter_fraction * approx_overlap_ * 2.f));
   }
 
   // set further parameter
@@ -326,8 +331,8 @@ pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scal
   max_pair_diff_ = delta_ * 2.f;
   max_edge_diff_ = delta_ * 4.f;
   coincidation_limit_ = delta_ * 2.f; // EDITED: originally std::sqrt (delta_ * 2.f)
-  max_mse_ = powf (delta_* 2.f, 2.f);
-  max_inlier_dist_sqr_ = powf (delta_ * 2.f, 2.f);
+  max_mse_ = powf(delta_ * 2.f, 2.f);
+  max_inlier_dist_sqr_ = powf(delta_ * 2.f, 2.f);
 
   // reset fitness_score
   fitness_score_ = FLT_MAX;
@@ -335,65 +340,64 @@ pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scal
   return (true);
 }
 
-
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
-pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::selectBase (
-  std::vector <int> &base_indices,
-  float (&ratio)[2])
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+int
+pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
+    selectBase(pcl::Indices& base_indices, float (&ratio)[2])
 {
-  const float too_close_sqr = max_base_diameter_sqr_*0.01;
+  const float too_close_sqr = max_base_diameter_sqr_ * 0.01;
 
-  Eigen::VectorXf coefficients (4);
-  pcl::SampleConsensusModelPlane <PointTarget> plane (target_);
-  plane.setIndices (target_indices_);
+  Eigen::VectorXf coefficients(4);
+  pcl::SampleConsensusModelPlane<PointTarget> plane(target_);
+  plane.setIndices(target_indices_);
   Eigen::Vector4f centre_pt;
   float nearest_to_plane = FLT_MAX;
 
-  // repeat base search until valid quadruple was found or ransac_iterations_ number of tries were unsuccessful
-  for (int i = 0; i < ransac_iterations_; i++)
-  {
+  // repeat base search until valid quadruple was found or ransac_iterations_ number of
+  // tries were unsuccessful
+  for (int i = 0; i < ransac_iterations_; i++) {
     // random select an appropriate point triple
-    if (selectBaseTriangle (base_indices) < 0)
+    if (selectBaseTriangle(base_indices) < 0)
       continue;
 
-    std::vector <int> base_triple (base_indices.begin (), base_indices.end () - 1);
-    plane.computeModelCoefficients (base_triple, coefficients);
-    pcl::compute3DCentroid (*target_, base_triple, centre_pt);
+    pcl::Indices base_triple(base_indices.begin(), base_indices.end() - 1);
+    plane.computeModelCoefficients(base_triple, coefficients);
+    pcl::compute3DCentroid(*target_, base_triple, centre_pt);
 
     // loop over all points in source cloud to find most suitable fourth point
-    const PointTarget *pt1 = &((*target_)[base_indices[0]]);
-    const PointTarget *pt2 = &((*target_)[base_indices[1]]);
-    const PointTarget *pt3 = &((*target_)[base_indices[2]]);
-
-    for (const int &target_index : *target_indices_)
-    {
-      const PointTarget *pt4 = &((*target_)[target_index]);
-
-      float d1 = pcl::squaredEuclideanDistance (*pt4, *pt1);
-      float d2 = pcl::squaredEuclideanDistance (*pt4, *pt2);
-      float d3 = pcl::squaredEuclideanDistance (*pt4, *pt3);
-      float d4 = (pt4->getVector3fMap ()  - centre_pt.head (3)).squaredNorm ();
-
-      // check distance between points w.r.t minimum sampling distance; EDITED -> 4th point now also limited by max base line
-      if (d1 < too_close_sqr || d2 < too_close_sqr || d3 < too_close_sqr || d4 < too_close_sqr ||
-        d1 > max_base_diameter_sqr_ || d2 > max_base_diameter_sqr_ || d3 > max_base_diameter_sqr_)
+    const PointTarget* pt1 = &((*target_)[base_indices[0]]);
+    const PointTarget* pt2 = &((*target_)[base_indices[1]]);
+    const PointTarget* pt3 = &((*target_)[base_indices[2]]);
+
+    for (const auto& target_index : *target_indices_) {
+      const PointTarget* pt4 = &((*target_)[target_index]);
+
+      float d1 = pcl::squaredEuclideanDistance(*pt4, *pt1);
+      float d2 = pcl::squaredEuclideanDistance(*pt4, *pt2);
+      float d3 = pcl::squaredEuclideanDistance(*pt4, *pt3);
+      float d4 = (pt4->getVector3fMap() - centre_pt.head(3)).squaredNorm();
+
+      // check distance between points w.r.t minimum sampling distance; EDITED -> 4th
+      // point now also limited by max base line
+      if (d1 < too_close_sqr || d2 < too_close_sqr || d3 < too_close_sqr ||
+          d4 < too_close_sqr || d1 > max_base_diameter_sqr_ ||
+          d2 > max_base_diameter_sqr_ || d3 > max_base_diameter_sqr_)
         continue;
 
       // check distance to plane to get point closest to plane
-      float dist_to_plane = pcl::pointToPlaneDistance (*pt4, coefficients);
-      if (dist_to_plane < nearest_to_plane)
-      {
+      float dist_to_plane = pcl::pointToPlaneDistance(*pt4, coefficients);
+      if (dist_to_plane < nearest_to_plane) {
         base_indices[3] = target_index;
         nearest_to_plane = dist_to_plane;
       }
     }
 
     // check if at least one point fulfilled the conditions
-    if (nearest_to_plane != FLT_MAX)
-    {
-      // order points to build largest quadrangle and calcuate intersection ratios of diagonals
-      setupBase (base_indices, ratio);
+    if (nearest_to_plane != FLT_MAX) {
+      // order points to build largest quadrangle and calcuate intersection ratios of
+      // diagonals
+      setupBase(base_indices, ratio);
       return (0);
     }
   }
@@ -402,31 +406,34 @@ pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scal
   return (-1);
 }
 
-
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
-pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::selectBaseTriangle (std::vector <int> &base_indices)
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+int
+pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
+    selectBaseTriangle(pcl::Indices& base_indices)
 {
-  int nr_points = static_cast <int> (target_indices_->size ());
+  const auto nr_points = target_indices_->size();
   float best_t = 0.f;
 
   // choose random first point
-  base_indices[0] = (*target_indices_)[rand () % nr_points];
-  int *index1 = &base_indices[0];
+  base_indices[0] = (*target_indices_)[rand() % nr_points];
+  auto* index1 = &base_indices[0];
 
   // random search for 2 other points (as far away as overlap allows)
-  for (int i = 0; i < ransac_iterations_; i++)
-  {
-    int *index2 = &(*target_indices_)[rand () % nr_points];
-    int *index3 = &(*target_indices_)[rand () % nr_points];
+  for (int i = 0; i < ransac_iterations_; i++) {
+    auto* index2 = &(*target_indices_)[rand() % nr_points];
+    auto* index3 = &(*target_indices_)[rand() % nr_points];
 
-    Eigen::Vector3f u = (*target_)[*index2].getVector3fMap () - (*target_)[*index1].getVector3fMap ();
-    Eigen::Vector3f v = (*target_)[*index3].getVector3fMap () - (*target_)[*index1].getVector3fMap ();
-    float t = u.cross (v).squaredNorm (); // triangle area (0.5 * sqrt(t)) should be maximal
+    Eigen::Vector3f u =
+        (*target_)[*index2].getVector3fMap() - (*target_)[*index1].getVector3fMap();
+    Eigen::Vector3f v =
+        (*target_)[*index3].getVector3fMap() - (*target_)[*index1].getVector3fMap();
+    float t =
+        u.cross(v).squaredNorm(); // triangle area (0.5 * sqrt(t)) should be maximal
 
     // check for most suitable point triple
-    if (t > best_t && u.squaredNorm () < max_base_diameter_sqr_ && v.squaredNorm () < max_base_diameter_sqr_)
-    {
+    if (t > best_t && u.squaredNorm() < max_base_diameter_sqr_ &&
+        v.squaredNorm() < max_base_diameter_sqr_) {
       best_t = t;
       base_indices[1] = *index2;
       base_indices[2] = *index3;
@@ -437,103 +444,97 @@ pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scal
   return (best_t == 0.f ? -1 : 0);
 }
 
-
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
-pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::setupBase (
-  std::vector <int> &base_indices,
-  float (&ratio)[2])
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+void
+pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
+    setupBase(pcl::Indices& base_indices, float (&ratio)[2])
 {
   float best_t = FLT_MAX;
-  const std::vector <int> copy (base_indices.begin (), base_indices.end ());
-  std::vector <int> temp (base_indices.begin (), base_indices.end ());
+  const pcl::Indices copy(base_indices.begin(), base_indices.end());
+  pcl::Indices temp(base_indices.begin(), base_indices.end());
 
   // loop over all combinations of base points
-  for (std::vector <int>::const_iterator i = copy.begin (), i_e = copy.end (); i != i_e; ++i)
-  for (std::vector <int>::const_iterator j = copy.begin (), j_e = copy.end (); j != j_e; ++j)
-  {
-    if (i == j)
-      continue;
-
-    for (std::vector <int>::const_iterator k = copy.begin (), k_e = copy.end (); k != k_e; ++k)
-    {
-      if (k == j || k == i)
+  for (auto i = copy.begin(), i_e = copy.end(); i != i_e; ++i)
+    for (auto j = copy.begin(), j_e = copy.end(); j != j_e; ++j) {
+      if (i == j)
         continue;
 
-      std::vector <int>::const_iterator l = copy.begin ();
-      while (l == i || l == j || l == k)
-        ++l;
-
-      temp[0] = *i;
-      temp[1] = *j;
-      temp[2] = *k;
-      temp[3] = *l;
-
-      // calculate diagonal intersection ratios and check for suitable segment to segment distances
-      float ratio_temp[2];
-      float t = segmentToSegmentDist (temp, ratio_temp);
-      if (t < best_t)
-      {
-        best_t = t;
-        ratio[0] = ratio_temp[0];
-        ratio[1] = ratio_temp[1];
-        base_indices = temp;
+      for (auto k = copy.begin(), k_e = copy.end(); k != k_e; ++k) {
+        if (k == j || k == i)
+          continue;
+
+        auto l = copy.begin();
+        while (l == i || l == j || l == k)
+          ++l;
+
+        temp[0] = *i;
+        temp[1] = *j;
+        temp[2] = *k;
+        temp[3] = *l;
+
+        // calculate diagonal intersection ratios and check for suitable segment to
+        // segment distances
+        float ratio_temp[2];
+        float t = segmentToSegmentDist(temp, ratio_temp);
+        if (t < best_t) {
+          best_t = t;
+          ratio[0] = ratio_temp[0];
+          ratio[1] = ratio_temp[1];
+          base_indices = temp;
+        }
       }
     }
-  }
 }
 
-
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> float
-pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::segmentToSegmentDist (
-  const std::vector <int> &base_indices,
-  float (&ratio)[2])
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+float
+pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
+    segmentToSegmentDist(const pcl::Indices& base_indices, float (&ratio)[2])
 {
   // get point vectors
-  Eigen::Vector3f u = (*target_)[base_indices[1]].getVector3fMap () - (*target_)[base_indices[0]].getVector3fMap ();
-  Eigen::Vector3f v = (*target_)[base_indices[3]].getVector3fMap () - (*target_)[base_indices[2]].getVector3fMap ();
-  Eigen::Vector3f w = (*target_)[base_indices[0]].getVector3fMap () - (*target_)[base_indices[2]].getVector3fMap ();
+  Eigen::Vector3f u = (*target_)[base_indices[1]].getVector3fMap() -
+                      (*target_)[base_indices[0]].getVector3fMap();
+  Eigen::Vector3f v = (*target_)[base_indices[3]].getVector3fMap() -
+                      (*target_)[base_indices[2]].getVector3fMap();
+  Eigen::Vector3f w = (*target_)[base_indices[0]].getVector3fMap() -
+                      (*target_)[base_indices[2]].getVector3fMap();
 
   // calculate segment distances
-  float a = u.dot (u);
-  float b = u.dot (v);
-  float c = v.dot (v);
-  float d = u.dot (w);
-  float e = v.dot (w);
+  float a = u.dot(u);
+  float b = u.dot(v);
+  float c = v.dot(v);
+  float d = u.dot(w);
+  float e = v.dot(w);
   float D = a * c - b * b;
   float sN = 0.f, sD = D;
   float tN = 0.f, tD = D;
 
   // check segments
-  if (D < small_error_)
-  {
+  if (D < small_error_) {
     sN = 0.f;
     sD = 1.f;
     tN = e;
     tD = c;
   }
-  else
-  {
+  else {
     sN = (b * e - c * d);
     tN = (a * e - b * d);
 
-    if (sN < 0.f)
-    {
+    if (sN < 0.f) {
       sN = 0.f;
       tN = e;
       tD = c;
     }
-    else if (sN > sD)
-    {
+    else if (sN > sD) {
       sN = sD;
       tN = e + b;
       tD = c;
     }
   }
 
-  if (tN < 0.f)
-  {
+  if (tN < 0.f) {
     tN = 0.f;
 
     if (-d < 0.f)
@@ -542,15 +543,13 @@ pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scal
     else if (-d > a)
       sN = sD;
 
-    else
-    {
+    else {
       sN = -d;
       sD = a;
     }
   }
 
-  else if (tN > tD)
-  {
+  else if (tN > tD) {
     tN = tD;
 
     if ((-d + b) < 0.f)
@@ -559,104 +558,106 @@ pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scal
     else if ((-d + b) > a)
       sN = sD;
 
-    else
-    {
+    else {
       sN = (-d + b);
       sD = a;
     }
   }
 
   // set intersection ratios
-  ratio[0] = (std::abs (sN) < small_error_) ? 0.f : sN / sD;
-  ratio[1] = (std::abs (tN) < small_error_) ? 0.f : tN / tD;
+  ratio[0] = (std::abs(sN) < small_error_) ? 0.f : sN / sD;
+  ratio[1] = (std::abs(tN) < small_error_) ? 0.f : tN / tD;
 
   Eigen::Vector3f x = w + (ratio[0] * u) - (ratio[1] * v);
-  return (x.norm ());
+  return (x.norm());
 }
 
-
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
-pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::bruteForceCorrespondences (
-  int idx1,
-  int idx2,
-  pcl::Correspondences &pairs)
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+int
+pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
+    bruteForceCorrespondences(int idx1, int idx2, pcl::Correspondences& pairs)
 {
   const float max_norm_diff = 0.5f * max_norm_diff_ * M_PI / 180.f;
 
   // calculate reference segment distance and normal angle
-  float ref_dist = pcl::euclideanDistance ((*target_)[idx1], (*target_)[idx2]);
-  float ref_norm_angle = (use_normals_ ? ((*target_normals_)[idx1].getNormalVector3fMap () -
-                                          (*target_normals_)[idx2].getNormalVector3fMap ()).norm () : 0.f);
+  float ref_dist = pcl::euclideanDistance((*target_)[idx1], (*target_)[idx2]);
+  float ref_norm_angle =
+      (use_normals_ ? ((*target_normals_)[idx1].getNormalVector3fMap() -
+                       (*target_normals_)[idx2].getNormalVector3fMap())
+                          .norm()
+                    : 0.f);
 
   // loop over all pairs of points in source point cloud
-  auto it_out = source_indices_->begin (), it_out_e = source_indices_->end () - 1;
-  auto it_in_e = source_indices_->end ();
-  for ( ; it_out != it_out_e; it_out++)
-  {
+  auto it_out = source_indices_->begin(), it_out_e = source_indices_->end() - 1;
+  auto it_in_e = source_indices_->end();
+  for (; it_out != it_out_e; it_out++) {
     auto it_in = it_out + 1;
-    const PointSource *pt1 = &(*input_)[*it_out];
-    for ( ; it_in != it_in_e; it_in++)
-    {
-      const PointSource *pt2 = &(*input_)[*it_in];
+    const PointSource* pt1 = &(*input_)[*it_out];
+    for (; it_in != it_in_e; it_in++) {
+      const PointSource* pt2 = &(*input_)[*it_in];
 
       // check point distance compared to reference dist (from base)
-      float dist = pcl::euclideanDistance (*pt1, *pt2);
-      if (std::abs(dist - ref_dist) < max_pair_diff_)
-      {
+      float dist = pcl::euclideanDistance(*pt1, *pt2);
+      if (std::abs(dist - ref_dist) < max_pair_diff_) {
         // add here normal evaluation if normals are given
-        if (use_normals_)
-        {
-          const NormalT *pt1_n = &((*source_normals_)[*it_out]);
-          const NormalT *pt2_n = &((*source_normals_)[*it_in]);
+        if (use_normals_) {
+          const NormalT* pt1_n = &((*source_normals_)[*it_out]);
+          const NormalT* pt2_n = &((*source_normals_)[*it_in]);
 
-          float norm_angle_1 = (pt1_n->getNormalVector3fMap () - pt2_n->getNormalVector3fMap ()).norm ();
-          float norm_angle_2 = (pt1_n->getNormalVector3fMap () + pt2_n->getNormalVector3fMap ()).norm ();
+          float norm_angle_1 =
+              (pt1_n->getNormalVector3fMap() - pt2_n->getNormalVector3fMap()).norm();
+          float norm_angle_2 =
+              (pt1_n->getNormalVector3fMap() + pt2_n->getNormalVector3fMap()).norm();
 
-          float norm_diff = std::min <float> (std::abs (norm_angle_1 - ref_norm_angle), std::abs (norm_angle_2 - ref_norm_angle));
+          float norm_diff = std::min<float>(std::abs(norm_angle_1 - ref_norm_angle),
+                                            std::abs(norm_angle_2 - ref_norm_angle));
           if (norm_diff > max_norm_diff)
             continue;
         }
 
-        pairs.push_back (pcl::Correspondence (*it_in, *it_out, dist));
-        pairs.push_back (pcl::Correspondence (*it_out, *it_in, dist));
+        pairs.push_back(pcl::Correspondence(*it_in, *it_out, dist));
+        pairs.push_back(pcl::Correspondence(*it_out, *it_in, dist));
       }
     }
   }
 
   // return success if at least one correspondence was found
-  return (pairs.empty () ? -1 : 0);
+  return (pairs.empty() ? -1 : 0);
 }
 
-
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
-pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::determineBaseMatches (
-  const std::vector <int> &base_indices,
-  std::vector <std::vector <int> > &matches,
-  const pcl::Correspondences &pairs_a,
-  const pcl::Correspondences &pairs_b,
-  const float (&ratio)[2])
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+int
+pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
+    determineBaseMatches(const pcl::Indices& base_indices,
+                         std::vector<pcl::Indices>& matches,
+                         const pcl::Correspondences& pairs_a,
+                         const pcl::Correspondences& pairs_b,
+                         const float (&ratio)[2])
 {
   // calculate edge lengths of base
   float dist_base[4];
-  dist_base[0] = pcl::euclideanDistance ((*target_)[base_indices[0]], (*target_)[base_indices[2]]);
-  dist_base[1] = pcl::euclideanDistance ((*target_)[base_indices[0]], (*target_)[base_indices[3]]);
-  dist_base[2] = pcl::euclideanDistance ((*target_)[base_indices[1]], (*target_)[base_indices[2]]);
-  dist_base[3] = pcl::euclideanDistance ((*target_)[base_indices[1]], (*target_)[base_indices[3]]);
-
-  // loop over first point pair correspondences and store intermediate points 'e' in new point cloud
-  PointCloudSourcePtr cloud_e (new PointCloudSource);
-  cloud_e->resize (pairs_a.size () * 2);
-  PointCloudSourceIterator it_pt = cloud_e->begin ();
-  for (const auto &pair : pairs_a)
-  {
-    const PointSource *pt1 = &((*input_)[pair.index_match]);
-    const PointSource *pt2 = &((*input_)[pair.index_query]);
+  dist_base[0] =
+      pcl::euclideanDistance((*target_)[base_indices[0]], (*target_)[base_indices[2]]);
+  dist_base[1] =
+      pcl::euclideanDistance((*target_)[base_indices[0]], (*target_)[base_indices[3]]);
+  dist_base[2] =
+      pcl::euclideanDistance((*target_)[base_indices[1]], (*target_)[base_indices[2]]);
+  dist_base[3] =
+      pcl::euclideanDistance((*target_)[base_indices[1]], (*target_)[base_indices[3]]);
+
+  // loop over first point pair correspondences and store intermediate points 'e' in new
+  // point cloud
+  PointCloudSourcePtr cloud_e(new PointCloudSource);
+  cloud_e->resize(pairs_a.size() * 2);
+  PointCloudSourceIterator it_pt = cloud_e->begin();
+  for (const auto& pair : pairs_a) {
+    const PointSource* pt1 = &((*input_)[pair.index_match]);
+    const PointSource* pt2 = &((*input_)[pair.index_query]);
 
     // calculate intermediate points using both ratios from base (r1,r2)
-    for (int i = 0; i < 2; i++, it_pt++)
-    {
+    for (int i = 0; i < 2; i++, it_pt++) {
       it_pt->x = pt1->x + ratio[i] * (pt2->x - pt1->x);
       it_pt->y = pt1->y + ratio[i] * (pt2->y - pt1->y);
       it_pt->z = pt1->z + ratio[i] * (pt2->z - pt1->z);
@@ -664,115 +665,123 @@ pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scal
   }
 
   // initialize new kd tree of intermediate points from first point pair correspondences
-  KdTreeReciprocalPtr tree_e (new KdTreeReciprocal);
-  tree_e->setInputCloud (cloud_e);
+  KdTreeReciprocalPtr tree_e(new KdTreeReciprocal);
+  tree_e->setInputCloud(cloud_e);
 
-  std::vector <int> ids;
-  std::vector <float> dists_sqr;
+  pcl::Indices ids;
+  std::vector<float> dists_sqr;
 
   // loop over second point pair correspondences
-  for (const auto &pair : pairs_b)
-  {
-    const PointTarget *pt1 = &((*input_)[pair.index_match]);
-    const PointTarget *pt2 = &((*input_)[pair.index_query]);
+  for (const auto& pair : pairs_b) {
+    const PointTarget* pt1 = &((*input_)[pair.index_match]);
+    const PointTarget* pt2 = &((*input_)[pair.index_query]);
 
     // calculate intermediate points using both ratios from base (r1,r2)
-    for (const float &r : ratio)
-    {
+    for (const float& r : ratio) {
       PointTarget pt_e;
       pt_e.x = pt1->x + r * (pt2->x - pt1->x);
       pt_e.y = pt1->y + r * (pt2->y - pt1->y);
       pt_e.z = pt1->z + r * (pt2->z - pt1->z);
 
       // search for corresponding intermediate points
-      tree_e->radiusSearch (pt_e, coincidation_limit_, ids, dists_sqr);
-      for (const int &id : ids)
-      {
-        std::vector <int> match_indices (4);
-
-        match_indices[0] = pairs_a[static_cast <int> (std::floor ((float)(id/2.f)))].index_match;
-        match_indices[1] = pairs_a[static_cast <int> (std::floor ((float)(id/2.f)))].index_query;
+      tree_e->radiusSearch(pt_e, coincidation_limit_, ids, dists_sqr);
+      for (const auto& id : ids) {
+        pcl::Indices match_indices(4);
+
+        match_indices[0] =
+            pairs_a[static_cast<int>(std::floor((float)(id / 2.f)))].index_match;
+        match_indices[1] =
+            pairs_a[static_cast<int>(std::floor((float)(id / 2.f)))].index_query;
         match_indices[2] = pair.index_match;
         match_indices[3] = pair.index_query;
 
         // EDITED: added coarse check of match based on edge length (due to rigid-body )
-        if (checkBaseMatch (match_indices, dist_base) < 0)
+        if (checkBaseMatch(match_indices, dist_base) < 0)
           continue;
 
-        matches.push_back (match_indices);
+        matches.push_back(match_indices);
       }
     }
   }
 
   // return unsuccessful if no match was found
-  return (!matches.empty () ? 0 : -1);
+  return (!matches.empty() ? 0 : -1);
 }
 
-
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
-pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::checkBaseMatch (
-  const std::vector <int> &match_indices,
-  const float (&dist_ref)[4])
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+int
+pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
+    checkBaseMatch(const pcl::Indices& match_indices, const float (&dist_ref)[4])
 {
-  float d0 = pcl::euclideanDistance ((*input_)[match_indices[0]], (*input_)[match_indices[2]]);
-  float d1 = pcl::euclideanDistance ((*input_)[match_indices[0]], (*input_)[match_indices[3]]);
-  float d2 = pcl::euclideanDistance ((*input_)[match_indices[1]], (*input_)[match_indices[2]]);
-  float d3 = pcl::euclideanDistance ((*input_)[match_indices[1]], (*input_)[match_indices[3]]);
+  float d0 =
+      pcl::euclideanDistance((*input_)[match_indices[0]], (*input_)[match_indices[2]]);
+  float d1 =
+      pcl::euclideanDistance((*input_)[match_indices[0]], (*input_)[match_indices[3]]);
+  float d2 =
+      pcl::euclideanDistance((*input_)[match_indices[1]], (*input_)[match_indices[2]]);
+  float d3 =
+      pcl::euclideanDistance((*input_)[match_indices[1]], (*input_)[match_indices[3]]);
 
   // check edge distances of match w.r.t the base
-  return (std::abs (d0 - dist_ref[0]) < max_edge_diff_ && std::abs (d1 - dist_ref[1]) < max_edge_diff_ &&
-          std::abs (d2 - dist_ref[2]) < max_edge_diff_ && std::abs (d3 - dist_ref[3]) < max_edge_diff_) ? 0 : -1;
+  return (std::abs(d0 - dist_ref[0]) < max_edge_diff_ &&
+          std::abs(d1 - dist_ref[1]) < max_edge_diff_ &&
+          std::abs(d2 - dist_ref[2]) < max_edge_diff_ &&
+          std::abs(d3 - dist_ref[3]) < max_edge_diff_)
+             ? 0
+             : -1;
 }
 
-
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
-pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::handleMatches (
-  const std::vector <int> &base_indices,
-  std::vector <std::vector <int> > &matches,
-  MatchingCandidates &candidates)
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+void
+pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
+    handleMatches(const pcl::Indices& base_indices,
+                  std::vector<pcl::Indices>& matches,
+                  MatchingCandidates& candidates)
 {
-  candidates.resize (1);
+  candidates.resize(1);
   float fitness_score = FLT_MAX;
 
   // loop over all Candidate matches
-  for (auto &match : matches)
-  {
+  for (auto& match : matches) {
     Eigen::Matrix4f transformation_temp;
     pcl::Correspondences correspondences_temp;
 
-    // determine corresondences between base and match according to their distance to centroid
-    linkMatchWithBase (base_indices, match, correspondences_temp);
+    // determine corresondences between base and match according to their distance to
+    // centroid
+    linkMatchWithBase(base_indices, match, correspondences_temp);
 
     // check match based on residuals of the corresponding points after
-    if (validateMatch (base_indices, match, correspondences_temp, transformation_temp) < 0)
+    if (validateMatch(base_indices, match, correspondences_temp, transformation_temp) <
+        0)
       continue;
 
-    // check resulting  using a sub sample of the source point cloud and compare to previous matches
-    if (validateTransformation (transformation_temp, fitness_score) < 0)
+    // check resulting  using a sub sample of the source point cloud and compare to
+    // previous matches
+    if (validateTransformation(transformation_temp, fitness_score) < 0)
       continue;
 
     // store best match as well as associated fitness_score and transformation
     candidates[0].fitness_score = fitness_score;
-    candidates [0].transformation = transformation_temp;
-    correspondences_temp.erase (correspondences_temp.end () - 1);
+    candidates[0].transformation = transformation_temp;
+    correspondences_temp.erase(correspondences_temp.end() - 1);
     candidates[0].correspondences = correspondences_temp;
   }
 }
 
-
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
-pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::linkMatchWithBase (
-  const std::vector <int> &base_indices,
-  std::vector <int> &match_indices,
-  pcl::Correspondences &correspondences)
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+void
+pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
+    linkMatchWithBase(const pcl::Indices& base_indices,
+                      pcl::Indices& match_indices,
+                      pcl::Correspondences& correspondences)
 {
   // calculate centroid of base and target
-  Eigen::Vector4f centre_base {0, 0, 0, 0}, centre_match {0, 0, 0, 0};
-  pcl::compute3DCentroid (*target_, base_indices, centre_base);
-  pcl::compute3DCentroid (*input_, match_indices, centre_match);
+  Eigen::Vector4f centre_base{0, 0, 0, 0}, centre_match{0, 0, 0, 0};
+  pcl::compute3DCentroid(*target_, base_indices, centre_base);
+  pcl::compute3DCentroid(*input_, match_indices, centre_match);
 
   PointTarget centre_pt_base;
   centre_pt_base.x = centre_base[0];
@@ -785,87 +794,91 @@ pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scal
   centre_pt_match.z = centre_match[2];
 
   // find corresponding points according to their distance to the centroid
-  std::vector <int> copy = match_indices;
-
-  auto it_match_orig = match_indices.begin ();
-  for (auto it_base = base_indices.cbegin (), it_base_e = base_indices.cend (); it_base != it_base_e; it_base++, it_match_orig++)
-  {
-    float dist_sqr_1 = pcl::squaredEuclideanDistance ((*target_)[*it_base], centre_pt_base);
+  pcl::Indices copy = match_indices;
+
+  auto it_match_orig = match_indices.begin();
+  for (auto it_base = base_indices.cbegin(), it_base_e = base_indices.cend();
+       it_base != it_base_e;
+       it_base++, it_match_orig++) {
+    float dist_sqr_1 =
+        pcl::squaredEuclideanDistance((*target_)[*it_base], centre_pt_base);
     float best_diff_sqr = FLT_MAX;
     int best_index = -1;
 
-    for (const int &match_index : copy)
-    {
+    for (const auto& match_index : copy) {
       // calculate difference of distances to centre point
-      float dist_sqr_2 = pcl::squaredEuclideanDistance ((*input_)[match_index], centre_pt_match);
+      float dist_sqr_2 =
+          pcl::squaredEuclideanDistance((*input_)[match_index], centre_pt_match);
       float diff_sqr = std::abs(dist_sqr_1 - dist_sqr_2);
 
-      if (diff_sqr < best_diff_sqr)
-      {
+      if (diff_sqr < best_diff_sqr) {
         best_diff_sqr = diff_sqr;
         best_index = match_index;
       }
     }
 
     // assign new correspondence and update indices of matched targets
-    correspondences.push_back (pcl::Correspondence (best_index, *it_base, best_diff_sqr));
+    correspondences.push_back(pcl::Correspondence(best_index, *it_base, best_diff_sqr));
     *it_match_orig = best_index;
   }
 }
 
-
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
-pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::validateMatch (
-  const std::vector <int> &base_indices,
-  const std::vector <int> &match_indices,
-  const pcl::Correspondences &correspondences,
-  Eigen::Matrix4f &transformation)
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+int
+pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
+    validateMatch(const pcl::Indices& base_indices,
+                  const pcl::Indices& match_indices,
+                  const pcl::Correspondences& correspondences,
+                  Eigen::Matrix4f& transformation)
 {
   // only use triplet of points to simlify process (possible due to planar case)
   pcl::Correspondences correspondences_temp = correspondences;
-  correspondences_temp.erase (correspondences_temp.end () - 1);
+  correspondences_temp.erase(correspondences_temp.end() - 1);
 
   // estimate transformation between correspondence set
-  transformation_estimation_->estimateRigidTransformation (*input_, *target_, correspondences_temp, transformation);
+  transformation_estimation_->estimateRigidTransformation(
+      *input_, *target_, correspondences_temp, transformation);
 
   // transform base points
   PointCloudSource match_transformed;
-  pcl::transformPointCloud (*input_, match_indices, match_transformed, transformation);
+  pcl::transformPointCloud(*input_, match_indices, match_transformed, transformation);
 
   // calculate residuals of transformation and check against maximum threshold
-  std::size_t nr_points = correspondences_temp.size ();
+  std::size_t nr_points = correspondences_temp.size();
   float mse = 0.f;
   for (std::size_t i = 0; i < nr_points; i++)
-    mse += pcl::squaredEuclideanDistance (match_transformed.points [i], target_->points [base_indices[i]]);
+    mse += pcl::squaredEuclideanDistance(match_transformed.points[i],
+                                         target_->points[base_indices[i]]);
 
   mse /= nr_points;
   return (mse < max_mse_ ? 0 : -1);
 }
 
-
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
-pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::validateTransformation (
-  Eigen::Matrix4f &transformation,
-  float &fitness_score)
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+int
+pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
+    validateTransformation(Eigen::Matrix4f& transformation, float& fitness_score)
 {
   // transform source point cloud
   PointCloudSource source_transformed;
-  pcl::transformPointCloud (*input_, *source_indices_, source_transformed, transformation);
+  pcl::transformPointCloud(
+      *input_, *source_indices_, source_transformed, transformation);
 
-  std::size_t nr_points = source_transformed.size ();
-  std::size_t terminate_value = fitness_score > 1 ? 0 : static_cast <std::size_t> ((1.f - fitness_score) * nr_points);
+  std::size_t nr_points = source_transformed.size();
+  std::size_t terminate_value =
+      fitness_score > 1 ? 0
+                        : static_cast<std::size_t>((1.f - fitness_score) * nr_points);
 
   float inlier_score_temp = 0;
-  std::vector <int> ids;
-  std::vector <float> dists_sqr;
-  PointCloudSourceIterator it = source_transformed.begin ();
+  pcl::Indices ids;
+  std::vector<float> dists_sqr;
+  PointCloudSourceIterator it = source_transformed.begin();
 
-  for (std::size_t i = 0; i < nr_points; it++, i++)
-  {
+  for (std::size_t i = 0; i < nr_points; it++, i++) {
     // search for nearest point using kd tree search
-    tree_->nearestKSearch (*it, 1, ids, dists_sqr);
+    tree_->nearestKSearch(*it, 1, ids, dists_sqr);
     inlier_score_temp += (dists_sqr[0] < max_inlier_dist_sqr_ ? 1 : 0);
 
     // early terminating
@@ -874,7 +887,7 @@ pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scal
   }
 
   // check current costs and return unsuccessful if larger than previous ones
-  inlier_score_temp /= static_cast <float> (nr_points);
+  inlier_score_temp /= static_cast<float>(nr_points);
   float fitness_score_temp = 1.f - inlier_score_temp;
 
   if (fitness_score_temp > fitness_score)
@@ -884,32 +897,29 @@ pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scal
   return (0);
 }
 
-
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
-pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::finalCompute (
-  const std::vector <MatchingCandidates > &candidates)
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+void
+pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
+    finalCompute(const std::vector<MatchingCandidates>& candidates)
 {
   // get best fitness_score over all tries
-  int nr_candidates = static_cast <int> (candidates.size ());
+  int nr_candidates = static_cast<int>(candidates.size());
   int best_index = -1;
   float best_score = FLT_MAX;
-  for (int i = 0; i < nr_candidates; i++)
-  {
-    const float &fitness_score = candidates [i][0].fitness_score;
-    if (fitness_score < best_score)
-    {
+  for (int i = 0; i < nr_candidates; i++) {
+    const float& fitness_score = candidates[i][0].fitness_score;
+    if (fitness_score < best_score) {
       best_score = fitness_score;
       best_index = i;
     }
   }
 
   // check if a valid candidate was available
-  if (!(best_index < 0))
-  {
-    fitness_score_ = candidates [best_index][0].fitness_score;
-    final_transformation_ = candidates [best_index][0].transformation;
-    *correspondences_ = candidates [best_index][0].correspondences;
+  if (!(best_index < 0)) {
+    fitness_score_ = candidates[best_index][0].fitness_score;
+    final_transformation_ = candidates[best_index][0].transformation;
+    *correspondences_ = candidates[best_index][0].correspondences;
 
     // here we define convergence if resulting fitness_score is below 1-threshold
     converged_ = fitness_score_ < score_threshold_;
diff --git a/registration/include/pcl/registration/impl/ia_kfpcs.hpp b/registration/include/pcl/registration/impl/ia_kfpcs.hpp
index 42c86e25..9358ced8 100644
--- a/registration/include/pcl/registration/impl/ia_kfpcs.hpp
+++ b/registration/include/pcl/registration/impl/ia_kfpcs.hpp
@@ -19,7 +19,7 @@
  *     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
+ *  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
@@ -37,44 +37,49 @@
 #ifndef PCL_REGISTRATION_IMPL_IA_KFPCS_H_
 #define PCL_REGISTRATION_IMPL_IA_KFPCS_H_
 
+namespace pcl {
 
-namespace pcl
-{
-
-namespace registration
-{
+namespace registration {
 
 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
-KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::KFPCSInitialAlignment () :
-  lower_trl_boundary_ (-1.f),
-  upper_trl_boundary_ (-1.f),
-  lambda_ (0.5f),
-  use_trl_score_ (false),
-  indices_validation_ (new std::vector <int>)
+KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
+    KFPCSInitialAlignment()
+: lower_trl_boundary_(-1.f)
+, upper_trl_boundary_(-1.f)
+, lambda_(0.5f)
+, use_trl_score_(false)
+, indices_validation_(new pcl::Indices)
 {
   reg_name_ = "pcl::registration::KFPCSInitialAlignment";
 }
 
-
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> bool
-KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::initCompute ()
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+bool
+KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::initCompute()
 {
   // due to sparse keypoint cloud, do not normalize delta with estimated point density
-  if (normalize_delta_)
-  {
-    PCL_WARN ("[%s::initCompute] Delta should be set according to keypoint precision! Normalization according to point cloud density is ignored.", reg_name_.c_str ());
+  if (normalize_delta_) {
+    PCL_WARN("[%s::initCompute] Delta should be set according to keypoint precision! "
+             "Normalization according to point cloud density is ignored.\n",
+             reg_name_.c_str());
     normalize_delta_ = false;
   }
 
   // initialize as in fpcs
-  pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::initCompute ();
+  pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
+      initCompute();
 
   // set the threshold values with respect to keypoint charactersitics
-  max_pair_diff_ = delta_ * 1.414f; // diff between 2 points of delta_ accuracy
+  max_pair_diff_ = delta_ * 1.414f;      // diff between 2 points of delta_ accuracy
   coincidation_limit_ = delta_ * 2.828f; // diff between diff of 2 points
-  max_edge_diff_ = delta_ * 3.f; // diff between 2 points + some inaccuracy due to quadruple orientation
-  max_mse_ = powf (delta_ * 4.f, 2.f); // diff between 2 points + some registration inaccuracy
-  max_inlier_dist_sqr_ = powf (delta_ * 8.f, 2.f); // set rel. high, because MSAC is used (residual based score function)
+  max_edge_diff_ =
+      delta_ *
+      3.f; // diff between 2 points + some inaccuracy due to quadruple orientation
+  max_mse_ =
+      powf(delta_ * 4.f, 2.f); // diff between 2 points + some registration inaccuracy
+  max_inlier_dist_sqr_ =
+      powf(delta_ * 8.f,
+           2.f); // set rel. high, because MSAC is used (residual based score function)
 
   // check use of translation costs and calculate upper boundary if not set by user
   if (upper_trl_boundary_ < 0)
@@ -85,83 +90,95 @@ KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::initCompute (
   else
     lambda_ = 0.f;
 
-  // generate a subset of indices of size ransac_iterations_ on which to evaluate candidates on
-  std::size_t nr_indices = indices_->size ();
-  if (nr_indices < std::size_t (ransac_iterations_))
+  // generate a subset of indices of size ransac_iterations_ on which to evaluate
+  // candidates on
+  std::size_t nr_indices = indices_->size();
+  if (nr_indices < std::size_t(ransac_iterations_))
     indices_validation_ = indices_;
   else
     for (int i = 0; i < ransac_iterations_; i++)
-      indices_validation_->push_back ((*indices_)[rand () % nr_indices]);
+      indices_validation_->push_back((*indices_)[rand() % nr_indices]);
 
   return (true);
 }
 
-
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
-KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::handleMatches (
-  const std::vector <int> &base_indices,
-  std::vector <std::vector <int> > &matches,
-  MatchingCandidates &candidates)
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+void
+KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::handleMatches(
+    const pcl::Indices& base_indices,
+    std::vector<pcl::Indices>& matches,
+    MatchingCandidates& candidates)
 {
-  candidates.clear ();
+  candidates.clear();
 
   // loop over all Candidate matches
-  for (auto &match : matches)
-  {
+  for (auto& match : matches) {
     Eigen::Matrix4f transformation_temp;
     pcl::Correspondences correspondences_temp;
-    float 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);
+    // determine corresondences between base and match according to their distance to
+    // centroid
+    linkMatchWithBase(base_indices, match, correspondences_temp);
 
     // check match based on residuals of the corresponding points after transformation
-    if (validateMatch (base_indices, match, correspondences_temp, transformation_temp) < 0)
+    if (validateMatch(base_indices, match, correspondences_temp, transformation_temp) <
+        0)
       continue;
 
     // check resulting transformation using a sub sample of the source point cloud
     // all candidates are stored and later sorted according to their fitness score
-    validateTransformation (transformation_temp, fitness_score);
+    validateTransformation(transformation_temp, fitness_score);
 
     // store all valid match as well as associated score and transformation
-    candidates.push_back (MatchingCandidate (fitness_score, correspondences_temp, transformation_temp));
+    candidates.push_back(
+        MatchingCandidate(fitness_score, correspondences_temp, transformation_temp));
   }
 }
 
-
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
-KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::validateTransformation (
-  Eigen::Matrix4f &transformation,
-  float &fitness_score)
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+int
+KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
+    validateTransformation(Eigen::Matrix4f& transformation, float& fitness_score)
 {
   // transform sub sampled source cloud
   PointCloudSource source_transformed;
-  pcl::transformPointCloud (*input_, *indices_validation_, source_transformed, transformation);
+  pcl::transformPointCloud(
+      *input_, *indices_validation_, source_transformed, transformation);
 
-  const std::size_t nr_points = source_transformed.size ();
+  const std::size_t nr_points = source_transformed.size();
   float score_a = 0.f, score_b = 0.f;
 
   // residual costs based on mse
-  std::vector <int> ids;
-  std::vector <float> dists_sqr;
-  for (PointCloudSourceIterator it = source_transformed.begin (), it_e = source_transformed.end (); it != it_e; ++it)
-  {
+  pcl::Indices ids;
+  std::vector<float> dists_sqr;
+  for (PointCloudSourceIterator it = source_transformed.begin(),
+                                it_e = source_transformed.end();
+       it != it_e;
+       ++it) {
     // search for nearest point using kd tree search
-    tree_->nearestKSearch (*it, 1, ids, dists_sqr);
-    score_a += (dists_sqr[0] < max_inlier_dist_sqr_ ? dists_sqr[0] : max_inlier_dist_sqr_); // MSAC
+    tree_->nearestKSearch(*it, 1, ids, dists_sqr);
+    score_a += (dists_sqr[0] < max_inlier_dist_sqr_ ? dists_sqr[0]
+                                                    : max_inlier_dist_sqr_); // MSAC
   }
 
   score_a /= (max_inlier_dist_sqr_ * nr_points); // MSAC
-  //score_a = 1.f - (1.f - score_a) / (1.f - approx_overlap_); // make score relative to estimated overlap
+  // score_a = 1.f - (1.f - score_a) / (1.f - approx_overlap_); // make score relative
+  // to estimated overlap
 
   // translation score (solutions with small translation are down-voted)
   float scale = 1.f;
-  if (use_trl_score_)
-  {
-    float trl = transformation.rightCols <1> ().head (3).norm ();
-    float trl_ratio = (trl - lower_trl_boundary_) / (upper_trl_boundary_ - lower_trl_boundary_);
-
-    score_b = (trl_ratio < 0.f ? 1.f : (trl_ratio > 1.f ? 0.f : 0.5f * sin (M_PI * trl_ratio + M_PI_2) + 0.5f)); // sinusoidal costs
+  if (use_trl_score_) {
+    float trl = transformation.rightCols<1>().head(3).norm();
+    float trl_ratio =
+        (trl - lower_trl_boundary_) / (upper_trl_boundary_ - lower_trl_boundary_);
+
+    score_b =
+        (trl_ratio < 0.f ? 1.f
+                         : (trl_ratio > 1.f ? 0.f
+                                            : 0.5f * sin(M_PI * trl_ratio + M_PI_2) +
+                                                  0.5f)); // sinusoidal costs
     scale += lambda_;
   }
 
@@ -174,114 +191,114 @@ KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::validateTrans
   return (0);
 }
 
-
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
-KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::finalCompute (
-  const std::vector <MatchingCandidates > &candidates)
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+void
+KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::finalCompute(
+    const std::vector<MatchingCandidates>& candidates)
 {
   // reorganize candidates into single vector
   std::size_t total_size = 0;
-  for (const auto &candidate : candidates)
-    total_size += candidate.size ();
+  for (const auto& candidate : candidates)
+    total_size += candidate.size();
 
-  candidates_.clear ();
-  candidates_.reserve (total_size);
+  candidates_.clear();
+  candidates_.reserve(total_size);
 
-  for (const auto &candidate : candidates)
-    for (const auto &match : candidate)
-      candidates_.push_back (match);
+  for (const auto& candidate : candidates)
+    for (const auto& match : candidate)
+      candidates_.push_back(match);
 
   // sort according to score value
-  std::sort (candidates_.begin (), candidates_.end (), by_score ());
+  std::sort(candidates_.begin(), candidates_.end(), by_score());
 
   // return here if no score was valid, i.e. all scores are FLT_MAX
-  if (candidates_[0].fitness_score == FLT_MAX)
-  {
+  if (candidates_[0].fitness_score == FLT_MAX) {
     converged_ = false;
     return;
   }
 
   // save best candidate as output result
-  // note, all other candidates are accessible via getNBestCandidates () and getTBestCandidates ()
-  fitness_score_ = candidates_ [0].fitness_score;
-  final_transformation_ = candidates_ [0].transformation;
-  *correspondences_ = candidates_ [0].correspondences;
+  // note, all other candidates are accessible via getNBestCandidates () and
+  // getTBestCandidates ()
+  fitness_score_ = candidates_[0].fitness_score;
+  final_transformation_ = candidates_[0].transformation;
+  *correspondences_ = candidates_[0].correspondences;
 
   // here we define convergence if resulting score is above threshold
   converged_ = fitness_score_ < score_threshold_;
 }
 
-
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
-KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::getNBestCandidates (
-  int n,
-  float min_angle3d,
-  float min_translation3d,
-  MatchingCandidates &candidates)
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+void
+KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::getNBestCandidates(
+    int n, float min_angle3d, float min_translation3d, MatchingCandidates& candidates)
 {
-  candidates.clear ();
+  candidates.clear();
 
   // loop over all candidates starting from the best one
-  for (MatchingCandidates::iterator it_candidate = candidates_.begin (), it_e = candidates_.end (); it_candidate != it_e; ++it_candidate)
-  {
+  for (MatchingCandidates::iterator it_candidate = candidates_.begin(),
+                                    it_e = candidates_.end();
+       it_candidate != it_e;
+       ++it_candidate) {
     // stop if current candidate has no valid score
     if (it_candidate->fitness_score == FLT_MAX)
       return;
 
-    // check if current candidate is a unique one compared to previous using the min_diff threshold
+    // check if current candidate is a unique one compared to previous using the
+    // min_diff threshold
     bool unique = true;
-    MatchingCandidates::iterator it = candidates.begin (), it_e2 = candidates.end ();
-    while (unique && it != it_e2)
-    {
-      Eigen::Matrix4f diff = it_candidate->transformation.colPivHouseholderQr ().solve (it->transformation);
-      const float angle3d = Eigen::AngleAxisf (diff.block <3, 3> (0, 0)).angle ();
-      const float translation3d = diff.block <3, 1> (0, 3).norm ();
+    MatchingCandidates::iterator it = candidates.begin(), it_e2 = candidates.end();
+    while (unique && it != it_e2) {
+      Eigen::Matrix4f diff =
+          it_candidate->transformation.colPivHouseholderQr().solve(it->transformation);
+      const float angle3d = Eigen::AngleAxisf(diff.block<3, 3>(0, 0)).angle();
+      const float translation3d = diff.block<3, 1>(0, 3).norm();
       unique = angle3d > min_angle3d && translation3d > min_translation3d;
       ++it;
     }
 
     // add candidate to best candidates
     if (unique)
-      candidates.push_back (*it_candidate);
+      candidates.push_back(*it_candidate);
 
     // stop if n candidates are reached
-    if (candidates.size () == n)
+    if (candidates.size() == n)
       return;
   }
 }
 
-
-template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
-KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::getTBestCandidates (
-  float t,
-  float min_angle3d,
-  float min_translation3d,
-  MatchingCandidates &candidates)
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+void
+KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::getTBestCandidates(
+    float t, float min_angle3d, float min_translation3d, MatchingCandidates& candidates)
 {
-  candidates.clear ();
+  candidates.clear();
 
   // loop over all candidates starting from the best one
-  for (MatchingCandidates::iterator it_candidate = candidates_.begin (), it_e = candidates_.end (); it_candidate != it_e; ++it_candidate)
-  {
+  for (MatchingCandidates::iterator it_candidate = candidates_.begin(),
+                                    it_e = candidates_.end();
+       it_candidate != it_e;
+       ++it_candidate) {
     // stop if current candidate has score below threshold
     if (it_candidate->fitness_score > t)
       return;
 
-    // check if current candidate is a unique one compared to previous using the min_diff threshold
+    // check if current candidate is a unique one compared to previous using the
+    // min_diff threshold
     bool unique = true;
-    MatchingCandidates::iterator it = candidates.begin (), it_e2 = candidates.end ();
-    while (unique && it != it_e2)
-    {
-      Eigen::Matrix4f diff = it_candidate->transformation.colPivHouseholderQr ().solve (it->transformation);
-      const float angle3d = Eigen::AngleAxisf (diff.block <3, 3> (0, 0)).angle ();
-      const float translation3d = diff.block <3, 1> (0, 3).norm ();
+    MatchingCandidates::iterator it = candidates.begin(), it_e2 = candidates.end();
+    while (unique && it != it_e2) {
+      Eigen::Matrix4f diff =
+          it_candidate->transformation.colPivHouseholderQr().solve(it->transformation);
+      const float angle3d = Eigen::AngleAxisf(diff.block<3, 3>(0, 0)).angle();
+      const float translation3d = diff.block<3, 1>(0, 3).norm();
       unique = angle3d > min_angle3d && translation3d > min_translation3d;
       ++it;
     }
 
     // add candidate to best candidates
     if (unique)
-      candidates.push_back (*it_candidate);
+      candidates.push_back(*it_candidate);
   }
 }
 
@@ -289,4 +306,3 @@ KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::getTBestCandi
 } // namespace pcl
 
 #endif // PCL_REGISTRATION_IMPL_IA_KFPCS_H_
-
diff --git a/registration/include/pcl/registration/impl/ia_ransac.hpp b/registration/include/pcl/registration/impl/ia_ransac.hpp
index b6bd0de1..77f3cdc5 100644
--- a/registration/include/pcl/registration/impl/ia_ransac.hpp
+++ b/registration/include/pcl/registration/impl/ia_ransac.hpp
@@ -43,44 +43,48 @@
 
 #include <pcl/common/distances.h>
 
+namespace pcl {
 
-namespace pcl
+template <typename PointSource, typename PointTarget, typename FeatureT>
+void
+SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::setSourceFeatures(
+    const FeatureCloudConstPtr& features)
 {
-
-template <typename PointSource, typename PointTarget, typename FeatureT> void
-SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::setSourceFeatures (const FeatureCloudConstPtr &features)
-{
-  if (features == nullptr || features->empty ())
-  {
-    PCL_ERROR ("[pcl::%s::setSourceFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
+  if (features == nullptr || features->empty()) {
+    PCL_ERROR(
+        "[pcl::%s::setSourceFeatures] Invalid or empty point cloud dataset given!\n",
+        getClassName().c_str());
     return;
   }
   input_features_ = features;
 }
 
-
-template <typename PointSource, typename PointTarget, typename FeatureT> void
-SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::setTargetFeatures (const FeatureCloudConstPtr &features)
+template <typename PointSource, typename PointTarget, typename FeatureT>
+void
+SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::setTargetFeatures(
+    const FeatureCloudConstPtr& features)
 {
-  if (features == nullptr || features->empty ())
-  {
-    PCL_ERROR ("[pcl::%s::setTargetFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
+  if (features == nullptr || features->empty()) {
+    PCL_ERROR(
+        "[pcl::%s::setTargetFeatures] Invalid or empty point cloud dataset given!\n",
+        getClassName().c_str());
     return;
   }
   target_features_ = features;
-  feature_tree_->setInputCloud (target_features_);
+  feature_tree_->setInputCloud(target_features_);
 }
 
-
-template <typename PointSource, typename PointTarget, typename FeatureT> void
-SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::selectSamples (
-    const PointCloudSource &cloud, int nr_samples, float min_sample_distance,
-    std::vector<int> &sample_indices)
+template <typename PointSource, typename PointTarget, typename FeatureT>
+void
+SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::selectSamples(
+    const PointCloudSource& cloud,
+    unsigned int nr_samples,
+    float min_sample_distance,
+    pcl::Indices& sample_indices)
 {
-  if (nr_samples > static_cast<int> (cloud.size ()))
-  {
-    PCL_ERROR ("[pcl::%s::selectSamples] ", getClassName ().c_str ());
-    PCL_ERROR("The number of samples (%d) must not be greater than the number of "
+  if (nr_samples > cloud.size()) {
+    PCL_ERROR("[pcl::%s::selectSamples] ", getClassName().c_str());
+    PCL_ERROR("The number of samples (%u) must not be greater than the number of "
               "points (%zu)!\n",
               nr_samples,
               static_cast<std::size_t>(cloud.size()));
@@ -89,41 +93,41 @@ SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::selectSampl
 
   // Iteratively draw random samples until nr_samples is reached
   index_t iterations_without_a_sample = 0;
-  const auto max_iterations_without_a_sample = 3 * cloud.size ();
-  sample_indices.clear ();
-  while (static_cast<int> (sample_indices.size ()) < nr_samples)
-  {
+  const auto max_iterations_without_a_sample = 3 * cloud.size();
+  sample_indices.clear();
+  while (sample_indices.size() < nr_samples) {
     // Choose a sample at random
-    int sample_index = getRandomIndex (static_cast<int> (cloud.size ()));
+    const auto sample_index = getRandomIndex(cloud.size());
 
     // Check to see if the sample is 1) unique and 2) far away from the other samples
     bool valid_sample = true;
-    for (const int &sample_idx : sample_indices)
-    {
-      float distance_between_samples = euclideanDistance (cloud[sample_index], cloud[sample_idx]);
+    for (const auto& sample_idx : sample_indices) {
+      float distance_between_samples =
+          euclideanDistance(cloud[sample_index], cloud[sample_idx]);
 
-      if (sample_index == sample_idx || distance_between_samples < min_sample_distance)
-      {
+      if (sample_index == sample_idx ||
+          distance_between_samples < min_sample_distance) {
         valid_sample = false;
         break;
       }
     }
 
     // If the sample is valid, add it to the output
-    if (valid_sample)
-    {
-      sample_indices.push_back (sample_index);
+    if (valid_sample) {
+      sample_indices.push_back(sample_index);
       iterations_without_a_sample = 0;
     }
     else
       ++iterations_without_a_sample;
 
     // If no valid samples can be found, relax the inter-sample distance requirements
-    if (static_cast<std::size_t>(iterations_without_a_sample) >= max_iterations_without_a_sample)
-    {
-      PCL_WARN ("[pcl::%s::selectSamples] ", getClassName ().c_str ());
-      PCL_WARN ("No valid sample found after %zu iterations. Relaxing min_sample_distance_ to %f\n",
-                static_cast<std::size_t>(iterations_without_a_sample), 0.5*min_sample_distance);
+    if (static_cast<std::size_t>(iterations_without_a_sample) >=
+        max_iterations_without_a_sample) {
+      PCL_WARN("[pcl::%s::selectSamples] ", getClassName().c_str());
+      PCL_WARN("No valid sample found after %zu iterations. Relaxing "
+               "min_sample_distance_ to %f\n",
+               static_cast<std::size_t>(iterations_without_a_sample),
+               0.5 * min_sample_distance);
 
       min_sample_distance_ *= 0.5f;
       min_sample_distance = min_sample_distance_;
@@ -132,132 +136,137 @@ SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::selectSampl
   }
 }
 
-
-template <typename PointSource, typename PointTarget, typename FeatureT> void
-SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::findSimilarFeatures (
-    const FeatureCloud &input_features, const std::vector<int> &sample_indices,
-    std::vector<int> &corresponding_indices)
+template <typename PointSource, typename PointTarget, typename FeatureT>
+void
+SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::
+    findSimilarFeatures(const FeatureCloud& input_features,
+                        const pcl::Indices& sample_indices,
+                        pcl::Indices& corresponding_indices)
 {
-  std::vector<int> nn_indices (k_correspondences_);
-  std::vector<float> nn_distances (k_correspondences_);
+  pcl::Indices nn_indices(k_correspondences_);
+  std::vector<float> nn_distances(k_correspondences_);
 
-  corresponding_indices.resize (sample_indices.size ());
-  for (std::size_t i = 0; i < sample_indices.size (); ++i)
-  {
+  corresponding_indices.resize(sample_indices.size());
+  for (std::size_t i = 0; i < sample_indices.size(); ++i) {
     // Find the k features nearest to input_features[sample_indices[i]]
-    feature_tree_->nearestKSearch (input_features, sample_indices[i], k_correspondences_, nn_indices, nn_distances);
+    feature_tree_->nearestKSearch(input_features,
+                                  sample_indices[i],
+                                  k_correspondences_,
+                                  nn_indices,
+                                  nn_distances);
 
     // Select one at random and add it to corresponding_indices
-    int random_correspondence = getRandomIndex (k_correspondences_);
+    const auto random_correspondence = getRandomIndex(k_correspondences_);
     corresponding_indices[i] = nn_indices[random_correspondence];
   }
 }
 
-
-template <typename PointSource, typename PointTarget, typename FeatureT> float
-SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::computeErrorMetric (
-    const PointCloudSource &cloud, float)
+template <typename PointSource, typename PointTarget, typename FeatureT>
+float
+SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::computeErrorMetric(
+    const PointCloudSource& cloud, float)
 {
-  std::vector<int> nn_index (1);
-  std::vector<float> nn_distance (1);
+  pcl::Indices nn_index(1);
+  std::vector<float> nn_distance(1);
 
-  const ErrorFunctor & compute_error = *error_functor_;
+  const ErrorFunctor& compute_error = *error_functor_;
   float error = 0;
 
-  for (int i = 0; i < static_cast<int> (cloud.size ()); ++i)
-  {
-    // Find the distance between cloud[i] and its nearest neighbor in the target point cloud
-    tree_->nearestKSearch (cloud, i, 1, nn_index, nn_distance);
+  for (const auto& point : cloud) {
+    // Find the distance between point and its nearest neighbor in the target point
+    // cloud
+    tree_->nearestKSearch(point, 1, nn_index, nn_distance);
 
     // Compute the error
-    error += compute_error (nn_distance[0]);
+    error += compute_error(nn_distance[0]);
   }
   return (error);
 }
 
-
-template <typename PointSource, typename PointTarget, typename FeatureT> void
-SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
+template <typename PointSource, typename PointTarget, typename FeatureT>
+void
+SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::
+    computeTransformation(PointCloudSource& output, const Eigen::Matrix4f& guess)
 {
   // Some sanity checks first
-  if (!input_features_)
-  {
-    PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
-    PCL_ERROR ("No source features were given! Call setSourceFeatures before aligning.\n");
+  if (!input_features_) {
+    PCL_ERROR("[pcl::%s::computeTransformation] ", getClassName().c_str());
+    PCL_ERROR(
+        "No source features were given! Call setSourceFeatures before aligning.\n");
     return;
   }
-  if (!target_features_)
-  {
-    PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
-    PCL_ERROR ("No target features were given! Call setTargetFeatures before aligning.\n");
+  if (!target_features_) {
+    PCL_ERROR("[pcl::%s::computeTransformation] ", getClassName().c_str());
+    PCL_ERROR(
+        "No target features were given! Call setTargetFeatures before aligning.\n");
     return;
   }
 
-  if (input_->size () != input_features_->size ())
-  {
-    PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
-    PCL_ERROR ("The source points and source feature points need to be in a one-to-one relationship! Current input cloud sizes: %ld vs %ld.\n",
-               input_->size (), input_features_->size ());
+  if (input_->size() != input_features_->size()) {
+    PCL_ERROR("[pcl::%s::computeTransformation] ", getClassName().c_str());
+    PCL_ERROR("The source points and source feature points need to be in a one-to-one "
+              "relationship! Current input cloud sizes: %ld vs %ld.\n",
+              input_->size(),
+              input_features_->size());
     return;
   }
 
-  if (target_->size () != target_features_->size ())
-  {
-    PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
-    PCL_ERROR ("The target points and target feature points need to be in a one-to-one relationship! Current input cloud sizes: %ld vs %ld.\n",
-               target_->size (), target_features_->size ());
+  if (target_->size() != target_features_->size()) {
+    PCL_ERROR("[pcl::%s::computeTransformation] ", getClassName().c_str());
+    PCL_ERROR("The target points and target feature points need to be in a one-to-one "
+              "relationship! Current input cloud sizes: %ld vs %ld.\n",
+              target_->size(),
+              target_features_->size());
     return;
   }
 
   if (!error_functor_)
-    error_functor_.reset (new TruncatedError (static_cast<float> (corr_dist_threshold_)));
+    error_functor_.reset(new TruncatedError(static_cast<float>(corr_dist_threshold_)));
 
-
-  std::vector<int> sample_indices (nr_samples_);
-  std::vector<int> corresponding_indices (nr_samples_);
+  pcl::Indices sample_indices(nr_samples_);
+  pcl::Indices corresponding_indices(nr_samples_);
   PointCloudSource input_transformed;
-  float lowest_error (0);
+  float lowest_error(0);
 
   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_);
-    lowest_error = computeErrorMetric (input_transformed, static_cast<float> (corr_dist_threshold_));
+    transformPointCloud(*input_, input_transformed, final_transformation_);
+    lowest_error =
+        computeErrorMetric(input_transformed, static_cast<float>(corr_dist_threshold_));
     i_iter = 1;
   }
 
-  for (; i_iter < max_iterations_; ++i_iter)
-  {
+  for (; i_iter < max_iterations_; ++i_iter) {
     // Draw nr_samples_ random samples
-    selectSamples (*input_, nr_samples_, min_sample_distance_, sample_indices);
+    selectSamples(*input_, nr_samples_, min_sample_distance_, sample_indices);
 
     // Find corresponding features in the target cloud
-    findSimilarFeatures (*input_features_, sample_indices, corresponding_indices);
+    findSimilarFeatures(*input_features_, sample_indices, corresponding_indices);
 
     // Estimate the transform from the samples to their corresponding points
-    transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_);
+    transformation_estimation_->estimateRigidTransformation(
+        *input_, sample_indices, *target_, corresponding_indices, transformation_);
 
     // Transform the data and compute the error
-    transformPointCloud (*input_, input_transformed, transformation_);
-    float error = computeErrorMetric (input_transformed, static_cast<float> (corr_dist_threshold_));
+    transformPointCloud(*input_, input_transformed, transformation_);
+    float error =
+        computeErrorMetric(input_transformed, static_cast<float>(corr_dist_threshold_));
 
     // If the new error is lower, update the final transformation
-    if (i_iter == 0 || error < lowest_error)
-    {
+    if (i_iter == 0 || error < lowest_error) {
       lowest_error = error;
       final_transformation_ = transformation_;
-      converged_=true;
+      converged_ = true;
     }
   }
 
   // Apply the final transformation
-  transformPointCloud (*input_, output, final_transformation_);
+  transformPointCloud(*input_, output, final_transformation_);
 }
 
 } // namespace pcl
 
-#endif  //#ifndef IA_RANSAC_HPP_
-
+#endif //#ifndef IA_RANSAC_HPP_
diff --git a/registration/include/pcl/registration/impl/icp.hpp b/registration/include/pcl/registration/impl/icp.hpp
index f533abbd..267f0118 100644
--- a/registration/include/pcl/registration/impl/icp.hpp
+++ b/registration/include/pcl/registration/impl/icp.hpp
@@ -41,88 +41,81 @@
 #ifndef PCL_REGISTRATION_IMPL_ICP_HPP_
 #define PCL_REGISTRATION_IMPL_ICP_HPP_
 
-#include <pcl/registration/boost.h>
 #include <pcl/correspondence.h>
 
+namespace pcl {
 
-namespace pcl
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
+IterativeClosestPoint<PointSource, PointTarget, Scalar>::transformCloud(
+    const PointCloudSource& input, PointCloudSource& output, const Matrix4& transform)
 {
+  Eigen::Vector4f pt(0.0f, 0.0f, 0.0f, 1.0f), pt_t;
+  Eigen::Matrix4f tr = transform.template cast<float>();
 
-template <typename PointSource, typename PointTarget, typename Scalar> void
-IterativeClosestPoint<PointSource, PointTarget, Scalar>::transformCloud (
-    const PointCloudSource &input,
-    PointCloudSource &output,
-    const Matrix4 &transform)
-{
-  Eigen::Vector4f pt (0.0f, 0.0f, 0.0f, 1.0f), pt_t;
-  Eigen::Matrix4f tr = transform.template cast<float> ();
-
-  // XYZ is ALWAYS present due to the templatization, so we only have to check for normals
-  if (source_has_normals_)
-  {
+  // XYZ is ALWAYS present due to the templatization, so we only have to check for
+  // normals
+  if (source_has_normals_) {
     Eigen::Vector3f nt, nt_t;
-    Eigen::Matrix3f rot = tr.block<3, 3> (0, 0);
+    Eigen::Matrix3f rot = tr.block<3, 3>(0, 0);
 
-    for (std::size_t i = 0; i < input.size (); ++i)
-    {
-      const std::uint8_t* data_in = reinterpret_cast<const std::uint8_t*> (&input[i]);
-      std::uint8_t* data_out = reinterpret_cast<std::uint8_t*> (&output[i]);
-      memcpy (&pt[0], data_in + x_idx_offset_, sizeof (float));
-      memcpy (&pt[1], data_in + y_idx_offset_, sizeof (float));
-      memcpy (&pt[2], data_in + z_idx_offset_, sizeof (float));
+    for (std::size_t i = 0; i < input.size(); ++i) {
+      const std::uint8_t* data_in = reinterpret_cast<const std::uint8_t*>(&input[i]);
+      std::uint8_t* data_out = reinterpret_cast<std::uint8_t*>(&output[i]);
+      memcpy(&pt[0], data_in + x_idx_offset_, sizeof(float));
+      memcpy(&pt[1], data_in + y_idx_offset_, sizeof(float));
+      memcpy(&pt[2], data_in + z_idx_offset_, sizeof(float));
 
-      if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2])) 
+      if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2]))
         continue;
 
       pt_t = tr * pt;
 
-      memcpy (data_out + x_idx_offset_, &pt_t[0], sizeof (float));
-      memcpy (data_out + y_idx_offset_, &pt_t[1], sizeof (float));
-      memcpy (data_out + z_idx_offset_, &pt_t[2], sizeof (float));
+      memcpy(data_out + x_idx_offset_, &pt_t[0], sizeof(float));
+      memcpy(data_out + y_idx_offset_, &pt_t[1], sizeof(float));
+      memcpy(data_out + z_idx_offset_, &pt_t[2], sizeof(float));
 
-      memcpy (&nt[0], data_in + nx_idx_offset_, sizeof (float));
-      memcpy (&nt[1], data_in + ny_idx_offset_, sizeof (float));
-      memcpy (&nt[2], data_in + nz_idx_offset_, sizeof (float));
+      memcpy(&nt[0], data_in + nx_idx_offset_, sizeof(float));
+      memcpy(&nt[1], data_in + ny_idx_offset_, sizeof(float));
+      memcpy(&nt[2], data_in + nz_idx_offset_, sizeof(float));
 
-      if (!std::isfinite (nt[0]) || !std::isfinite (nt[1]) || !std::isfinite (nt[2])) 
+      if (!std::isfinite(nt[0]) || !std::isfinite(nt[1]) || !std::isfinite(nt[2]))
         continue;
 
       nt_t = rot * nt;
 
-      memcpy (data_out + nx_idx_offset_, &nt_t[0], sizeof (float));
-      memcpy (data_out + ny_idx_offset_, &nt_t[1], sizeof (float));
-      memcpy (data_out + nz_idx_offset_, &nt_t[2], sizeof (float));
+      memcpy(data_out + nx_idx_offset_, &nt_t[0], sizeof(float));
+      memcpy(data_out + ny_idx_offset_, &nt_t[1], sizeof(float));
+      memcpy(data_out + nz_idx_offset_, &nt_t[2], sizeof(float));
     }
   }
-  else
-  {
-    for (std::size_t i = 0; i < input.size (); ++i)
-    {
-      const std::uint8_t* data_in = reinterpret_cast<const std::uint8_t*> (&input[i]);
-      std::uint8_t* data_out = reinterpret_cast<std::uint8_t*> (&output[i]);
-      memcpy (&pt[0], data_in + x_idx_offset_, sizeof (float));
-      memcpy (&pt[1], data_in + y_idx_offset_, sizeof (float));
-      memcpy (&pt[2], data_in + z_idx_offset_, sizeof (float));
-
-      if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2])) 
+  else {
+    for (std::size_t i = 0; i < input.size(); ++i) {
+      const std::uint8_t* data_in = reinterpret_cast<const std::uint8_t*>(&input[i]);
+      std::uint8_t* data_out = reinterpret_cast<std::uint8_t*>(&output[i]);
+      memcpy(&pt[0], data_in + x_idx_offset_, sizeof(float));
+      memcpy(&pt[1], data_in + y_idx_offset_, sizeof(float));
+      memcpy(&pt[2], data_in + z_idx_offset_, sizeof(float));
+
+      if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2]))
         continue;
 
       pt_t = tr * pt;
 
-      memcpy (data_out + x_idx_offset_, &pt_t[0], sizeof (float));
-      memcpy (data_out + y_idx_offset_, &pt_t[1], sizeof (float));
-      memcpy (data_out + z_idx_offset_, &pt_t[2], sizeof (float));
+      memcpy(data_out + x_idx_offset_, &pt_t[0], sizeof(float));
+      memcpy(data_out + y_idx_offset_, &pt_t[1], sizeof(float));
+      memcpy(data_out + z_idx_offset_, &pt_t[2], sizeof(float));
     }
   }
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> void
-IterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation (
-    PointCloudSource &output, const Matrix4 &guess)
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
+IterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation(
+    PointCloudSource& output, const Matrix4& guess)
 {
   // Point cloud containing the correspondences of each point in <input, indices>
-  PointCloudSourcePtr input_transformed (new PointCloudSource);
+  PointCloudSourcePtr input_transformed(new PointCloudSource);
 
   nr_iterations_ = 0;
   converged_ = false;
@@ -131,100 +124,102 @@ IterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation (
   final_transformation_ = guess;
 
   // If the guessed transformation is non identity
-  if (guess != Matrix4::Identity ())
-  {
-    input_transformed->resize (input_->size ());
-     // Apply guessed transformation prior to search for neighbours
-    transformCloud (*input_, *input_transformed, guess);
+  if (guess != Matrix4::Identity()) {
+    input_transformed->resize(input_->size());
+    // Apply guessed transformation prior to search for neighbours
+    transformCloud(*input_, *input_transformed, guess);
   }
   else
     *input_transformed = *input_;
 
-  transformation_ = Matrix4::Identity ();
+  transformation_ = Matrix4::Identity();
 
   // Make blobs if necessary
-  determineRequiredBlobData ();
-  PCLPointCloud2::Ptr target_blob (new PCLPointCloud2);
+  determineRequiredBlobData();
+  PCLPointCloud2::Ptr target_blob(new PCLPointCloud2);
   if (need_target_blob_)
-    pcl::toPCLPointCloud2 (*target_, *target_blob);
+    pcl::toPCLPointCloud2(*target_, *target_blob);
 
   // Pass in the default target for the Correspondence Estimation/Rejection code
-  correspondence_estimation_->setInputTarget (target_);
-  if (correspondence_estimation_->requiresTargetNormals ())
-    correspondence_estimation_->setTargetNormals (target_blob);
+  correspondence_estimation_->setInputTarget(target_);
+  if (correspondence_estimation_->requiresTargetNormals())
+    correspondence_estimation_->setTargetNormals(target_blob);
   // Correspondence Rejectors need a binary blob
-  for (std::size_t i = 0; i < correspondence_rejectors_.size (); ++i)
-  {
+  for (std::size_t i = 0; i < correspondence_rejectors_.size(); ++i) {
     registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
-    if (rej->requiresTargetPoints ())
-      rej->setTargetPoints (target_blob);
-    if (rej->requiresTargetNormals () && target_has_normals_)
-      rej->setTargetNormals (target_blob);
+    if (rej->requiresTargetPoints())
+      rej->setTargetPoints(target_blob);
+    if (rej->requiresTargetNormals() && target_has_normals_)
+      rej->setTargetNormals(target_blob);
   }
 
-  convergence_criteria_->setMaximumIterations (max_iterations_);
-  convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_);
-  convergence_criteria_->setTranslationThreshold (transformation_epsilon_);
+  convergence_criteria_->setMaximumIterations(max_iterations_);
+  convergence_criteria_->setRelativeMSE(euclidean_fitness_epsilon_);
+  convergence_criteria_->setTranslationThreshold(transformation_epsilon_);
   if (transformation_rotation_epsilon_ > 0)
-    convergence_criteria_->setRotationThreshold (transformation_rotation_epsilon_);
+    convergence_criteria_->setRotationThreshold(transformation_rotation_epsilon_);
   else
-    convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_);
+    convergence_criteria_->setRotationThreshold(1.0 - transformation_epsilon_);
 
   // Repeat until convergence
-  do
-  {
+  do {
     // Get blob data if needed
     PCLPointCloud2::Ptr input_transformed_blob;
-    if (need_source_blob_)
-    {
-      input_transformed_blob.reset (new PCLPointCloud2);
-      toPCLPointCloud2 (*input_transformed, *input_transformed_blob);
+    if (need_source_blob_) {
+      input_transformed_blob.reset(new PCLPointCloud2);
+      toPCLPointCloud2(*input_transformed, *input_transformed_blob);
     }
     // Save the previously estimated transformation
     previous_transformation_ = transformation_;
 
     // Set the source each iteration, to ensure the dirty flag is updated
-    correspondence_estimation_->setInputSource (input_transformed);
-    if (correspondence_estimation_->requiresSourceNormals ())
-      correspondence_estimation_->setSourceNormals (input_transformed_blob);
+    correspondence_estimation_->setInputSource(input_transformed);
+    if (correspondence_estimation_->requiresSourceNormals())
+      correspondence_estimation_->setSourceNormals(input_transformed_blob);
     // Estimate correspondences
     if (use_reciprocal_correspondence_)
-      correspondence_estimation_->determineReciprocalCorrespondences (*correspondences_, corr_dist_threshold_);
+      correspondence_estimation_->determineReciprocalCorrespondences(
+          *correspondences_, corr_dist_threshold_);
     else
-      correspondence_estimation_->determineCorrespondences (*correspondences_, corr_dist_threshold_);
+      correspondence_estimation_->determineCorrespondences(*correspondences_,
+                                                           corr_dist_threshold_);
 
-    //if (correspondence_rejectors_.empty ())
-    CorrespondencesPtr temp_correspondences (new Correspondences (*correspondences_));
-    for (std::size_t i = 0; i < correspondence_rejectors_.size (); ++i)
-    {
+    // if (correspondence_rejectors_.empty ())
+    CorrespondencesPtr temp_correspondences(new Correspondences(*correspondences_));
+    for (std::size_t i = 0; i < correspondence_rejectors_.size(); ++i) {
       registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
-      PCL_DEBUG ("Applying a correspondence rejector method: %s.\n", rej->getClassName ().c_str ());
-      if (rej->requiresSourcePoints ())
-        rej->setSourcePoints (input_transformed_blob);
-      if (rej->requiresSourceNormals () && source_has_normals_)
-        rej->setSourceNormals (input_transformed_blob);
-      rej->setInputCorrespondences (temp_correspondences);
-      rej->getCorrespondences (*correspondences_);
+      PCL_DEBUG("Applying a correspondence rejector method: %s.\n",
+                rej->getClassName().c_str());
+      if (rej->requiresSourcePoints())
+        rej->setSourcePoints(input_transformed_blob);
+      if (rej->requiresSourceNormals() && source_has_normals_)
+        rej->setSourceNormals(input_transformed_blob);
+      rej->setInputCorrespondences(temp_correspondences);
+      rej->getCorrespondences(*correspondences_);
       // Modify input for the next iteration
-      if (i < correspondence_rejectors_.size () - 1)
+      if (i < correspondence_rejectors_.size() - 1)
         *temp_correspondences = *correspondences_;
     }
 
-    std::size_t cnt = correspondences_->size ();
+    std::size_t cnt = correspondences_->size();
     // Check whether we have enough correspondences
-    if (static_cast<int> (cnt) < min_number_correspondences_)
-    {
-      PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
-      convergence_criteria_->setConvergenceState(pcl::registration::DefaultConvergenceCriteria<Scalar>::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES);
+    if (static_cast<int>(cnt) < min_number_correspondences_) {
+      PCL_ERROR("[pcl::%s::computeTransformation] Not enough correspondences found. "
+                "Relax your threshold parameters.\n",
+                getClassName().c_str());
+      convergence_criteria_->setConvergenceState(
+          pcl::registration::DefaultConvergenceCriteria<
+              Scalar>::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES);
       converged_ = false;
       break;
     }
 
     // Estimate the transform
-    transformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);
+    transformation_estimation_->estimateRigidTransformation(
+        *input_transformed, *target_, *correspondences_, transformation_);
 
     // Transform the data
-    transformCloud (*input_transformed, *input_transformed, transformation_);
+    transformCloud(*input_transformed, *input_transformed, transformation_);
 
     // Obtain the final transformation
     final_transformation_ = transformation_ * final_transformation_;
@@ -232,74 +227,91 @@ IterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation (
     ++nr_iterations_;
 
     // Update the vizualization of icp convergence
-    //if (update_visualizer_ != 0)
+    // if (update_visualizer_ != 0)
     //  update_visualizer_(output, source_indices_good, *target_, target_indices_good );
 
-    converged_ = static_cast<bool> ((*convergence_criteria_));
-  }
-  while (convergence_criteria_->getConvergenceState() == pcl::registration::DefaultConvergenceCriteria<Scalar>::CONVERGENCE_CRITERIA_NOT_CONVERGED);
+    converged_ = static_cast<bool>((*convergence_criteria_));
+  } while (convergence_criteria_->getConvergenceState() ==
+           pcl::registration::DefaultConvergenceCriteria<
+               Scalar>::CONVERGENCE_CRITERIA_NOT_CONVERGED);
 
   // Transform the input cloud using the final transformation
-  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),
-      final_transformation_ (3, 0), final_transformation_ (3, 1), final_transformation_ (3, 2), final_transformation_ (3, 3));
+  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),
+            final_transformation_(3, 0),
+            final_transformation_(3, 1),
+            final_transformation_(3, 2),
+            final_transformation_(3, 3));
 
   // Copy all the values
   output = *input_;
   // Transform the XYZ + normals
-  transformCloud (*input_, output, final_transformation_);
+  transformCloud(*input_, output, final_transformation_);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> void
-IterativeClosestPoint<PointSource, PointTarget, Scalar>::determineRequiredBlobData ()
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
+IterativeClosestPoint<PointSource, PointTarget, Scalar>::determineRequiredBlobData()
 {
   need_source_blob_ = false;
   need_target_blob_ = false;
   // Check estimator
-  need_source_blob_ |= correspondence_estimation_->requiresSourceNormals ();
-  need_target_blob_ |= correspondence_estimation_->requiresTargetNormals ();
+  need_source_blob_ |= correspondence_estimation_->requiresSourceNormals();
+  need_target_blob_ |= correspondence_estimation_->requiresTargetNormals();
   // Add warnings if necessary
-  if (correspondence_estimation_->requiresSourceNormals () && !source_has_normals_)
-  {
-      PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects source normals, but we can't provide them.\n", getClassName ().c_str ());
+  if (correspondence_estimation_->requiresSourceNormals() && !source_has_normals_) {
+    PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects source normals, "
+             "but we can't provide them.\n",
+             getClassName().c_str());
   }
-  if (correspondence_estimation_->requiresTargetNormals () && !target_has_normals_)
-  {
-      PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects target normals, but we can't provide them.\n", getClassName ().c_str ());
+  if (correspondence_estimation_->requiresTargetNormals() && !target_has_normals_) {
+    PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects target normals, "
+             "but we can't provide them.\n",
+             getClassName().c_str());
   }
   // Check rejectors
-  for (std::size_t i = 0; i < correspondence_rejectors_.size (); i++)
-  {
+  for (std::size_t i = 0; i < correspondence_rejectors_.size(); i++) {
     registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
-    need_source_blob_ |= rej->requiresSourcePoints ();
-    need_source_blob_ |= rej->requiresSourceNormals ();
-    need_target_blob_ |= rej->requiresTargetPoints ();
-    need_target_blob_ |= rej->requiresTargetNormals ();
-    if (rej->requiresSourceNormals () && !source_has_normals_)
-    {
-      PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects source normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ());
+    need_source_blob_ |= rej->requiresSourcePoints();
+    need_source_blob_ |= rej->requiresSourceNormals();
+    need_target_blob_ |= rej->requiresTargetPoints();
+    need_target_blob_ |= rej->requiresTargetNormals();
+    if (rej->requiresSourceNormals() && !source_has_normals_) {
+      PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects source "
+               "normals, but we can't provide them.\n",
+               getClassName().c_str(),
+               rej->getClassName().c_str());
     }
-    if (rej->requiresTargetNormals () && !target_has_normals_)
-    {
-      PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects target normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ());
+    if (rej->requiresTargetNormals() && !target_has_normals_) {
+      PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects target "
+               "normals, but we can't provide them.\n",
+               getClassName().c_str(),
+               rej->getClassName().c_str());
     }
   }
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> void
-IterativeClosestPointWithNormals<PointSource, PointTarget, Scalar>::transformCloud (
-    const PointCloudSource &input,
-    PointCloudSource &output,
-    const Matrix4 &transform)
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
+IterativeClosestPointWithNormals<PointSource, PointTarget, Scalar>::transformCloud(
+    const PointCloudSource& input, PointCloudSource& output, const Matrix4& transform)
 {
-  pcl::transformPointCloudWithNormals (input, output, transform);
+  pcl::transformPointCloudWithNormals(input, output, transform);
 }
 
 } // namespace pcl
 
 #endif /* PCL_REGISTRATION_IMPL_ICP_HPP_ */
-
diff --git a/registration/include/pcl/registration/impl/icp_nl.hpp b/registration/include/pcl/registration/impl/icp_nl.hpp
index 0cbf0ba5..3e01b981 100644
--- a/registration/include/pcl/registration/impl/icp_nl.hpp
+++ b/registration/include/pcl/registration/impl/icp_nl.hpp
@@ -41,4 +41,3 @@
 #define PCL_REGISTRATION_ICP_NL_HPP_
 
 #endif /* PCL_REGISTRATION_ICP_NL_HPP_ */
-
diff --git a/registration/include/pcl/registration/impl/incremental_registration.hpp b/registration/include/pcl/registration/impl/incremental_registration.hpp
index d7bc4116..cb1dadb1 100644
--- a/registration/include/pcl/registration/impl/incremental_registration.hpp
+++ b/registration/include/pcl/registration/impl/incremental_registration.hpp
@@ -38,43 +38,40 @@
 #ifndef PCL_REGISTRATION_IMPL_INCREMENTAL_REGISTRATION_HPP_
 #define PCL_REGISTRATION_IMPL_INCREMENTAL_REGISTRATION_HPP_
 
+namespace pcl {
 
-namespace pcl
-{
-
-namespace registration
-{
+namespace registration {
 
 template <typename PointT, typename Scalar>
-IncrementalRegistration<PointT, Scalar>::IncrementalRegistration () :
-  delta_transform_ (Matrix4::Identity ()),
-  abs_transform_ (Matrix4::Identity ())
+IncrementalRegistration<PointT, Scalar>::IncrementalRegistration()
+: delta_transform_(Matrix4::Identity()), abs_transform_(Matrix4::Identity())
 {}
 
-template <typename PointT, typename Scalar> bool
-IncrementalRegistration<PointT, Scalar>::registerCloud (const PointCloudConstPtr& cloud, const Matrix4& delta_estimate)
+template <typename PointT, typename Scalar>
+bool
+IncrementalRegistration<PointT, Scalar>::registerCloud(const PointCloudConstPtr& cloud,
+                                                       const Matrix4& delta_estimate)
 {
-  assert (registration_);
+  assert(registration_);
 
-  if (!last_cloud_)
-  {
+  if (!last_cloud_) {
     last_cloud_ = cloud;
     abs_transform_ = delta_transform_ = delta_estimate;
     return (true);
   }
 
-  registration_->setInputSource (cloud);
-  registration_->setInputTarget (last_cloud_);
+  registration_->setInputSource(cloud);
+  registration_->setInputTarget(last_cloud_);
 
   {
-  pcl::PointCloud<PointT> p;
-  registration_->align (p, delta_estimate);
+    pcl::PointCloud<PointT> p;
+    registration_->align(p, delta_estimate);
   }
 
-  bool converged = registration_->hasConverged ();
+  bool converged = registration_->hasConverged();
 
-  if ( converged ){
-    delta_transform_ = registration_->getFinalTransformation ();
+  if (converged) {
+    delta_transform_ = registration_->getFinalTransformation();
     abs_transform_ *= delta_transform_;
     last_cloud_ = cloud;
   }
@@ -82,27 +79,31 @@ IncrementalRegistration<PointT, Scalar>::registerCloud (const PointCloudConstPtr
   return (converged);
 }
 
-template <typename PointT, typename Scalar> inline typename pcl::registration::IncrementalRegistration<PointT, Scalar>::Matrix4
-IncrementalRegistration<PointT, Scalar>::getDeltaTransform () const
+template <typename PointT, typename Scalar>
+inline typename pcl::registration::IncrementalRegistration<PointT, Scalar>::Matrix4
+IncrementalRegistration<PointT, Scalar>::getDeltaTransform() const
 {
   return (delta_transform_);
 }
 
-template <typename PointT, typename Scalar> inline typename pcl::registration::IncrementalRegistration<PointT, Scalar>::Matrix4
-IncrementalRegistration<PointT, Scalar>::getAbsoluteTransform () const
+template <typename PointT, typename Scalar>
+inline typename pcl::registration::IncrementalRegistration<PointT, Scalar>::Matrix4
+IncrementalRegistration<PointT, Scalar>::getAbsoluteTransform() const
 {
   return (abs_transform_);
 }
 
-template <typename PointT, typename Scalar> inline void
-IncrementalRegistration<PointT, Scalar>::reset ()
+template <typename PointT, typename Scalar>
+inline void
+IncrementalRegistration<PointT, Scalar>::reset()
 {
-  last_cloud_.reset ();
-  delta_transform_ = abs_transform_ = Matrix4::Identity ();
+  last_cloud_.reset();
+  delta_transform_ = abs_transform_ = Matrix4::Identity();
 }
 
-template <typename PointT, typename Scalar> inline void
-IncrementalRegistration<PointT, Scalar>::setRegistration (RegistrationPtr registration)
+template <typename PointT, typename Scalar>
+inline void
+IncrementalRegistration<PointT, Scalar>::setRegistration(RegistrationPtr registration)
 {
   registration_ = registration;
 }
@@ -111,4 +112,3 @@ IncrementalRegistration<PointT, Scalar>::setRegistration (RegistrationPtr regist
 } // 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 bde6c317..edab731b 100644
--- a/registration/include/pcl/registration/impl/joint_icp.hpp
+++ b/registration/include/pcl/registration/impl/joint_icp.hpp
@@ -39,49 +39,44 @@
 #ifndef PCL_REGISTRATION_IMPL_JOINT_ICP_HPP_
 #define PCL_REGISTRATION_IMPL_JOINT_ICP_HPP_
 
-#include <pcl/registration/boost.h>
-#include <pcl/correspondence.h>
 #include <pcl/console/print.h>
+#include <pcl/correspondence.h>
 
+namespace pcl {
 
-namespace pcl
-{
-
-template <typename PointSource, typename PointTarget, typename Scalar> void
-JointIterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation (
-    PointCloudSource &output, const Matrix4 &guess)
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
+JointIterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation(
+    PointCloudSource& output, const Matrix4& guess)
 {
   // Point clouds containing the correspondences of each point in <input, indices>
-  if (sources_.size () != targets_.size () || sources_.empty () || targets_.empty ())
-  {
-    PCL_ERROR ("[pcl::%s::computeTransformation] Must set InputSources and InputTargets to the same, nonzero size!\n", 
-        getClassName ().c_str ());
+  if (sources_.size() != targets_.size() || sources_.empty() || targets_.empty()) {
+    PCL_ERROR("[pcl::%s::computeTransformation] Must set InputSources and InputTargets "
+              "to the same, nonzero size!\n",
+              getClassName().c_str());
     return;
   }
   bool manual_correspondence_estimations_set = true;
-  if (correspondence_estimations_.empty ())
-  {
+  if (correspondence_estimations_.empty()) {
     manual_correspondence_estimations_set = false;
-    correspondence_estimations_.resize (sources_.size ());
-    for (std::size_t i = 0; i < sources_.size (); i++)
-    {
-      correspondence_estimations_[i] = correspondence_estimation_->clone ();
-      KdTreeReciprocalPtr src_tree (new KdTreeReciprocal);
-      KdTreePtr tgt_tree (new KdTree);
-      correspondence_estimations_[i]->setSearchMethodTarget (tgt_tree);
-      correspondence_estimations_[i]->setSearchMethodSource (src_tree);
+    correspondence_estimations_.resize(sources_.size());
+    for (std::size_t i = 0; i < sources_.size(); i++) {
+      correspondence_estimations_[i] = correspondence_estimation_->clone();
+      KdTreeReciprocalPtr src_tree(new KdTreeReciprocal);
+      KdTreePtr tgt_tree(new KdTree);
+      correspondence_estimations_[i]->setSearchMethodTarget(tgt_tree);
+      correspondence_estimations_[i]->setSearchMethodSource(src_tree);
     }
   }
-  if (correspondence_estimations_.size () != sources_.size ())
-  {
-    PCL_ERROR ("[pcl::%s::computeTransform] Must set CorrespondenceEstimations to be the same size as the joint\n", 
-        getClassName ().c_str ());
+  if (correspondence_estimations_.size() != sources_.size()) {
+    PCL_ERROR("[pcl::%s::computeTransform] Must set CorrespondenceEstimations to be "
+              "the same size as the joint\n",
+              getClassName().c_str());
     return;
   }
-  std::vector<PointCloudSourcePtr> inputs_transformed (sources_.size ());
-  for (std::size_t i = 0; i < sources_.size (); i++)
-  {
-    inputs_transformed[i].reset (new PointCloudSource);
+  std::vector<PointCloudSourcePtr> inputs_transformed(sources_.size());
+  for (std::size_t i = 0; i < sources_.size(); i++) {
+    inputs_transformed[i].reset(new PointCloudSource);
   }
 
   nr_iterations_ = 0;
@@ -91,23 +86,20 @@ JointIterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformat
   final_transformation_ = guess;
 
   // Make a combined transformed input and output
-  std::vector<std::size_t> input_offsets (sources_.size ());
-  std::vector<std::size_t> target_offsets (targets_.size ());
-  PointCloudSourcePtr sources_combined (new PointCloudSource);
-  PointCloudSourcePtr inputs_transformed_combined (new PointCloudSource);
-  PointCloudTargetPtr targets_combined (new PointCloudTarget);
+  std::vector<std::size_t> input_offsets(sources_.size());
+  std::vector<std::size_t> target_offsets(targets_.size());
+  PointCloudSourcePtr sources_combined(new PointCloudSource);
+  PointCloudSourcePtr inputs_transformed_combined(new PointCloudSource);
+  PointCloudTargetPtr targets_combined(new PointCloudTarget);
   std::size_t input_offset = 0;
   std::size_t target_offset = 0;
-  for (std::size_t i = 0; i < sources_.size (); i++)
-  {
+  for (std::size_t i = 0; i < sources_.size(); i++) {
     // If the guessed transformation is non identity
-    if (guess != Matrix4::Identity ())
-    {
-       // Apply guessed transformation prior to search for neighbours
-      this->transformCloud (*sources_[i], *inputs_transformed[i], guess);
+    if (guess != Matrix4::Identity()) {
+      // Apply guessed transformation prior to search for neighbours
+      this->transformCloud(*sources_[i], *inputs_transformed[i], guess);
     }
-    else
-    {
+    else {
       *inputs_transformed[i] = *sources_[i];
     }
     *sources_combined += *sources_[i];
@@ -115,133 +107,137 @@ JointIterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformat
     *targets_combined += *targets_[i];
     input_offsets[i] = input_offset;
     target_offsets[i] = target_offset;
-    input_offset += inputs_transformed[i]->size ();
-    target_offset += targets_[i]->size ();
+    input_offset += inputs_transformed[i]->size();
+    target_offset += targets_[i]->size();
   }
 
-  transformation_ = Matrix4::Identity ();
+  transformation_ = Matrix4::Identity();
   // Make blobs if necessary
-  determineRequiredBlobData ();
+  determineRequiredBlobData();
   // Pass in the default target for the Correspondence Estimation/Rejection code
-  for (std::size_t i = 0; i < sources_.size (); i++)
-  {
-    correspondence_estimations_[i]->setInputTarget (targets_[i]);
-    if (correspondence_estimations_[i]->requiresTargetNormals ())
-    {
-      PCLPointCloud2::Ptr target_blob (new PCLPointCloud2);
-      pcl::toPCLPointCloud2 (*targets_[i], *target_blob);
-      correspondence_estimations_[i]->setTargetNormals (target_blob);
+  for (std::size_t i = 0; i < sources_.size(); i++) {
+    correspondence_estimations_[i]->setInputTarget(targets_[i]);
+    if (correspondence_estimations_[i]->requiresTargetNormals()) {
+      PCLPointCloud2::Ptr target_blob(new PCLPointCloud2);
+      pcl::toPCLPointCloud2(*targets_[i], *target_blob);
+      correspondence_estimations_[i]->setTargetNormals(target_blob);
     }
   }
 
-  PCLPointCloud2::Ptr targets_combined_blob (new PCLPointCloud2);
-  if (!correspondence_rejectors_.empty () && need_target_blob_)
-    pcl::toPCLPointCloud2 (*targets_combined, *targets_combined_blob);
+  PCLPointCloud2::Ptr targets_combined_blob(new PCLPointCloud2);
+  if (!correspondence_rejectors_.empty() && need_target_blob_)
+    pcl::toPCLPointCloud2(*targets_combined, *targets_combined_blob);
 
-  for (std::size_t i = 0; i < correspondence_rejectors_.size (); ++i)
-  {
+  for (std::size_t i = 0; i < correspondence_rejectors_.size(); ++i) {
     registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
-    if (rej->requiresTargetPoints ())
-      rej->setTargetPoints (targets_combined_blob);
-    if (rej->requiresTargetNormals () && target_has_normals_)
-      rej->setTargetNormals (targets_combined_blob);
+    if (rej->requiresTargetPoints())
+      rej->setTargetPoints(targets_combined_blob);
+    if (rej->requiresTargetNormals() && target_has_normals_)
+      rej->setTargetNormals(targets_combined_blob);
   }
 
-  convergence_criteria_->setMaximumIterations (max_iterations_);
-  convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_);
-  convergence_criteria_->setTranslationThreshold (transformation_epsilon_);
-  convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_);
+  convergence_criteria_->setMaximumIterations(max_iterations_);
+  convergence_criteria_->setRelativeMSE(euclidean_fitness_epsilon_);
+  convergence_criteria_->setTranslationThreshold(transformation_epsilon_);
+  convergence_criteria_->setRotationThreshold(1.0 - transformation_epsilon_);
 
   // Repeat until convergence
-  std::vector<CorrespondencesPtr> partial_correspondences_ (sources_.size ());
-  for (std::size_t i = 0; i < sources_.size (); i++)
-  {
-    partial_correspondences_[i].reset (new pcl::Correspondences);
+  std::vector<CorrespondencesPtr> partial_correspondences_(sources_.size());
+  for (std::size_t i = 0; i < sources_.size(); i++) {
+    partial_correspondences_[i].reset(new pcl::Correspondences);
   }
 
-  do
-  {
+  do {
     // Save the previously estimated transformation
     previous_transformation_ = transformation_;
 
     // Set the source each iteration, to ensure the dirty flag is updated
-    correspondences_->clear ();
-    for (std::size_t i = 0; i < correspondence_estimations_.size (); i++)
-    {
-      correspondence_estimations_[i]->setInputSource (inputs_transformed[i]);
+    correspondences_->clear();
+    for (std::size_t i = 0; i < correspondence_estimations_.size(); i++) {
+      correspondence_estimations_[i]->setInputSource(inputs_transformed[i]);
       // Get blob data if needed
-      if (correspondence_estimations_[i]->requiresSourceNormals ())
-      {
-        PCLPointCloud2::Ptr input_transformed_blob (new PCLPointCloud2);
-        toPCLPointCloud2 (*inputs_transformed[i], *input_transformed_blob);
-        correspondence_estimations_[i]->setSourceNormals (input_transformed_blob);
+      if (correspondence_estimations_[i]->requiresSourceNormals()) {
+        PCLPointCloud2::Ptr input_transformed_blob(new PCLPointCloud2);
+        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_)
-      {
-        correspondence_estimations_[i]->determineReciprocalCorrespondences (*partial_correspondences_[i], corr_dist_threshold_);
+      if (use_reciprocal_correspondence_) {
+        correspondence_estimations_[i]->determineReciprocalCorrespondences(
+            *partial_correspondences_[i], corr_dist_threshold_);
       }
-      else
-      {
-        correspondence_estimations_[i]->determineCorrespondences (*partial_correspondences_[i], corr_dist_threshold_);
+      else {
+        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 (),
-          partial_correspondences_[i]->size (), i);
-      for (std::size_t j = 0; j < partial_correspondences_[i]->size (); j++)
-      {
-        pcl::Correspondence corr = partial_correspondences_[i]->at (j);
+      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++) {
+        pcl::Correspondence corr = partial_correspondences_[i]->at(j);
         // Update the offsets to be for the combined clouds
         corr.index_query += input_offsets[i];
         corr.index_match += target_offsets[i];
-        correspondences_->push_back (corr);
+        correspondences_->push_back(corr);
       }
     }
-    PCL_DEBUG ("[pcl::%s::computeTransformation] Total correspondences: %d\n", getClassName ().c_str (), correspondences_->size ());
+    PCL_DEBUG("[pcl::%s::computeTransformation] Total correspondences: %d\n",
+              getClassName().c_str(),
+              correspondences_->size());
 
     PCLPointCloud2::Ptr inputs_transformed_combined_blob;
-    if (need_source_blob_)
-    {
-      inputs_transformed_combined_blob.reset (new PCLPointCloud2);
-      toPCLPointCloud2 (*inputs_transformed_combined, *inputs_transformed_combined_blob);
+    if (need_source_blob_) {
+      inputs_transformed_combined_blob.reset(new PCLPointCloud2);
+      toPCLPointCloud2(*inputs_transformed_combined, *inputs_transformed_combined_blob);
     }
-    CorrespondencesPtr temp_correspondences (new Correspondences (*correspondences_));
-    for (std::size_t i = 0; i < correspondence_rejectors_.size (); ++i)
-    {
-      PCL_DEBUG ("Applying a correspondence rejector method: %s.\n", correspondence_rejectors_[i]->getClassName ().c_str ());
+    CorrespondencesPtr temp_correspondences(new Correspondences(*correspondences_));
+    for (std::size_t i = 0; i < correspondence_rejectors_.size(); ++i) {
+      PCL_DEBUG("Applying a correspondence rejector method: %s.\n",
+                correspondence_rejectors_[i]->getClassName().c_str());
       registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
-      PCL_DEBUG ("Applying a correspondence rejector method: %s.\n", rej->getClassName ().c_str ());
-      if (rej->requiresSourcePoints ())
-        rej->setSourcePoints (inputs_transformed_combined_blob);
-      if (rej->requiresSourceNormals () && source_has_normals_)
-        rej->setSourceNormals (inputs_transformed_combined_blob);
-      rej->setInputCorrespondences (temp_correspondences);
-      rej->getCorrespondences (*correspondences_);
+      PCL_DEBUG("Applying a correspondence rejector method: %s.\n",
+                rej->getClassName().c_str());
+      if (rej->requiresSourcePoints())
+        rej->setSourcePoints(inputs_transformed_combined_blob);
+      if (rej->requiresSourceNormals() && source_has_normals_)
+        rej->setSourceNormals(inputs_transformed_combined_blob);
+      rej->setInputCorrespondences(temp_correspondences);
+      rej->getCorrespondences(*correspondences_);
       // Modify input for the next iteration
-      if (i < correspondence_rejectors_.size () - 1)
+      if (i < correspondence_rejectors_.size() - 1)
         *temp_correspondences = *correspondences_;
     }
 
-    int cnt = correspondences_->size ();
+    int cnt = correspondences_->size();
     // Check whether we have enough correspondences
-    if (cnt < min_number_correspondences_)
-    {
-      PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
-      convergence_criteria_->setConvergenceState(pcl::registration::DefaultConvergenceCriteria<Scalar>::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES);
+    if (cnt < min_number_correspondences_) {
+      PCL_ERROR("[pcl::%s::computeTransformation] Not enough correspondences found. "
+                "Relax your threshold parameters.\n",
+                getClassName().c_str());
+      convergence_criteria_->setConvergenceState(
+          pcl::registration::DefaultConvergenceCriteria<
+              Scalar>::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES);
       converged_ = false;
       break;
     }
 
     // Estimate the transform jointly, on a combined correspondence set
-    transformation_estimation_->estimateRigidTransformation (*inputs_transformed_combined, *targets_combined, *correspondences_, transformation_);
+    transformation_estimation_->estimateRigidTransformation(
+        *inputs_transformed_combined,
+        *targets_combined,
+        *correspondences_,
+        transformation_);
 
     // Transform the combined data
-    this->transformCloud (*inputs_transformed_combined, *inputs_transformed_combined, transformation_);
+    this->transformCloud(
+        *inputs_transformed_combined, *inputs_transformed_combined, transformation_);
     // And all its components
-    for (std::size_t i = 0; i < sources_.size (); i++)
-    {
-      this->transformCloud (*inputs_transformed[i], *inputs_transformed[i], transformation_);
+    for (std::size_t i = 0; i < sources_.size(); i++) {
+      this->transformCloud(
+          *inputs_transformed[i], *inputs_transformed[i], transformation_);
     }
 
     // Obtain the final transformation
@@ -250,73 +246,93 @@ JointIterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformat
     ++nr_iterations_;
 
     // Update the vizualization of icp convergence
-    //if (update_visualizer_ != 0)
+    // if (update_visualizer_ != 0)
     //  update_visualizer_(output, source_indices_good, *target_, target_indices_good );
 
-    converged_ = static_cast<bool> ((*convergence_criteria_));
-  }
-  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",
-      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),
-      final_transformation_ (3, 0), final_transformation_ (3, 1), final_transformation_ (3, 2), final_transformation_ (3, 3));
-
-  // For fitness checks, etc, we'll use an aggregated cloud for now (should be evaluating independently for correctness, but this requires propagating a few virtual methods from Registration)
-  IterativeClosestPoint<PointSource, PointTarget, Scalar>::setInputSource (sources_combined);
-  IterativeClosestPoint<PointSource, PointTarget, Scalar>::setInputTarget (targets_combined);
+    converged_ = static_cast<bool>((*convergence_criteria_));
+  } 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",
+            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),
+            final_transformation_(3, 0),
+            final_transformation_(3, 1),
+            final_transformation_(3, 2),
+            final_transformation_(3, 3));
+
+  // For fitness checks, etc, we'll use an aggregated cloud for now (should be
+  // evaluating independently for correctness, but this requires propagating a few
+  // virtual methods from Registration)
+  IterativeClosestPoint<PointSource, PointTarget, Scalar>::setInputSource(
+      sources_combined);
+  IterativeClosestPoint<PointSource, PointTarget, Scalar>::setInputTarget(
+      targets_combined);
 
   // If we automatically set the correspondence estimators, we should clear them now
-  if (!manual_correspondence_estimations_set)
-  {
-    correspondence_estimations_.clear ();
+  if (!manual_correspondence_estimations_set) {
+    correspondence_estimations_.clear();
   }
 
-
-  // 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 ();
+  // 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 <typename PointSource, typename PointTarget, typename Scalar> void
-JointIterativeClosestPoint<PointSource, PointTarget, Scalar>::determineRequiredBlobData ()
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
+JointIterativeClosestPoint<PointSource, PointTarget, Scalar>::
+    determineRequiredBlobData()
 {
   need_source_blob_ = false;
   need_target_blob_ = false;
   // Check estimators
-  for (std::size_t i = 0; i < correspondence_estimations_.size (); i++)
-  {
+  for (std::size_t i = 0; i < correspondence_estimations_.size(); i++) {
     CorrespondenceEstimationPtr& ce = correspondence_estimations_[i];
 
-    need_source_blob_ |= ce->requiresSourceNormals ();
-    need_target_blob_ |= ce->requiresTargetNormals ();
+    need_source_blob_ |= ce->requiresSourceNormals();
+    need_target_blob_ |= ce->requiresTargetNormals();
     // Add warnings if necessary
-    if (ce->requiresSourceNormals () && !source_has_normals_)
-    {
-        PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects source normals, but we can't provide them.\n", getClassName ().c_str ());
+    if (ce->requiresSourceNormals() && !source_has_normals_) {
+      PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects source normals, "
+               "but we can't provide them.\n",
+               getClassName().c_str());
     }
-    if (ce->requiresTargetNormals () && !target_has_normals_)
-    {
-        PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects target normals, but we can't provide them.\n", getClassName ().c_str ());
+    if (ce->requiresTargetNormals() && !target_has_normals_) {
+      PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects target normals, "
+               "but we can't provide them.\n",
+               getClassName().c_str());
     }
   }
   // Check rejectors
-  for (std::size_t i = 0; i < correspondence_rejectors_.size (); i++)
-  {
+  for (std::size_t i = 0; i < correspondence_rejectors_.size(); i++) {
     registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
-    need_source_blob_ |= rej->requiresSourcePoints ();
-    need_source_blob_ |= rej->requiresSourceNormals ();
-    need_target_blob_ |= rej->requiresTargetPoints ();
-    need_target_blob_ |= rej->requiresTargetNormals ();
-    if (rej->requiresSourceNormals () && !source_has_normals_)
-    {
-      PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects source normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ());
+    need_source_blob_ |= rej->requiresSourcePoints();
+    need_source_blob_ |= rej->requiresSourceNormals();
+    need_target_blob_ |= rej->requiresTargetPoints();
+    need_target_blob_ |= rej->requiresTargetNormals();
+    if (rej->requiresSourceNormals() && !source_has_normals_) {
+      PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects source "
+               "normals, but we can't provide them.\n",
+               getClassName().c_str(),
+               rej->getClassName().c_str());
     }
-    if (rej->requiresTargetNormals () && !target_has_normals_)
-    {
-      PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects target normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ());
+    if (rej->requiresTargetNormals() && !target_has_normals_) {
+      PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects target "
+               "normals, but we can't provide them.\n",
+               getClassName().c_str(),
+               rej->getClassName().c_str());
     }
   }
 }
@@ -324,4 +340,3 @@ JointIterativeClosestPoint<PointSource, PointTarget, Scalar>::determineRequiredB
 } // 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 81e37a65..5bc532d8 100644
--- a/registration/include/pcl/registration/impl/lum.hpp
+++ b/registration/include/pcl/registration/impl/lum.hpp
@@ -43,287 +43,307 @@
 
 #include <tuple>
 
+namespace pcl {
 
-namespace pcl
-{
-
-namespace registration
-{
+namespace registration {
 
-template<typename PointT> inline void
-LUM<PointT>::setLoopGraph (const SLAMGraphPtr &slam_graph)
+template <typename PointT>
+inline void
+LUM<PointT>::setLoopGraph(const SLAMGraphPtr& slam_graph)
 {
   slam_graph_ = slam_graph;
 }
 
-
-template<typename PointT> inline typename LUM<PointT>::SLAMGraphPtr
-LUM<PointT>::getLoopGraph () const
+template <typename PointT>
+inline typename LUM<PointT>::SLAMGraphPtr
+LUM<PointT>::getLoopGraph() const
 {
   return (slam_graph_);
 }
 
-
-template<typename PointT> typename LUM<PointT>::SLAMGraph::vertices_size_type
-LUM<PointT>::getNumVertices () const
+template <typename PointT>
+typename LUM<PointT>::SLAMGraph::vertices_size_type
+LUM<PointT>::getNumVertices() const
 {
-  return (num_vertices (*slam_graph_));
+  return (num_vertices(*slam_graph_));
 }
 
-
-template<typename PointT> void
-LUM<PointT>::setMaxIterations (int max_iterations)
+template <typename PointT>
+void
+LUM<PointT>::setMaxIterations(int max_iterations)
 {
   max_iterations_ = max_iterations;
 }
 
-
-template<typename PointT> inline int
-LUM<PointT>::getMaxIterations () const
+template <typename PointT>
+inline int
+LUM<PointT>::getMaxIterations() const
 {
   return (max_iterations_);
 }
 
-
-template<typename PointT> void
-LUM<PointT>::setConvergenceThreshold (float convergence_threshold)
+template <typename PointT>
+void
+LUM<PointT>::setConvergenceThreshold(float convergence_threshold)
 {
   convergence_threshold_ = convergence_threshold;
 }
 
-
-template<typename PointT> inline float
-LUM<PointT>::getConvergenceThreshold () const
+template <typename PointT>
+inline float
+LUM<PointT>::getConvergenceThreshold() const
 {
   return (convergence_threshold_);
 }
 
-
-template<typename PointT> typename LUM<PointT>::Vertex
-LUM<PointT>::addPointCloud (const PointCloudPtr &cloud, const Eigen::Vector6f &pose)
+template <typename PointT>
+typename LUM<PointT>::Vertex
+LUM<PointT>::addPointCloud(const PointCloudPtr& cloud, const Eigen::Vector6f& pose)
 {
-  Vertex v = add_vertex (*slam_graph_);
+  Vertex v = add_vertex(*slam_graph_);
   (*slam_graph_)[v].cloud_ = cloud;
-  if (v == 0 && pose != Eigen::Vector6f::Zero ())
-  {
-    PCL_WARN("[pcl::registration::LUM::addPointCloud] The pose estimate is ignored for the first cloud in the graph since that will become the reference pose.\n");
-    (*slam_graph_)[v].pose_ = Eigen::Vector6f::Zero ();
+  if (v == 0 && pose != Eigen::Vector6f::Zero()) {
+    PCL_WARN(
+        "[pcl::registration::LUM::addPointCloud] The pose estimate is ignored for the "
+        "first cloud in the graph since that will become the reference pose.\n");
+    (*slam_graph_)[v].pose_ = Eigen::Vector6f::Zero();
     return (v);
   }
   (*slam_graph_)[v].pose_ = pose;
   return (v);
 }
 
-
-template<typename PointT> inline void
-LUM<PointT>::setPointCloud (const Vertex &vertex, const PointCloudPtr &cloud)
+template <typename PointT>
+inline void
+LUM<PointT>::setPointCloud(const Vertex& vertex, const PointCloudPtr& cloud)
 {
-  if (vertex >= getNumVertices ())
-  {
-    PCL_ERROR("[pcl::registration::LUM::setPointCloud] You are attempting to set a point cloud to a non-existing graph vertex.\n");
+  if (vertex >= getNumVertices()) {
+    PCL_ERROR("[pcl::registration::LUM::setPointCloud] You are attempting to set a "
+              "point cloud to a non-existing graph vertex.\n");
     return;
   }
   (*slam_graph_)[vertex].cloud_ = cloud;
 }
 
-
-template<typename PointT> inline typename LUM<PointT>::PointCloudPtr
-LUM<PointT>::getPointCloud (const Vertex &vertex) const
+template <typename PointT>
+inline typename LUM<PointT>::PointCloudPtr
+LUM<PointT>::getPointCloud(const Vertex& vertex) const
 {
-  if (vertex >= getNumVertices ())
-  {
-    PCL_ERROR("[pcl::registration::LUM::getPointCloud] You are attempting to get a point cloud from a non-existing graph vertex.\n");
-    return (PointCloudPtr ());
+  if (vertex >= getNumVertices()) {
+    PCL_ERROR("[pcl::registration::LUM::getPointCloud] You are attempting to get a "
+              "point cloud from a non-existing graph vertex.\n");
+    return (PointCloudPtr());
   }
   return ((*slam_graph_)[vertex].cloud_);
 }
 
-
-template<typename PointT> inline void
-LUM<PointT>::setPose (const Vertex &vertex, const Eigen::Vector6f &pose)
+template <typename PointT>
+inline void
+LUM<PointT>::setPose(const Vertex& vertex, const Eigen::Vector6f& pose)
 {
-  if (vertex >= getNumVertices ())
-  {
-    PCL_ERROR("[pcl::registration::LUM::setPose] You are attempting to set a pose estimate to a non-existing graph vertex.\n");
+  if (vertex >= getNumVertices()) {
+    PCL_ERROR("[pcl::registration::LUM::setPose] You are attempting to set a pose "
+              "estimate to a non-existing graph vertex.\n");
     return;
   }
-  if (vertex == 0)
-  {
-    PCL_ERROR("[pcl::registration::LUM::setPose] The pose estimate is ignored for the first cloud in the graph since that will become the reference pose.\n");
+  if (vertex == 0) {
+    PCL_ERROR("[pcl::registration::LUM::setPose] The pose estimate is ignored for the "
+              "first cloud in the graph since that will become the reference pose.\n");
     return;
   }
   (*slam_graph_)[vertex].pose_ = pose;
 }
 
-
-template<typename PointT> inline Eigen::Vector6f
-LUM<PointT>::getPose (const Vertex &vertex) const
+template <typename PointT>
+inline Eigen::Vector6f
+LUM<PointT>::getPose(const Vertex& vertex) const
 {
-  if (vertex >= getNumVertices ())
-  {
-    PCL_ERROR("[pcl::registration::LUM::getPose] You are attempting to get a pose estimate from a non-existing graph vertex.\n");
-    return (Eigen::Vector6f::Zero ());
+  if (vertex >= getNumVertices()) {
+    PCL_ERROR("[pcl::registration::LUM::getPose] You are attempting to get a pose "
+              "estimate from a non-existing graph vertex.\n");
+    return (Eigen::Vector6f::Zero());
   }
   return ((*slam_graph_)[vertex].pose_);
 }
 
-
-template<typename PointT> inline Eigen::Affine3f
-LUM<PointT>::getTransformation (const Vertex &vertex) const
+template <typename PointT>
+inline Eigen::Affine3f
+LUM<PointT>::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)));
+  Eigen::Vector6f pose = getPose(vertex);
+  return (pcl::getTransformation(pose(0), pose(1), pose(2), pose(3), pose(4), pose(5)));
 }
 
-
-template<typename PointT> void
-LUM<PointT>::setCorrespondences (const Vertex &source_vertex, const Vertex &target_vertex, const pcl::CorrespondencesPtr &corrs)
+template <typename PointT>
+void
+LUM<PointT>::setCorrespondences(const Vertex& source_vertex,
+                                const Vertex& target_vertex,
+                                const pcl::CorrespondencesPtr& corrs)
 {
-  if (source_vertex >= getNumVertices () || target_vertex >= getNumVertices () || source_vertex == target_vertex)
-  {
-    PCL_ERROR("[pcl::registration::LUM::setCorrespondences] You are attempting to set a set of correspondences between non-existing or identical graph vertices.\n");
+  if (source_vertex >= getNumVertices() || target_vertex >= getNumVertices() ||
+      source_vertex == target_vertex) {
+    PCL_ERROR(
+        "[pcl::registration::LUM::setCorrespondences] You are attempting to set a set "
+        "of correspondences between non-existing or identical graph vertices.\n");
     return;
   }
   Edge e;
   bool present;
-  std::tie (e, present) = edge (source_vertex, target_vertex, *slam_graph_);
+  std::tie(e, present) = edge(source_vertex, target_vertex, *slam_graph_);
   if (!present)
-    std::tie (e, present) = add_edge (source_vertex, target_vertex, *slam_graph_);
+    std::tie(e, present) = add_edge(source_vertex, target_vertex, *slam_graph_);
   (*slam_graph_)[e].corrs_ = corrs;
 }
 
-
-template<typename PointT> inline pcl::CorrespondencesPtr
-LUM<PointT>::getCorrespondences (const Vertex &source_vertex, const Vertex &target_vertex) const
+template <typename PointT>
+inline pcl::CorrespondencesPtr
+LUM<PointT>::getCorrespondences(const Vertex& source_vertex,
+                                const Vertex& target_vertex) const
 {
-  if (source_vertex >= getNumVertices () || target_vertex >= getNumVertices ())
-  {
-    PCL_ERROR("[pcl::registration::LUM::getCorrespondences] You are attempting to get a set of correspondences between non-existing graph vertices.\n");
-    return (pcl::CorrespondencesPtr ());
+  if (source_vertex >= getNumVertices() || target_vertex >= getNumVertices()) {
+    PCL_ERROR("[pcl::registration::LUM::getCorrespondences] You are attempting to get "
+              "a set of correspondences between non-existing graph vertices.\n");
+    return (pcl::CorrespondencesPtr());
   }
   Edge e;
   bool present;
-  std::tie (e, present) = edge (source_vertex, target_vertex, *slam_graph_);
-  if (!present)
-  {
-    PCL_ERROR("[pcl::registration::LUM::getCorrespondences] You are attempting to get a set of correspondences from a non-existing graph edge.\n");
-    return (pcl::CorrespondencesPtr ());
+  std::tie(e, present) = edge(source_vertex, target_vertex, *slam_graph_);
+  if (!present) {
+    PCL_ERROR("[pcl::registration::LUM::getCorrespondences] You are attempting to get "
+              "a set of correspondences from a non-existing graph edge.\n");
+    return (pcl::CorrespondencesPtr());
   }
   return ((*slam_graph_)[e].corrs_);
 }
 
-
-template<typename PointT> void
-LUM<PointT>::compute ()
+template <typename PointT>
+void
+LUM<PointT>::compute()
 {
-  int n = static_cast<int> (getNumVertices ());
-  if (n < 2)
-  {
-    PCL_ERROR("[pcl::registration::LUM::compute] The slam graph needs at least 2 vertices.\n");
+  int n = static_cast<int>(getNumVertices());
+  if (n < 2) {
+    PCL_ERROR("[pcl::registration::LUM::compute] The slam graph needs at least 2 "
+              "vertices.\n");
     return;
   }
-  for (int i = 0; i < max_iterations_; ++i)
-  {
-    // Linearized computation of C^-1 and C^-1*D and convergence checking for all edges in the graph (results stored in slam_graph_)
+  for (int i = 0; i < max_iterations_; ++i) {
+    // Linearized computation of C^-1 and C^-1*D and convergence checking for all edges
+    // in the graph (results stored in slam_graph_)
     typename SLAMGraph::edge_iterator e, e_end;
-    for (std::tie (e, e_end) = edges (*slam_graph_); e != e_end; ++e)
-      computeEdge (*e);
+    for (std::tie(e, e_end) = edges(*slam_graph_); e != e_end; ++e)
+      computeEdge(*e);
 
     // Declare matrices G and B
-    Eigen::MatrixXf G = Eigen::MatrixXf::Zero (6 * (n - 1), 6 * (n - 1));
-    Eigen::VectorXf B = Eigen::VectorXf::Zero (6 * (n - 1));
+    Eigen::MatrixXf G = Eigen::MatrixXf::Zero(6 * (n - 1), 6 * (n - 1));
+    Eigen::VectorXf B = Eigen::VectorXf::Zero(6 * (n - 1));
 
     // Start at 1 because 0 is the reference pose
-    for (int vi = 1; vi != n; ++vi)
-    {
-      for (int vj = 0; vj != n; ++vj)
-      {
-        // Attempt to use the forward edge, otherwise use backward edge, otherwise there was no edge
+    for (int vi = 1; vi != n; ++vi) {
+      for (int vj = 0; vj != n; ++vj) {
+        // Attempt to use the forward edge, otherwise use backward edge, otherwise there
+        // was no edge
         Edge e;
         bool present1;
-        std::tie (e, present1) = edge (vi, vj, *slam_graph_);
-        if (!present1)
-        {
+        std::tie(e, present1) = edge(vi, vj, *slam_graph_);
+        if (!present1) {
           bool present2;
-          std::tie (e, present2) = edge (vj, vi, *slam_graph_);
+          std::tie(e, present2) = edge(vj, vi, *slam_graph_);
           if (!present2)
             continue;
         }
 
         // Fill in elements of G and B
         if (vj > 0)
-          G.block (6 * (vi - 1), 6 * (vj - 1), 6, 6) = -(*slam_graph_)[e].cinv_;
-        G.block (6 * (vi - 1), 6 * (vi - 1), 6, 6) += (*slam_graph_)[e].cinv_;
-        B.segment (6 * (vi - 1), 6) += (present1 ? 1 : -1) * (*slam_graph_)[e].cinvd_;
+          G.block(6 * (vi - 1), 6 * (vj - 1), 6, 6) = -(*slam_graph_)[e].cinv_;
+        G.block(6 * (vi - 1), 6 * (vi - 1), 6, 6) += (*slam_graph_)[e].cinv_;
+        B.segment(6 * (vi - 1), 6) += (present1 ? 1 : -1) * (*slam_graph_)[e].cinvd_;
       }
     }
 
     // Computation of the linear equation system: GX = B
-    // TODO investigate accuracy vs. speed tradeoff and find the best solving method for our type of linear equation (sparse)
-    Eigen::VectorXf X = G.colPivHouseholderQr ().solve (B);
+    // TODO investigate accuracy vs. speed tradeoff and find the best solving method for
+    // our type of linear equation (sparse)
+    Eigen::VectorXf X = G.colPivHouseholderQr().solve(B);
 
     // Update the poses
     float sum = 0.0;
-    for (int vi = 1; vi != n; ++vi)
-    {
-      Eigen::Vector6f difference_pose = static_cast<Eigen::Vector6f> (-incidenceCorrection (getPose (vi)).inverse () * X.segment (6 * (vi - 1), 6));
-      sum += difference_pose.norm ();
-      setPose (vi, getPose (vi) + difference_pose);
+    for (int vi = 1; vi != n; ++vi) {
+      Eigen::Vector6f difference_pose = static_cast<Eigen::Vector6f>(
+          -incidenceCorrection(getPose(vi)).inverse() * X.segment(6 * (vi - 1), 6));
+      sum += difference_pose.norm();
+      setPose(vi, getPose(vi) + difference_pose);
     }
 
     // Convergence check
-    if (sum <= convergence_threshold_ * static_cast<float> (n - 1))
+    if (sum <= convergence_threshold_ * static_cast<float>(n - 1))
       return;
   }
 }
 
-
-template<typename PointT> typename LUM<PointT>::PointCloudPtr
-LUM<PointT>::getTransformedCloud (const Vertex &vertex) const
+template <typename PointT>
+typename LUM<PointT>::PointCloudPtr
+LUM<PointT>::getTransformedCloud(const Vertex& vertex) const
 {
-  PointCloudPtr out (new PointCloud);
-  pcl::transformPointCloud (*getPointCloud (vertex), *out, getTransformation (vertex));
+  PointCloudPtr out(new PointCloud);
+  pcl::transformPointCloud(*getPointCloud(vertex), *out, getTransformation(vertex));
   return (out);
 }
 
-
-template<typename PointT> typename LUM<PointT>::PointCloudPtr
-LUM<PointT>::getConcatenatedCloud () const
+template <typename PointT>
+typename LUM<PointT>::PointCloudPtr
+LUM<PointT>::getConcatenatedCloud() const
 {
-  PointCloudPtr out (new PointCloud);
+  PointCloudPtr out(new PointCloud);
   typename SLAMGraph::vertex_iterator v, v_end;
-  for (std::tie (v, v_end) = vertices (*slam_graph_); v != v_end; ++v)
-  {
+  for (std::tie(v, v_end) = vertices(*slam_graph_); v != v_end; ++v) {
     PointCloud temp;
-    pcl::transformPointCloud (*getPointCloud (*v), temp, getTransformation (*v));
+    pcl::transformPointCloud(*getPointCloud(*v), temp, getTransformation(*v));
     *out += temp;
   }
   return (out);
 }
 
-
-template<typename PointT> void
-LUM<PointT>::computeEdge (const Edge &e)
+template <typename PointT>
+void
+LUM<PointT>::computeEdge(const Edge& e)
 {
   // Get necessary local data from graph
-  PointCloudPtr source_cloud = (*slam_graph_)[source (e, *slam_graph_)].cloud_;
-  PointCloudPtr target_cloud = (*slam_graph_)[target (e, *slam_graph_)].cloud_;
-  Eigen::Vector6f source_pose = (*slam_graph_)[source (e, *slam_graph_)].pose_;
-  Eigen::Vector6f target_pose = (*slam_graph_)[target (e, *slam_graph_)].pose_;
+  PointCloudPtr source_cloud = (*slam_graph_)[source(e, *slam_graph_)].cloud_;
+  PointCloudPtr target_cloud = (*slam_graph_)[target(e, *slam_graph_)].cloud_;
+  Eigen::Vector6f source_pose = (*slam_graph_)[source(e, *slam_graph_)].pose_;
+  Eigen::Vector6f target_pose = (*slam_graph_)[target(e, *slam_graph_)].pose_;
   pcl::CorrespondencesPtr corrs = (*slam_graph_)[e].corrs_;
 
   // Build the average and difference vectors for all correspondences
-  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > corrs_aver (corrs->size ());
-  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > corrs_diff (corrs->size ());
-  int oci = 0;  // oci = output correspondence iterator
-  for (int ici = 0; ici != static_cast<int> (corrs->size ()); ++ici)  // ici = input correspondence iterator
+  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> corrs_aver(
+      corrs->size());
+  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> corrs_diff(
+      corrs->size());
+  int oci = 0;                       // oci = output correspondence iterator
+  for (const auto& icorr : (*corrs)) // icorr = input correspondence
   {
     // Compound the point pair onto the current pose
-    Eigen::Vector3f source_compounded = pcl::getTransformation (source_pose (0), source_pose (1), source_pose (2), source_pose (3), source_pose (4), source_pose (5)) * (*source_cloud)[(*corrs)[ici].index_query].getVector3fMap ();
-    Eigen::Vector3f target_compounded = pcl::getTransformation (target_pose (0), target_pose (1), target_pose (2), target_pose (3), target_pose (4), target_pose (5)) * (*target_cloud)[(*corrs)[ici].index_match].getVector3fMap ();
+    Eigen::Vector3f source_compounded =
+        pcl::getTransformation(source_pose(0),
+                               source_pose(1),
+                               source_pose(2),
+                               source_pose(3),
+                               source_pose(4),
+                               source_pose(5)) *
+        (*source_cloud)[icorr.index_query].getVector3fMap();
+    Eigen::Vector3f target_compounded =
+        pcl::getTransformation(target_pose(0),
+                               target_pose(1),
+                               target_pose(2),
+                               target_pose(3),
+                               target_pose(4),
+                               target_pose(5)) *
+        (*target_cloud)[icorr.index_match].getVector3fMap();
 
     // NaN points can not be passed to the remaining computational pipeline
-    if (!std::isfinite (source_compounded (0)) || !std::isfinite (source_compounded (1)) || !std::isfinite (source_compounded (2)) || !std::isfinite (target_compounded (0)) || !std::isfinite (target_compounded (1)) || !std::isfinite (target_compounded (2)))
+    if (!std::isfinite(source_compounded(0)) || !std::isfinite(source_compounded(1)) ||
+        !std::isfinite(source_compounded(2)) || !std::isfinite(target_compounded(0)) ||
+        !std::isfinite(target_compounded(1)) || !std::isfinite(target_compounded(2)))
       continue;
 
     // Compute the point pair average and difference and store for later use
@@ -331,72 +351,87 @@ LUM<PointT>::computeEdge (const Edge &e)
     corrs_diff[oci] = source_compounded - target_compounded;
     oci++;
   }
-  corrs_aver.resize (oci);
-  corrs_diff.resize (oci);
+  corrs_aver.resize(oci);
+  corrs_diff.resize(oci);
 
   // Need enough valid correspondences to get a good triangulation
-  if (oci < 3)
-  {
-    PCL_WARN("[pcl::registration::LUM::compute] The correspondences between vertex %d and %d do not contain enough valid correspondences to be considered for computation.\n", source (e, *slam_graph_), target (e, *slam_graph_));
-    (*slam_graph_)[e].cinv_ = Eigen::Matrix6f::Zero ();
-    (*slam_graph_)[e].cinvd_ = Eigen::Vector6f::Zero ();
+  if (oci < 3) {
+    PCL_WARN("[pcl::registration::LUM::compute] The correspondences between vertex %d "
+             "and %d do not contain enough valid correspondences to be considered for "
+             "computation.\n",
+             source(e, *slam_graph_),
+             target(e, *slam_graph_));
+    (*slam_graph_)[e].cinv_ = Eigen::Matrix6f::Zero();
+    (*slam_graph_)[e].cinvd_ = Eigen::Vector6f::Zero();
     return;
   }
 
   // Build the matrices M'M and M'Z
-  Eigen::Matrix6f MM = Eigen::Matrix6f::Zero ();
-  Eigen::Vector6f MZ = Eigen::Vector6f::Zero ();
-  for (int ci = 0; ci != oci; ++ci)  // ci = correspondence iterator
+  Eigen::Matrix6f MM = Eigen::Matrix6f::Zero();
+  Eigen::Vector6f MZ = Eigen::Vector6f::Zero();
+  for (int ci = 0; ci != oci; ++ci) // ci = correspondence iterator
   {
     // Fast computation of summation elements of M'M
-    MM (0, 4) -= corrs_aver[ci] (1);
-    MM (0, 5) += corrs_aver[ci] (2);
-    MM (1, 3) -= corrs_aver[ci] (2);
-    MM (1, 4) += corrs_aver[ci] (0);
-    MM (2, 3) += corrs_aver[ci] (1);
-    MM (2, 5) -= corrs_aver[ci] (0);
-    MM (3, 4) -= corrs_aver[ci] (0) * corrs_aver[ci] (2);
-    MM (3, 5) -= corrs_aver[ci] (0) * corrs_aver[ci] (1);
-    MM (4, 5) -= corrs_aver[ci] (1) * corrs_aver[ci] (2);
-    MM (3, 3) += corrs_aver[ci] (1) * corrs_aver[ci] (1) + corrs_aver[ci] (2) * corrs_aver[ci] (2);
-    MM (4, 4) += corrs_aver[ci] (0) * corrs_aver[ci] (0) + corrs_aver[ci] (1) * corrs_aver[ci] (1);
-    MM (5, 5) += corrs_aver[ci] (0) * corrs_aver[ci] (0) + corrs_aver[ci] (2) * corrs_aver[ci] (2);
+    MM(0, 4) -= corrs_aver[ci](1);
+    MM(0, 5) += corrs_aver[ci](2);
+    MM(1, 3) -= corrs_aver[ci](2);
+    MM(1, 4) += corrs_aver[ci](0);
+    MM(2, 3) += corrs_aver[ci](1);
+    MM(2, 5) -= corrs_aver[ci](0);
+    MM(3, 4) -= corrs_aver[ci](0) * corrs_aver[ci](2);
+    MM(3, 5) -= corrs_aver[ci](0) * corrs_aver[ci](1);
+    MM(4, 5) -= corrs_aver[ci](1) * corrs_aver[ci](2);
+    MM(3, 3) +=
+        corrs_aver[ci](1) * corrs_aver[ci](1) + corrs_aver[ci](2) * corrs_aver[ci](2);
+    MM(4, 4) +=
+        corrs_aver[ci](0) * corrs_aver[ci](0) + corrs_aver[ci](1) * corrs_aver[ci](1);
+    MM(5, 5) +=
+        corrs_aver[ci](0) * corrs_aver[ci](0) + corrs_aver[ci](2) * corrs_aver[ci](2);
 
     // Fast computation of M'Z
-    MZ (0) += corrs_diff[ci] (0);
-    MZ (1) += corrs_diff[ci] (1);
-    MZ (2) += corrs_diff[ci] (2);
-    MZ (3) += corrs_aver[ci] (1) * corrs_diff[ci] (2) - corrs_aver[ci] (2) * corrs_diff[ci] (1);
-    MZ (4) += corrs_aver[ci] (0) * corrs_diff[ci] (1) - corrs_aver[ci] (1) * corrs_diff[ci] (0);
-    MZ (5) += corrs_aver[ci] (2) * corrs_diff[ci] (0) - corrs_aver[ci] (0) * corrs_diff[ci] (2);
+    MZ(0) += corrs_diff[ci](0);
+    MZ(1) += corrs_diff[ci](1);
+    MZ(2) += corrs_diff[ci](2);
+    MZ(3) +=
+        corrs_aver[ci](1) * corrs_diff[ci](2) - corrs_aver[ci](2) * corrs_diff[ci](1);
+    MZ(4) +=
+        corrs_aver[ci](0) * corrs_diff[ci](1) - corrs_aver[ci](1) * corrs_diff[ci](0);
+    MZ(5) +=
+        corrs_aver[ci](2) * corrs_diff[ci](0) - corrs_aver[ci](0) * corrs_diff[ci](2);
   }
   // Remaining elements of M'M
-  MM (0, 0) = MM (1, 1) = MM (2, 2) = static_cast<float> (oci);
-  MM (4, 0) = MM (0, 4);
-  MM (5, 0) = MM (0, 5);
-  MM (3, 1) = MM (1, 3);
-  MM (4, 1) = MM (1, 4);
-  MM (3, 2) = MM (2, 3);
-  MM (5, 2) = MM (2, 5);
-  MM (4, 3) = MM (3, 4);
-  MM (5, 3) = MM (3, 5);
-  MM (5, 4) = MM (4, 5);
+  MM(0, 0) = MM(1, 1) = MM(2, 2) = static_cast<float>(oci);
+  MM(4, 0) = MM(0, 4);
+  MM(5, 0) = MM(0, 5);
+  MM(3, 1) = MM(1, 3);
+  MM(4, 1) = MM(1, 4);
+  MM(3, 2) = MM(2, 3);
+  MM(5, 2) = MM(2, 5);
+  MM(4, 3) = MM(3, 4);
+  MM(5, 3) = MM(3, 5);
+  MM(5, 4) = MM(4, 5);
 
   // Compute pose difference estimation
-  Eigen::Vector6f D = static_cast<Eigen::Vector6f> (MM.inverse () * MZ);
+  Eigen::Vector6f D = static_cast<Eigen::Vector6f>(MM.inverse() * MZ);
 
   // Compute s^2
   float ss = 0.0f;
-  for (int ci = 0; ci != oci; ++ci)  // ci = correspondence iterator
-    ss += static_cast<float> (std::pow (corrs_diff[ci] (0) - (D (0) + corrs_aver[ci] (2) * D (5) - corrs_aver[ci] (1) * D (4)), 2.0f)
-                            + std::pow (corrs_diff[ci] (1) - (D (1) + corrs_aver[ci] (0) * D (4) - corrs_aver[ci] (2) * D (3)), 2.0f)
-                            + std::pow (corrs_diff[ci] (2) - (D (2) + corrs_aver[ci] (1) * D (3) - corrs_aver[ci] (0) * D (5)), 2.0f));
+  for (int ci = 0; ci != oci; ++ci) // ci = correspondence iterator
+    ss += static_cast<float>(
+        std::pow(corrs_diff[ci](0) -
+                     (D(0) + corrs_aver[ci](2) * D(5) - corrs_aver[ci](1) * D(4)),
+                 2.0f) +
+        std::pow(corrs_diff[ci](1) -
+                     (D(1) + corrs_aver[ci](0) * D(4) - corrs_aver[ci](2) * D(3)),
+                 2.0f) +
+        std::pow(corrs_diff[ci](2) -
+                     (D(2) + corrs_aver[ci](1) * D(3) - corrs_aver[ci](0) * D(5)),
+                 2.0f));
 
   // When reaching the limitations of computation due to linearization
-  if (ss < 0.0000000000001 || !std::isfinite (ss))
-  {
-    (*slam_graph_)[e].cinv_ = Eigen::Matrix6f::Zero ();
-    (*slam_graph_)[e].cinvd_ = Eigen::Vector6f::Zero ();
+  if (ss < 0.0000000000001 || !std::isfinite(ss)) {
+    (*slam_graph_)[e].cinv_ = Eigen::Matrix6f::Zero();
+    (*slam_graph_)[e].cinvd_ = Eigen::Vector6f::Zero();
     return;
   }
 
@@ -405,25 +440,26 @@ LUM<PointT>::computeEdge (const Edge &e)
   (*slam_graph_)[e].cinvd_ = MZ * (1.0f / ss);
 }
 
-
-template<typename PointT> inline Eigen::Matrix6f
-LUM<PointT>::incidenceCorrection (const Eigen::Vector6f &pose)
+template <typename PointT>
+inline Eigen::Matrix6f
+LUM<PointT>::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));
-  out (0, 4) = pose (1) * sx - pose (2) * cx;
-  out (0, 5) = pose (1) * cx * cy + pose (2) * sx * cy;
-  out (1, 3) = pose (2);
-  out (1, 4) = -pose (0) * sx;
-  out (1, 5) = -pose (0) * cx * cy + pose (2) * sy;
-  out (2, 3) = -pose (1);
-  out (2, 4) = pose (0) * cx;
-  out (2, 5) = -pose (0) * sx * cy - pose (1) * sy;
-  out (3, 5) = sy;
-  out (4, 4) = sx;
-  out (4, 5) = cx * cy;
-  out (5, 4) = cx;
-  out (5, 5) = -sx * cy;
+  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));
+  out(0, 4) = pose(1) * sx - pose(2) * cx;
+  out(0, 5) = pose(1) * cx * cy + pose(2) * sx * cy;
+  out(1, 3) = pose(2);
+  out(1, 4) = -pose(0) * sx;
+  out(1, 5) = -pose(0) * cx * cy + pose(2) * sy;
+  out(2, 3) = -pose(1);
+  out(2, 4) = pose(0) * cx;
+  out(2, 5) = -pose(0) * sx * cy - pose(1) * sy;
+  out(3, 5) = sy;
+  out(4, 4) = sx;
+  out(4, 5) = cx * cy;
+  out(5, 4) = cx;
+  out(5, 5) = -sx * cy;
   return (out);
 }
 
@@ -432,5 +468,4 @@ LUM<PointT>::incidenceCorrection (const Eigen::Vector6f &pose)
 
 #define PCL_INSTANTIATE_LUM(T) template class PCL_EXPORTS pcl::registration::LUM<T>;
 
-#endif  // PCL_REGISTRATION_IMPL_LUM_HPP_
-
+#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 9a3479f7..b5ec52c5 100644
--- a/registration/include/pcl/registration/impl/meta_registration.hpp
+++ b/registration/include/pcl/registration/impl/meta_registration.hpp
@@ -38,70 +38,71 @@
 #ifndef PCL_REGISTRATION_IMPL_META_REGISTRATION_HPP_
 #define PCL_REGISTRATION_IMPL_META_REGISTRATION_HPP_
 
+namespace pcl {
 
-namespace pcl
-{
-
-namespace registration
-{
+namespace registration {
 
 template <typename PointT, typename Scalar>
-MetaRegistration<PointT, Scalar>::MetaRegistration () :
-  abs_transform_ (Matrix4::Identity ())
+MetaRegistration<PointT, Scalar>::MetaRegistration()
+: abs_transform_(Matrix4::Identity())
 {}
 
-template <typename PointT, typename Scalar> bool
-MetaRegistration<PointT, Scalar>::registerCloud (const PointCloudConstPtr& new_cloud, const Matrix4& delta_estimate)
+template <typename PointT, typename Scalar>
+bool
+MetaRegistration<PointT, Scalar>::registerCloud(const PointCloudConstPtr& new_cloud,
+                                                const Matrix4& delta_estimate)
 {
-  assert (registration_);
+  assert(registration_);
 
-  PointCloudPtr new_cloud_transformed (new pcl::PointCloud<PointT> ());
+  PointCloudPtr new_cloud_transformed(new pcl::PointCloud<PointT>());
 
-  if (!full_cloud_)
-  {
+  if (!full_cloud_) {
     pcl::transformPointCloud(*new_cloud, *new_cloud_transformed, delta_estimate);
     full_cloud_ = new_cloud_transformed;
     abs_transform_ = delta_estimate;
     return (true);
   }
 
-  registration_->setInputSource (new_cloud);
-  registration_->setInputTarget (full_cloud_);
+  registration_->setInputSource(new_cloud);
+  registration_->setInputTarget(full_cloud_);
 
-  registration_->align (*new_cloud_transformed, abs_transform_ * delta_estimate);
+  registration_->align(*new_cloud_transformed, abs_transform_ * delta_estimate);
 
-  bool converged = registration_->hasConverged ();
+  bool converged = registration_->hasConverged();
 
-  if (converged)
-  {
-    abs_transform_ = registration_->getFinalTransformation ();
+  if (converged) {
+    abs_transform_ = registration_->getFinalTransformation();
     *full_cloud_ += *new_cloud_transformed;
   }
 
   return (converged);
 }
 
-template <typename PointT, typename Scalar> inline typename MetaRegistration<PointT, Scalar>::Matrix4
-MetaRegistration<PointT, Scalar>::getAbsoluteTransform () const
+template <typename PointT, typename Scalar>
+inline typename MetaRegistration<PointT, Scalar>::Matrix4
+MetaRegistration<PointT, Scalar>::getAbsoluteTransform() const
 {
   return (abs_transform_);
 }
 
-template <typename PointT, typename Scalar> inline void
-MetaRegistration<PointT, Scalar>::reset ()
+template <typename PointT, typename Scalar>
+inline void
+MetaRegistration<PointT, Scalar>::reset()
 {
-  full_cloud_.reset ();
-  abs_transform_ = Matrix4::Identity ();
+  full_cloud_.reset();
+  abs_transform_ = Matrix4::Identity();
 }
 
-template <typename PointT, typename Scalar> inline void
-MetaRegistration<PointT, Scalar>::setRegistration (RegistrationPtr reg)
+template <typename PointT, typename Scalar>
+inline void
+MetaRegistration<PointT, Scalar>::setRegistration(RegistrationPtr reg)
 {
   registration_ = reg;
 }
 
-template <typename PointT, typename Scalar> inline typename MetaRegistration<PointT, Scalar>::PointCloudConstPtr
-MetaRegistration<PointT, Scalar>::getMetaCloud () const
+template <typename PointT, typename Scalar>
+inline typename MetaRegistration<PointT, Scalar>::PointCloudConstPtr
+MetaRegistration<PointT, Scalar>::getMetaCloud() const
 {
   return full_cloud_;
 }
@@ -110,4 +111,3 @@ MetaRegistration<PointT, Scalar>::getMetaCloud () const
 } // 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 bae45ab4..c6d65c75 100644
--- a/registration/include/pcl/registration/impl/ndt.hpp
+++ b/registration/include/pcl/registration/impl/ndt.hpp
@@ -41,302 +41,313 @@
 #ifndef PCL_REGISTRATION_NDT_IMPL_H_
 #define PCL_REGISTRATION_NDT_IMPL_H_
 
-
-namespace pcl
-{
-
-template<typename PointSource, typename PointTarget>
-NormalDistributionsTransform<PointSource, PointTarget>::NormalDistributionsTransform ()
-  : target_cells_ ()
-  , resolution_ (1.0f)
-  , step_size_ (0.1)
-  , outlier_ratio_ (0.55)
-  , gauss_d1_ ()
-  , gauss_d2_ ()
-  , trans_probability_ ()
+namespace pcl {
+
+template <typename PointSource, typename PointTarget>
+NormalDistributionsTransform<PointSource, PointTarget>::NormalDistributionsTransform()
+: target_cells_()
+, resolution_(1.0f)
+, step_size_(0.1)
+, outlier_ratio_(0.55)
+, gauss_d1_()
+, gauss_d2_()
+, trans_probability_()
 {
   reg_name_ = "NormalDistributionsTransform";
 
-  double gauss_c1, gauss_c2, gauss_d3;
-
   // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009]
-  gauss_c1 = 10.0 * (1 - outlier_ratio_);
-  gauss_c2 = outlier_ratio_ / pow (resolution_, 3);
-  gauss_d3 = -std::log (gauss_c2);
-  gauss_d1_ = -std::log ( gauss_c1 + gauss_c2 ) - gauss_d3;
-  gauss_d2_ = -2 * std::log ((-std::log ( gauss_c1 * std::exp ( -0.5 ) + gauss_c2 ) - gauss_d3) / gauss_d1_);
+  const double gauss_c1 = 10.0 * (1 - outlier_ratio_);
+  const double gauss_c2 = outlier_ratio_ / pow(resolution_, 3);
+  const double gauss_d3 = -std::log(gauss_c2);
+  gauss_d1_ = -std::log(gauss_c1 + gauss_c2) - gauss_d3;
+  gauss_d2_ =
+      -2 * std::log((-std::log(gauss_c1 * std::exp(-0.5) + gauss_c2) - gauss_d3) /
+                    gauss_d1_);
 
   transformation_epsilon_ = 0.1;
   max_iterations_ = 35;
 }
 
-
-template<typename PointSource, typename PointTarget> void
-NormalDistributionsTransform<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
+template <typename PointSource, typename PointTarget>
+void
+NormalDistributionsTransform<PointSource, PointTarget>::computeTransformation(
+    PointCloudSource& output, const Eigen::Matrix4f& guess)
 {
   nr_iterations_ = 0;
   converged_ = false;
 
-  double gauss_c1, gauss_c2, gauss_d3;
-
   // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009]
-  gauss_c1 = 10 * (1 - outlier_ratio_);
-  gauss_c2 = outlier_ratio_ / pow (resolution_, 3);
-  gauss_d3 = -std::log (gauss_c2);
-  gauss_d1_ = -std::log ( gauss_c1 + gauss_c2 ) - gauss_d3;
-  gauss_d2_ = -2 * std::log ((-std::log ( gauss_c1 * std::exp ( -0.5 ) + gauss_c2 ) - gauss_d3) / gauss_d1_);
-
-  if (guess != Eigen::Matrix4f::Identity ())
-  {
+  const double gauss_c1 = 10 * (1 - outlier_ratio_);
+  const double gauss_c2 = outlier_ratio_ / pow(resolution_, 3);
+  const double gauss_d3 = -std::log(gauss_c2);
+  gauss_d1_ = -std::log(gauss_c1 + gauss_c2) - gauss_d3;
+  gauss_d2_ =
+      -2 * std::log((-std::log(gauss_c1 * std::exp(-0.5) + gauss_c2) - gauss_d3) /
+                    gauss_d1_);
+
+  if (guess != Eigen::Matrix4f::Identity()) {
     // Initialise final transformation to the guessed one
     final_transformation_ = guess;
     // Apply guessed transformation prior to search for neighbours
-    transformPointCloud (output, output, guess);
+    transformPointCloud(output, output, guess);
   }
 
   // Initialize Point Gradient and Hessian
-  point_gradient_.setZero ();
-  point_gradient_.block<3, 3>(0, 0).setIdentity ();
-  point_hessian_.setZero ();
+  point_jacobian_.setZero();
+  point_jacobian_.block<3, 3>(0, 0).setIdentity();
+  point_hessian_.setZero();
 
   Eigen::Transform<float, 3, Eigen::Affine, Eigen::ColMajor> eig_transformation;
-  eig_transformation.matrix () = final_transformation_;
+  eig_transformation.matrix() = final_transformation_;
 
   // Convert initial guess matrix to 6 element transformation vector
-  Eigen::Matrix<double, 6, 1> p, delta_p, score_gradient;
-  Eigen::Vector3f init_translation = eig_transformation.translation ();
-  Eigen::Vector3f init_rotation = eig_transformation.rotation ().eulerAngles (0, 1, 2);
-  p << init_translation (0), init_translation (1), init_translation (2),
-  init_rotation (0), init_rotation (1), init_rotation (2);
+  Eigen::Matrix<double, 6, 1> transform, score_gradient;
+  Eigen::Vector3f init_translation = eig_transformation.translation();
+  Eigen::Vector3f init_rotation = eig_transformation.rotation().eulerAngles(0, 1, 2);
+  transform << init_translation.cast<double>(), init_rotation.cast<double>();
 
   Eigen::Matrix<double, 6, 6> hessian;
 
-  double score = 0;
-
-  // Calculate derivates of initial transform vector, subsequent derivative calculations are done in the step length determination.
-  score = computeDerivatives (score_gradient, hessian, output, p);
+  // Calculate derivates of initial transform vector, subsequent derivative calculations
+  // are done in the step length determination.
+  double score = computeDerivatives(score_gradient, hessian, output, transform);
 
-  while (!converged_)
-  {
+  while (!converged_) {
     // Store previous transformation
     previous_transformation_ = transformation_;
 
-    // Solve for decent direction using newton method, line 23 in Algorithm 2 [Magnusson 2009]
-    Eigen::JacobiSVD<Eigen::Matrix<double, 6, 6> > sv (hessian, Eigen::ComputeFullU | Eigen::ComputeFullV);
+    // Solve for decent direction using newton method, line 23 in Algorithm 2 [Magnusson
+    // 2009]
+    Eigen::JacobiSVD<Eigen::Matrix<double, 6, 6>> sv(
+        hessian, Eigen::ComputeFullU | Eigen::ComputeFullV);
     // Negative for maximization as opposed to minimization
-    delta_p = sv.solve (-score_gradient);
+    Eigen::Matrix<double, 6, 1> delta = sv.solve(-score_gradient);
 
-    //Calculate step length with guarnteed sufficient decrease [More, Thuente 1994]
-    double delta_p_norm = delta_p.norm ();
+    // Calculate step length with guarnteed sufficient decrease [More, Thuente 1994]
+    double delta_norm = delta.norm();
 
-    if (delta_p_norm == 0 || std::isnan(delta_p_norm))
-    {
-      trans_probability_ = score / static_cast<double> (input_->size ());
-      converged_ = delta_p_norm == delta_p_norm;
+    if (delta_norm == 0 || std::isnan(delta_norm)) {
+      trans_probability_ = score / static_cast<double>(input_->size());
+      converged_ = delta_norm == 0;
       return;
     }
 
-    delta_p.normalize ();
-    delta_p_norm = computeStepLengthMT (p, delta_p, delta_p_norm, step_size_, transformation_epsilon_ / 2, score, score_gradient, hessian, output);
-    delta_p *= delta_p_norm;
-
-
-    transformation_ = (Eigen::Translation<float, 3> (static_cast<float> (delta_p (0)), static_cast<float> (delta_p (1)), static_cast<float> (delta_p (2))) *
-                       Eigen::AngleAxis<float> (static_cast<float> (delta_p (3)), Eigen::Vector3f::UnitX ()) *
-                       Eigen::AngleAxis<float> (static_cast<float> (delta_p (4)), Eigen::Vector3f::UnitY ()) *
-                       Eigen::AngleAxis<float> (static_cast<float> (delta_p (5)), Eigen::Vector3f::UnitZ ())).matrix ();
+    delta /= delta_norm;
+    delta_norm = computeStepLengthMT(transform,
+                                     delta,
+                                     delta_norm,
+                                     step_size_,
+                                     transformation_epsilon_ / 2,
+                                     score,
+                                     score_gradient,
+                                     hessian,
+                                     output);
+    delta *= delta_norm;
 
+    // Convert delta into matrix form
+    convertTransform(delta, transformation_);
 
-    p += delta_p;
+    transform += delta;
 
     // Update Visualizer (untested)
     if (update_visualizer_)
-      update_visualizer_ (output, std::vector<int>(), *target_, std::vector<int>() );
+      update_visualizer_(output, pcl::Indices(), *target_, pcl::Indices());
 
-    double cos_angle = 0.5 * (transformation_.coeff (0, 0) + transformation_.coeff (1, 1) + transformation_.coeff (2, 2) - 1);
-    double translation_sqr = transformation_.coeff (0, 3) * transformation_.coeff (0, 3) +
-                             transformation_.coeff (1, 3) * transformation_.coeff (1, 3) +
-                             transformation_.coeff (2, 3) * transformation_.coeff (2, 3);
+    const double cos_angle =
+        0.5 * transformation_.template block<3, 3>(0, 0).trace() - 1;
+    const double translation_sqr =
+        transformation_.template block<3, 1>(0, 3).squaredNorm();
 
     nr_iterations_++;
 
     if (nr_iterations_ >= max_iterations_ ||
-        ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) && (transformation_rotation_epsilon_ > 0 && cos_angle >= transformation_rotation_epsilon_)) ||
-        ((transformation_epsilon_ <= 0)                                             && (transformation_rotation_epsilon_ > 0 && cos_angle >= transformation_rotation_epsilon_)) ||
-        ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) && (transformation_rotation_epsilon_ <= 0)))
-    {
+        ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) &&
+         (transformation_rotation_epsilon_ > 0 &&
+          cos_angle >= transformation_rotation_epsilon_)) ||
+        ((transformation_epsilon_ <= 0) &&
+         (transformation_rotation_epsilon_ > 0 &&
+          cos_angle >= transformation_rotation_epsilon_)) ||
+        ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) &&
+         (transformation_rotation_epsilon_ <= 0))) {
       converged_ = true;
     }
   }
 
-  // Store transformation probability.  The realtive differences within each scan registration are accurate
-  // but the normalization constants need to be modified for it to be globally accurate
-  trans_probability_ = score / static_cast<double> (input_->size ());
+  // Store transformation probability.  The realtive differences within each scan
+  // registration are accurate but the normalization constants need to be modified for
+  // it to be globally accurate
+  trans_probability_ = score / static_cast<double>(input_->size());
 }
 
-
-template<typename PointSource, typename PointTarget> double
-NormalDistributionsTransform<PointSource, PointTarget>::computeDerivatives (Eigen::Matrix<double, 6, 1> &score_gradient,
-                                                                            Eigen::Matrix<double, 6, 6> &hessian,
-                                                                            PointCloudSource &trans_cloud,
-                                                                            Eigen::Matrix<double, 6, 1> &p,
-                                                                            bool compute_hessian)
+template <typename PointSource, typename PointTarget>
+double
+NormalDistributionsTransform<PointSource, PointTarget>::computeDerivatives(
+    Eigen::Matrix<double, 6, 1>& score_gradient,
+    Eigen::Matrix<double, 6, 6>& hessian,
+    const PointCloudSource& trans_cloud,
+    const Eigen::Matrix<double, 6, 1>& transform,
+    bool compute_hessian)
 {
-  // Original Point and Transformed Point
-  PointSource x_pt, x_trans_pt;
-  // Original Point and Transformed Point (for math)
-  Eigen::Vector3d x, x_trans;
-  // Occupied Voxel
-  TargetGridLeafConstPtr cell;
-  // Inverse Covariance of Occupied Voxel
-  Eigen::Matrix3d c_inv;
-
-  score_gradient.setZero ();
-  hessian.setZero ();
+  score_gradient.setZero();
+  hessian.setZero();
   double score = 0;
 
   // Precompute Angular Derivatives (eq. 6.19 and 6.21)[Magnusson 2009]
-  computeAngleDerivatives (p);
+  computeAngleDerivatives(transform);
 
   // Update gradient and hessian for each point, line 17 in Algorithm 2 [Magnusson 2009]
-  for (std::size_t idx = 0; idx < input_->size (); idx++)
-  {
-    x_trans_pt = trans_cloud[idx];
+  for (std::size_t idx = 0; idx < input_->size(); idx++) {
+    // Transformed Point
+    const auto& x_trans_pt = trans_cloud[idx];
 
-    // Find nieghbors (Radius search has been experimentally faster than direct neighbor checking.
+    // Find neighbors (Radius search has been experimentally faster than direct neighbor
+    // checking.
     std::vector<TargetGridLeafConstPtr> neighborhood;
     std::vector<float> distances;
-    target_cells_.radiusSearch (x_trans_pt, resolution_, neighborhood, distances);
-
-    for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); ++neighborhood_it)
-    {
-      cell = *neighborhood_it;
-      x_pt = (*input_)[idx];
-      x = Eigen::Vector3d (x_pt.x, x_pt.y, x_pt.z);
+    target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances);
 
-      x_trans = Eigen::Vector3d (x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
+    for (const auto& cell : neighborhood) {
+      // Original Point
+      const auto& x_pt = (*input_)[idx];
+      const Eigen::Vector3d x = x_pt.getVector3fMap().template cast<double>();
 
       // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
-      x_trans -= cell->getMean ();
+      const Eigen::Vector3d x_trans =
+          x_trans_pt.getVector3fMap().template cast<double>() - cell->getMean();
+      // Inverse Covariance of Occupied Voxel
       // Uses precomputed covariance for speed.
-      c_inv = cell->getInverseCov ();
-
-      // Compute derivative of transform function w.r.t. transform vector, J_E and H_E in Equations 6.18 and 6.20 [Magnusson 2009]
-      computePointDerivatives (x);
-      // Update score, gradient and hessian, lines 19-21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13, respectively [Magnusson 2009]
-      score += updateDerivatives (score_gradient, hessian, x_trans, c_inv, compute_hessian);
-
+      const Eigen::Matrix3d c_inv = cell->getInverseCov();
+
+      // Compute derivative of transform function w.r.t. transform vector, J_E and H_E
+      // in Equations 6.18 and 6.20 [Magnusson 2009]
+      computePointDerivatives(x);
+      // Update score, gradient and hessian, lines 19-21 in Algorithm 2, according to
+      // Equations 6.10, 6.12 and 6.13, respectively [Magnusson 2009]
+      score +=
+          updateDerivatives(score_gradient, hessian, x_trans, c_inv, compute_hessian);
     }
   }
-  return (score);
+  return score;
 }
 
-
-template<typename PointSource, typename PointTarget> void
-NormalDistributionsTransform<PointSource, PointTarget>::computeAngleDerivatives (Eigen::Matrix<double, 6, 1> &p, bool compute_hessian)
+template <typename PointSource, typename PointTarget>
+void
+NormalDistributionsTransform<PointSource, PointTarget>::computeAngleDerivatives(
+    const Eigen::Matrix<double, 6, 1>& transform, bool compute_hessian)
 {
   // Simplified math for near 0 angles
-  double cx, cy, cz, sx, sy, sz;
-  if (std::abs (p (3)) < 10e-5)
-  {
-    //p(3) = 0;
-    cx = 1.0;
-    sx = 0.0;
-  }
-  else
-  {
-    cx = std::cos (p (3));
-    sx = sin (p (3));
-  }
-  if (std::abs (p (4)) < 10e-5)
-  {
-    //p(4) = 0;
-    cy = 1.0;
-    sy = 0.0;
-  }
-  else
-  {
-    cy = std::cos (p (4));
-    sy = sin (p (4));
-  }
-
-  if (std::abs (p (5)) < 10e-5)
-  {
-    //p(5) = 0;
-    cz = 1.0;
-    sz = 0.0;
-  }
-  else
-  {
-    cz = std::cos (p (5));
-    sz = sin (p (5));
-  }
+  const auto calculate_cos_sin = [](double angle, double& c, double& s) {
+    if (std::abs(angle) < 10e-5) {
+      c = 1.0;
+      s = 0.0;
+    }
+    else {
+      c = std::cos(angle);
+      s = std::sin(angle);
+    }
+  };
 
-  // Precomputed angular gradiant components. Letters correspond to Equation 6.19 [Magnusson 2009]
-  j_ang_a_ << (-sx * sz + cx * sy * cz), (-sx * cz - cx * sy * sz), (-cx * cy);
-  j_ang_b_ << (cx * sz + sx * sy * cz), (cx * cz - sx * sy * sz), (-sx * cy);
-  j_ang_c_ << (-sy * cz), sy * sz, cy;
-  j_ang_d_ << sx * cy * cz, (-sx * cy * sz), sx * sy;
-  j_ang_e_ << (-cx * cy * cz), cx * cy * sz, (-cx * sy);
-  j_ang_f_ << (-cy * sz), (-cy * cz), 0;
-  j_ang_g_ << (cx * cz - sx * sy * sz), (-cx * sz - sx * sy * cz), 0;
-  j_ang_h_ << (sx * cz + cx * sy * sz), (cx * sy * cz - sx * sz), 0;
-
-  if (compute_hessian)
-  {
-    // Precomputed angular hessian components. Letters correspond to Equation 6.21 and numbers correspond to row index [Magnusson 2009]
-    h_ang_a2_ << (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), sx * cy;
-    h_ang_a3_ << (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), (-cx * cy);
-
-    h_ang_b2_ << (cx * cy * cz), (-cx * cy * sz), (cx * sy);
-    h_ang_b3_ << (sx * cy * cz), (-sx * cy * sz), (sx * sy);
-
-    h_ang_c2_ << (-sx * cz - cx * sy * sz), (sx * sz - cx * sy * cz), 0;
-    h_ang_c3_ << (cx * cz - sx * sy * sz), (-sx * sy * cz - cx * sz), 0;
-
-    h_ang_d1_ << (-cy * cz), (cy * sz), (sy);
-    h_ang_d2_ << (-sx * sy * cz), (sx * sy * sz), (sx * cy);
-    h_ang_d3_ << (cx * sy * cz), (-cx * sy * sz), (-cx * cy);
-
-    h_ang_e1_ << (sy * sz), (sy * cz), 0;
-    h_ang_e2_ << (-sx * cy * sz), (-sx * cy * cz), 0;
-    h_ang_e3_ << (cx * cy * sz), (cx * cy * cz), 0;
-
-    h_ang_f1_ << (-cy * cz), (cy * sz), 0;
-    h_ang_f2_ << (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), 0;
-    h_ang_f3_ << (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), 0;
+  double cx, cy, cz, sx, sy, sz;
+  calculate_cos_sin(transform(3), cx, sx);
+  calculate_cos_sin(transform(4), cy, sy);
+  calculate_cos_sin(transform(5), cz, sz);
+
+  // Precomputed angular gradient components. Letters correspond to Equation 6.19
+  // [Magnusson 2009]
+  angular_jacobian_.setZero();
+  angular_jacobian_.row(0).noalias() = Eigen::Vector4d(
+      (-sx * sz + cx * sy * cz), (-sx * cz - cx * sy * sz), (-cx * cy), 1.0); // a
+  angular_jacobian_.row(1).noalias() = Eigen::Vector4d(
+      (cx * sz + sx * sy * cz), (cx * cz - sx * sy * sz), (-sx * cy), 1.0); // b
+  angular_jacobian_.row(2).noalias() =
+      Eigen::Vector4d((-sy * cz), sy * sz, cy, 1.0); // c
+  angular_jacobian_.row(3).noalias() =
+      Eigen::Vector4d(sx * cy * cz, (-sx * cy * sz), sx * sy, 1.0); // d
+  angular_jacobian_.row(4).noalias() =
+      Eigen::Vector4d((-cx * cy * cz), cx * cy * sz, (-cx * sy), 1.0); // e
+  angular_jacobian_.row(5).noalias() =
+      Eigen::Vector4d((-cy * sz), (-cy * cz), 0, 1.0); // f
+  angular_jacobian_.row(6).noalias() =
+      Eigen::Vector4d((cx * cz - sx * sy * sz), (-cx * sz - sx * sy * cz), 0, 1.0); // g
+  angular_jacobian_.row(7).noalias() =
+      Eigen::Vector4d((sx * cz + cx * sy * sz), (cx * sy * cz - sx * sz), 0, 1.0); // h
+
+  if (compute_hessian) {
+    // Precomputed angular hessian components. Letters correspond to Equation 6.21 and
+    // numbers correspond to row index [Magnusson 2009]
+    angular_hessian_.setZero();
+    angular_hessian_.row(0).noalias() = Eigen::Vector4d(
+        (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), sx * cy, 0.0f); // a2
+    angular_hessian_.row(1).noalias() = Eigen::Vector4d(
+        (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), (-cx * cy), 0.0f); // a3
+
+    angular_hessian_.row(2).noalias() =
+        Eigen::Vector4d((cx * cy * cz), (-cx * cy * sz), (cx * sy), 0.0f); // b2
+    angular_hessian_.row(3).noalias() =
+        Eigen::Vector4d((sx * cy * cz), (-sx * cy * sz), (sx * sy), 0.0f); // b3
+
+    angular_hessian_.row(4).noalias() = Eigen::Vector4d(
+        (-sx * cz - cx * sy * sz), (sx * sz - cx * sy * cz), 0, 0.0f); // c2
+    angular_hessian_.row(5).noalias() = Eigen::Vector4d(
+        (cx * cz - sx * sy * sz), (-sx * sy * cz - cx * sz), 0, 0.0f); // c3
+
+    angular_hessian_.row(6).noalias() =
+        Eigen::Vector4d((-cy * cz), (cy * sz), (sy), 0.0f); // d1
+    angular_hessian_.row(7).noalias() =
+        Eigen::Vector4d((-sx * sy * cz), (sx * sy * sz), (sx * cy), 0.0f); // d2
+    angular_hessian_.row(8).noalias() =
+        Eigen::Vector4d((cx * sy * cz), (-cx * sy * sz), (-cx * cy), 0.0f); // d3
+
+    angular_hessian_.row(9).noalias() =
+        Eigen::Vector4d((sy * sz), (sy * cz), 0, 0.0f); // e1
+    angular_hessian_.row(10).noalias() =
+        Eigen::Vector4d((-sx * cy * sz), (-sx * cy * cz), 0, 0.0f); // e2
+    angular_hessian_.row(11).noalias() =
+        Eigen::Vector4d((cx * cy * sz), (cx * cy * cz), 0, 0.0f); // e3
+
+    angular_hessian_.row(12).noalias() =
+        Eigen::Vector4d((-cy * cz), (cy * sz), 0, 0.0f); // f1
+    angular_hessian_.row(13).noalias() = Eigen::Vector4d(
+        (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), 0, 0.0f); // f2
+    angular_hessian_.row(14).noalias() = Eigen::Vector4d(
+        (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), 0, 0.0f); // f3
   }
 }
 
-
-template<typename PointSource, typename PointTarget> void
-NormalDistributionsTransform<PointSource, PointTarget>::computePointDerivatives (Eigen::Vector3d &x, bool compute_hessian)
+template <typename PointSource, typename PointTarget>
+void
+NormalDistributionsTransform<PointSource, PointTarget>::computePointDerivatives(
+    const 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]
-  point_gradient_ (1, 3) = x.dot (j_ang_a_);
-  point_gradient_ (2, 3) = x.dot (j_ang_b_);
-  point_gradient_ (0, 4) = x.dot (j_ang_c_);
-  point_gradient_ (1, 4) = x.dot (j_ang_d_);
-  point_gradient_ (2, 4) = x.dot (j_ang_e_);
-  point_gradient_ (0, 5) = x.dot (j_ang_f_);
-  point_gradient_ (1, 5) = x.dot (j_ang_g_);
-  point_gradient_ (2, 5) = x.dot (j_ang_h_);
-
-  if (compute_hessian)
-  {
-    // Vectors from Equation 6.21 [Magnusson 2009]
-    Eigen::Vector3d a, b, c, d, e, f;
-
-    a << 0, x.dot (h_ang_a2_), x.dot (h_ang_a3_);
-    b << 0, x.dot (h_ang_b2_), x.dot (h_ang_b3_);
-    c << 0, x.dot (h_ang_c2_), x.dot (h_ang_c3_);
-    d << x.dot (h_ang_d1_), x.dot (h_ang_d2_), x.dot (h_ang_d3_);
-    e << x.dot (h_ang_e1_), x.dot (h_ang_e2_), x.dot (h_ang_e3_);
-    f << x.dot (h_ang_f1_), x.dot (h_ang_f2_), x.dot (h_ang_f3_);
+  // Calculate first derivative of Transformation Equation 6.17 w.r.t. transform vector.
+  // Derivative w.r.t. ith element of transform vector corresponds to column i,
+  // Equation 6.18 and 6.19 [Magnusson 2009]
+  Eigen::Matrix<double, 8, 1> point_angular_jacobian =
+      angular_jacobian_ * Eigen::Vector4d(x[0], x[1], x[2], 0.0);
+  point_jacobian_(1, 3) = point_angular_jacobian[0];
+  point_jacobian_(2, 3) = point_angular_jacobian[1];
+  point_jacobian_(0, 4) = point_angular_jacobian[2];
+  point_jacobian_(1, 4) = point_angular_jacobian[3];
+  point_jacobian_(2, 4) = point_angular_jacobian[4];
+  point_jacobian_(0, 5) = point_angular_jacobian[5];
+  point_jacobian_(1, 5) = point_angular_jacobian[6];
+  point_jacobian_(2, 5) = point_angular_jacobian[7];
+
+  if (compute_hessian) {
+    Eigen::Matrix<double, 15, 1> point_angular_hessian =
+        angular_hessian_ * Eigen::Vector4d(x[0], x[1], x[2], 0.0);
 
-    // Calculate second derivative of Transformation Equation 6.17 w.r.t. transform vector p.
-    // Derivative w.r.t. ith and jth elements of transform vector corresponds to the 3x1 block matrix starting at (3i,j), Equation 6.20 and 6.21 [Magnusson 2009]
+    // Vectors from Equation 6.21 [Magnusson 2009]
+    const Eigen::Vector3d a(0, point_angular_hessian[0], point_angular_hessian[1]);
+    const Eigen::Vector3d b(0, point_angular_hessian[2], point_angular_hessian[3]);
+    const Eigen::Vector3d c(0, point_angular_hessian[4], point_angular_hessian[5]);
+    const Eigen::Vector3d d = point_angular_hessian.block<3, 1>(6, 0);
+    const Eigen::Vector3d e = point_angular_hessian.block<3, 1>(9, 0);
+    const Eigen::Vector3d f = point_angular_hessian.block<3, 1>(12, 0);
+
+    // Calculate second derivative of Transformation Equation 6.17 w.r.t. transform
+    // vector. Derivative w.r.t. ith and jth elements of transform vector corresponds to
+    // the 3x1 block matrix starting at (3i,j), Equation 6.20 and 6.21 [Magnusson 2009]
     point_hessian_.block<3, 1>(9, 3) = a;
     point_hessian_.block<3, 1>(12, 3) = b;
     point_hessian_.block<3, 1>(15, 3) = c;
@@ -349,160 +360,163 @@ NormalDistributionsTransform<PointSource, PointTarget>::computePointDerivatives
   }
 }
 
-
-template<typename PointSource, typename PointTarget> double
-NormalDistributionsTransform<PointSource, PointTarget>::updateDerivatives (Eigen::Matrix<double, 6, 1> &score_gradient,
-                                                                           Eigen::Matrix<double, 6, 6> &hessian,
-                                                                           Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv,
-                                                                           bool compute_hessian)
+template <typename PointSource, typename PointTarget>
+double
+NormalDistributionsTransform<PointSource, PointTarget>::updateDerivatives(
+    Eigen::Matrix<double, 6, 1>& score_gradient,
+    Eigen::Matrix<double, 6, 6>& hessian,
+    const Eigen::Vector3d& x_trans,
+    const Eigen::Matrix3d& c_inv,
+    bool compute_hessian) const
 {
-  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]
-  double e_x_cov_x = std::exp (-gauss_d2_ * x_trans.dot (c_inv * x_trans) / 2);
-  // Calculate probability of transformed points existence, Equation 6.9 [Magnusson 2009]
-  double score_inc = -gauss_d1_ * e_x_cov_x;
+  double e_x_cov_x = std::exp(-gauss_d2_ * x_trans.dot(c_inv * x_trans) / 2);
+  // Calculate probability of transformed points existence, Equation 6.9 [Magnusson
+  // 2009]
+  const double score_inc = -gauss_d1_ * e_x_cov_x;
 
   e_x_cov_x = gauss_d2_ * e_x_cov_x;
 
   // Error checking for invalid values.
-  if (e_x_cov_x > 1 || e_x_cov_x < 0 || std::isnan(e_x_cov_x))
-    return (0);
+  if (e_x_cov_x > 1 || e_x_cov_x < 0 || std::isnan(e_x_cov_x)) {
+    return 0;
+  }
 
   // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009]
   e_x_cov_x *= gauss_d1_;
 
-
-  for (int i = 0; i < 6; i++)
-  {
-    // Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009]
-    cov_dxd_pi = c_inv * point_gradient_.col (i);
+  for (int i = 0; i < 6; i++) {
+    // Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13 [Magnusson
+    // 2009]
+    const Eigen::Vector3d cov_dxd_pi = c_inv * point_jacobian_.col(i);
 
     // Update gradient, Equation 6.12 [Magnusson 2009]
-    score_gradient (i) += x_trans.dot (cov_dxd_pi) * e_x_cov_x;
+    score_gradient(i) += x_trans.dot(cov_dxd_pi) * e_x_cov_x;
 
-    if (compute_hessian)
-    {
-      for (Eigen::Index j = 0; j < hessian.cols (); j++)
-      {
+    if (compute_hessian) {
+      for (Eigen::Index j = 0; j < hessian.cols(); j++) {
         // Update hessian, Equation 6.13 [Magnusson 2009]
-        hessian (i, j) += e_x_cov_x * (-gauss_d2_ * x_trans.dot (cov_dxd_pi) * x_trans.dot (c_inv * point_gradient_.col (j)) +
-                                    x_trans.dot (c_inv * point_hessian_.block<3, 1>(3 * i, j)) +
-                                    point_gradient_.col (j).dot (cov_dxd_pi) );
+        hessian(i, j) +=
+            e_x_cov_x * (-gauss_d2_ * x_trans.dot(cov_dxd_pi) *
+                             x_trans.dot(c_inv * point_jacobian_.col(j)) +
+                         x_trans.dot(c_inv * point_hessian_.block<3, 1>(3 * i, j)) +
+                         point_jacobian_.col(j).dot(cov_dxd_pi));
       }
     }
   }
 
-  return (score_inc);
+  return score_inc;
 }
 
-
-template<typename PointSource, typename PointTarget> void
-NormalDistributionsTransform<PointSource, PointTarget>::computeHessian (Eigen::Matrix<double, 6, 6> &hessian,
-                                                                        PointCloudSource &trans_cloud, Eigen::Matrix<double, 6, 1> &)
+template <typename PointSource, typename PointTarget>
+void
+NormalDistributionsTransform<PointSource, PointTarget>::computeHessian(
+    Eigen::Matrix<double, 6, 6>& hessian, const PointCloudSource& trans_cloud)
 {
-  // Original Point and Transformed Point
-  PointSource x_pt, x_trans_pt;
-  // Original Point and Transformed Point (for math)
-  Eigen::Vector3d x, x_trans;
-  // Occupied Voxel
-  TargetGridLeafConstPtr cell;
-  // Inverse Covariance of Occupied Voxel
-  Eigen::Matrix3d c_inv;
+  hessian.setZero();
 
-  hessian.setZero ();
+  // Precompute Angular Derivatives unessisary because only used after regular
+  // derivative calculation Update hessian for each point, line 17 in Algorithm 2
+  // [Magnusson 2009]
+  for (std::size_t idx = 0; idx < input_->size(); idx++) {
+    // Transformed Point
+    const auto& x_trans_pt = trans_cloud[idx];
 
-  // Precompute Angular Derivatives unessisary because only used after regular derivative calculation
-
-  // Update hessian for each point, line 17 in Algorithm 2 [Magnusson 2009]
-  for (std::size_t idx = 0; idx < input_->size (); idx++)
-  {
-    x_trans_pt = trans_cloud[idx];
-
-    // Find nieghbors (Radius search has been experimentally faster than direct neighbor checking.
+    // Find nieghbors (Radius search has been experimentally faster than direct neighbor
+    // checking.
     std::vector<TargetGridLeafConstPtr> neighborhood;
     std::vector<float> distances;
-    target_cells_.radiusSearch (x_trans_pt, resolution_, neighborhood, distances);
-
-    for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); ++neighborhood_it)
-    {
-      cell = *neighborhood_it;
-
-      {
-        x_pt = (*input_)[idx];
-        x = Eigen::Vector3d (x_pt.x, x_pt.y, x_pt.z);
+    target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances);
 
-        x_trans = Eigen::Vector3d (x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
+    for (const auto& cell : neighborhood) {
+      // Original Point
+      const auto& x_pt = (*input_)[idx];
+      const Eigen::Vector3d x = x_pt.getVector3fMap().template cast<double>();
 
-        // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
-        x_trans -= cell->getMean ();
-        // Uses precomputed covariance for speed.
-        c_inv = cell->getInverseCov ();
-
-        // Compute derivative of transform function w.r.t. transform vector, J_E and H_E in Equations 6.18 and 6.20 [Magnusson 2009]
-        computePointDerivatives (x);
-        // Update hessian, lines 21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13, respectively [Magnusson 2009]
-        updateHessian (hessian, x_trans, c_inv);
-      }
+      // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
+      const Eigen::Vector3d x_trans =
+          x_trans_pt.getVector3fMap().template cast<double>() - cell->getMean();
+      // Inverse Covariance of Occupied Voxel
+      // Uses precomputed covariance for speed.
+      const Eigen::Matrix3d c_inv = cell->getInverseCov();
+
+      // Compute derivative of transform function w.r.t. transform vector, J_E and H_E
+      // in Equations 6.18 and 6.20 [Magnusson 2009]
+      computePointDerivatives(x);
+      // Update hessian, lines 21 in Algorithm 2, according to Equations 6.10, 6.12
+      // and 6.13, respectively [Magnusson 2009]
+      updateHessian(hessian, x_trans, c_inv);
     }
   }
 }
 
-
-template<typename PointSource, typename PointTarget> void
-NormalDistributionsTransform<PointSource, PointTarget>::updateHessian (Eigen::Matrix<double, 6, 6> &hessian, Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv)
+template <typename PointSource, typename PointTarget>
+void
+NormalDistributionsTransform<PointSource, PointTarget>::updateHessian(
+    Eigen::Matrix<double, 6, 6>& hessian,
+    const Eigen::Vector3d& x_trans,
+    const Eigen::Matrix3d& c_inv) const
 {
-  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]
-  double e_x_cov_x = gauss_d2_ * std::exp (-gauss_d2_ * x_trans.dot (c_inv * x_trans) / 2);
+  double e_x_cov_x =
+      gauss_d2_ * std::exp(-gauss_d2_ * x_trans.dot(c_inv * x_trans) / 2);
 
   // Error checking for invalid values.
-  if (e_x_cov_x > 1 || e_x_cov_x < 0 || std::isnan(e_x_cov_x))
+  if (e_x_cov_x > 1 || e_x_cov_x < 0 || std::isnan(e_x_cov_x)) {
     return;
+  }
 
   // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009]
   e_x_cov_x *= gauss_d1_;
 
-  for (int i = 0; i < 6; i++)
-  {
-    // Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009]
-    cov_dxd_pi = c_inv * point_gradient_.col (i);
+  for (int i = 0; i < 6; i++) {
+    // Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13 [Magnusson
+    // 2009]
+    const Eigen::Vector3d cov_dxd_pi = c_inv * point_jacobian_.col(i);
 
-    for (Eigen::Index j = 0; j < hessian.cols (); j++)
-    {
+    for (Eigen::Index j = 0; j < hessian.cols(); j++) {
       // Update hessian, Equation 6.13 [Magnusson 2009]
-      hessian (i, j) += e_x_cov_x * (-gauss_d2_ * x_trans.dot (cov_dxd_pi) * x_trans.dot (c_inv * point_gradient_.col (j)) +
-                                  x_trans.dot (c_inv * point_hessian_.block<3, 1>(3 * i, j)) +
-                                  point_gradient_.col (j).dot (cov_dxd_pi) );
+      hessian(i, j) +=
+          e_x_cov_x * (-gauss_d2_ * x_trans.dot(cov_dxd_pi) *
+                           x_trans.dot(c_inv * point_jacobian_.col(j)) +
+                       x_trans.dot(c_inv * point_hessian_.block<3, 1>(3 * i, j)) +
+                       point_jacobian_.col(j).dot(cov_dxd_pi));
     }
   }
-
 }
 
-
-template<typename PointSource, typename PointTarget> bool
-NormalDistributionsTransform<PointSource, PointTarget>::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)
+template <typename PointSource, typename PointTarget>
+bool
+NormalDistributionsTransform<PointSource, PointTarget>::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) const
 {
-  // Case U1 in Update Algorithm and Case a in Modified Update Algorithm [More, Thuente 1994]
-  if (f_t > f_l)
-  {
+  // Case U1 in Update Algorithm and Case a in Modified Update Algorithm [More, Thuente
+  // 1994]
+  if (f_t > f_l) {
     a_u = a_t;
     f_u = f_t;
     g_u = g_t;
-    return (false);
+    return false;
   }
-  // Case U2 in Update Algorithm and Case b in Modified Update Algorithm [More, Thuente 1994]
-  if (g_t * (a_l - a_t) > 0)
-  {
+  // Case U2 in Update Algorithm and Case b in Modified Update Algorithm [More, Thuente
+  // 1994]
+  if (g_t * (a_l - a_t) > 0) {
     a_l = a_t;
     f_l = f_t;
     g_l = g_t;
-    return (false);
+    return false;
   }
-  // Case U3 in Update Algorithm and Case c in Modified Update Algorithm [More, Thuente 1994]
-  if (g_t * (a_l - a_t) < 0)
-  {
+  // Case U3 in Update Algorithm and Case c in Modified Update Algorithm [More, Thuente
+  // 1994]
+  if (g_t * (a_l - a_t) < 0) {
     a_u = a_l;
     f_u = f_l;
     g_u = g_l;
@@ -510,209 +524,247 @@ NormalDistributionsTransform<PointSource, PointTarget>::updateIntervalMT (double
     a_l = a_t;
     f_l = f_t;
     g_l = g_t;
-    return (false);
+    return false;
   }
   // Interval Converged
-  return (true);
+  return true;
 }
 
-
-template<typename PointSource, typename PointTarget> double
-NormalDistributionsTransform<PointSource, PointTarget>::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)
+template <typename PointSource, typename PointTarget>
+double
+NormalDistributionsTransform<PointSource, PointTarget>::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) const
 {
-  // Case 1 in Trial Value Selection [More, Thuente 1994]
-  if (f_t > f_l)
-  {
+  if (a_t == a_l && a_t == a_u) {
+    return a_t;
+  }
+
+  // Endpoints condition check [More, Thuente 1994], p.299 - 300
+  enum class EndpointsCondition { Case1, Case2, Case3, Case4 };
+  EndpointsCondition condition;
+
+  if (a_t == a_l) {
+    condition = EndpointsCondition::Case4;
+  }
+  else if (f_t > f_l) {
+    condition = EndpointsCondition::Case1;
+  }
+  else if (g_t * g_l < 0) {
+    condition = EndpointsCondition::Case2;
+  }
+  else if (std::fabs(g_t) <= std::fabs(g_l)) {
+    condition = EndpointsCondition::Case3;
+  }
+  else {
+    condition = EndpointsCondition::Case4;
+  }
+
+  switch (condition) {
+  case EndpointsCondition::Case1: {
     // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t
     // Equation 2.4.52 [Sun, Yuan 2006]
-    double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
-    double w = std::sqrt (z * z - g_t * g_l);
+    const double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
+    const double w = std::sqrt(z * z - g_t * g_l);
     // Equation 2.4.56 [Sun, Yuan 2006]
-    double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
+    const double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
 
     // Calculate the minimizer of the quadratic that interpolates f_l, f_t and g_l
     // Equation 2.4.2 [Sun, Yuan 2006]
-    double a_q = a_l - 0.5 * (a_l - a_t) * g_l / (g_l - (f_l - f_t) / (a_l - a_t));
+    const double a_q =
+        a_l - 0.5 * (a_l - a_t) * g_l / (g_l - (f_l - f_t) / (a_l - a_t));
 
-    if (std::fabs (a_c - a_l) < std::fabs (a_q - a_l))
-      return (a_c);
-    return (0.5 * (a_q + a_c));
+    if (std::fabs(a_c - a_l) < std::fabs(a_q - a_l)) {
+      return a_c;
+    }
+    return 0.5 * (a_q + a_c);
   }
-  // Case 2 in Trial Value Selection [More, Thuente 1994]
-  if (g_t * g_l < 0)
-  {
+
+  case EndpointsCondition::Case2: {
     // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t
     // Equation 2.4.52 [Sun, Yuan 2006]
-    double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
-    double w = std::sqrt (z * z - g_t * g_l);
+    const double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
+    const double w = std::sqrt(z * z - g_t * g_l);
     // Equation 2.4.56 [Sun, Yuan 2006]
-    double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
+    const double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
 
     // Calculate the minimizer of the quadratic that interpolates f_l, g_l and g_t
     // Equation 2.4.5 [Sun, Yuan 2006]
-    double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l;
+    const double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l;
 
-    if (std::fabs (a_c - a_t) >= std::fabs (a_s - a_t))
-      return (a_c);
-    return (a_s);
+    if (std::fabs(a_c - a_t) >= std::fabs(a_s - a_t)) {
+      return a_c;
+    }
+    return a_s;
   }
-  // Case 3 in Trial Value Selection [More, Thuente 1994]
-  if (std::fabs (g_t) <= std::fabs (g_l))
-  {
+
+  case EndpointsCondition::Case3: {
     // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t
     // Equation 2.4.52 [Sun, Yuan 2006]
-    double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
-    double w = std::sqrt (z * z - g_t * g_l);
-    double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
+    const double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
+    const double w = std::sqrt(z * z - g_t * g_l);
+    const double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
 
     // Calculate the minimizer of the quadratic that interpolates g_l and g_t
     // Equation 2.4.5 [Sun, Yuan 2006]
-    double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l;
+    const double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l;
 
     double a_t_next;
 
-    if (std::fabs (a_c - a_t) < std::fabs (a_s - a_t))
+    if (std::fabs(a_c - a_t) < std::fabs(a_s - a_t)) {
       a_t_next = a_c;
-    else
+    }
+    else {
       a_t_next = a_s;
+    }
 
-    if (a_t > a_l)
-      return (std::min (a_t + 0.66 * (a_u - a_t), a_t_next));
-    return (std::max (a_t + 0.66 * (a_u - a_t), a_t_next));
+    if (a_t > a_l) {
+      return std::min(a_t + 0.66 * (a_u - a_t), a_t_next);
+    }
+    return std::max(a_t + 0.66 * (a_u - a_t), a_t_next);
   }
-  // Case 4 in Trial Value Selection [More, Thuente 1994]
-  // Calculate the minimizer of the cubic that interpolates f_u, f_t, g_u and g_t
-  // Equation 2.4.52 [Sun, Yuan 2006]
-  double z = 3 * (f_t - f_u) / (a_t - a_u) - g_t - g_u;
-  double w = std::sqrt (z * z - g_t * g_u);
-  // Equation 2.4.56 [Sun, Yuan 2006]
-  return (a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w));
-}
 
+  default:
+  case EndpointsCondition::Case4: {
+    // Calculate the minimizer of the cubic that interpolates f_u, f_t, g_u and g_t
+    // Equation 2.4.52 [Sun, Yuan 2006]
+    const double z = 3 * (f_t - f_u) / (a_t - a_u) - g_t - g_u;
+    const double w = std::sqrt(z * z - g_t * g_u);
+    // Equation 2.4.56 [Sun, Yuan 2006]
+    return a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w);
+  }
+  }
+}
 
-template<typename PointSource, typename PointTarget> double
-NormalDistributionsTransform<PointSource, PointTarget>::computeStepLengthMT (const Eigen::Matrix<double, 6, 1> &x, Eigen::Matrix<double, 6, 1> &step_dir, double step_init, double step_max,
-                                                                             double step_min, double &score, Eigen::Matrix<double, 6, 1> &score_gradient, Eigen::Matrix<double, 6, 6> &hessian,
-                                                                             PointCloudSource &trans_cloud)
+template <typename PointSource, typename PointTarget>
+double
+NormalDistributionsTransform<PointSource, PointTarget>::computeStepLengthMT(
+    const Eigen::Matrix<double, 6, 1>& x,
+    Eigen::Matrix<double, 6, 1>& step_dir,
+    double step_init,
+    double step_max,
+    double step_min,
+    double& score,
+    Eigen::Matrix<double, 6, 1>& score_gradient,
+    Eigen::Matrix<double, 6, 6>& hessian,
+    PointCloudSource& trans_cloud)
 {
   // Set the value of phi(0), Equation 1.3 [More, Thuente 1994]
-  double phi_0 = -score;
+  const double phi_0 = -score;
   // Set the value of phi'(0), Equation 1.3 [More, Thuente 1994]
-  double d_phi_0 = -(score_gradient.dot (step_dir));
+  double d_phi_0 = -(score_gradient.dot(step_dir));
 
-  Eigen::Matrix<double, 6, 1>  x_t;
-
-  if (d_phi_0 >= 0)
-  {
+  if (d_phi_0 >= 0) {
     // Not a decent direction
-    if (d_phi_0 == 0)
+    if (d_phi_0 == 0) {
       return 0;
+    }
     // Reverse step direction and calculate optimal step.
     d_phi_0 *= -1;
     step_dir *= -1;
-
   }
 
   // The Search Algorithm for T(mu) [More, Thuente 1994]
 
-  int max_step_iterations = 10;
+  const int max_step_iterations = 10;
   int step_iterations = 0;
 
   // Sufficient decreace constant, Equation 1.1 [More, Thuete 1994]
-  double mu = 1.e-4;
+  const double mu = 1.e-4;
   // Curvature condition constant, Equation 1.2 [More, Thuete 1994]
-  double nu = 0.9;
+  const double nu = 0.9;
 
   // Initial endpoints of Interval I,
   double a_l = 0, a_u = 0;
 
-  // Auxiliary function psi is used until I is determined ot be a closed interval, Equation 2.1 [More, Thuente 1994]
-  double f_l = auxilaryFunction_PsiMT (a_l, phi_0, phi_0, d_phi_0, mu);
-  double g_l = auxilaryFunction_dPsiMT (d_phi_0, d_phi_0, mu);
+  // Auxiliary function psi is used until I is determined ot be a closed interval,
+  // Equation 2.1 [More, Thuente 1994]
+  double f_l = auxilaryFunction_PsiMT(a_l, phi_0, phi_0, d_phi_0, mu);
+  double g_l = auxilaryFunction_dPsiMT(d_phi_0, d_phi_0, mu);
 
-  double f_u = auxilaryFunction_PsiMT (a_u, phi_0, phi_0, d_phi_0, mu);
-  double g_u = auxilaryFunction_dPsiMT (d_phi_0, d_phi_0, mu);
+  double f_u = auxilaryFunction_PsiMT(a_u, phi_0, phi_0, d_phi_0, mu);
+  double g_u = auxilaryFunction_dPsiMT(d_phi_0, d_phi_0, mu);
 
-  // Check used to allow More-Thuente step length calculation to be skipped by making step_min == step_max
+  // Check used to allow More-Thuente step length calculation to be skipped by making
+  // step_min == step_max
   bool interval_converged = (step_max - step_min) < 0, open_interval = true;
 
   double a_t = step_init;
-  a_t = std::min (a_t, step_max);
-  a_t = std::max (a_t, step_min);
+  a_t = std::min(a_t, step_max);
+  a_t = std::max(a_t, step_min);
 
-  x_t = x + step_dir * a_t;
+  Eigen::Matrix<double, 6, 1> x_t = x + step_dir * a_t;
 
-  final_transformation_ = (Eigen::Translation<float, 3>(static_cast<float> (x_t (0)), static_cast<float> (x_t (1)), static_cast<float> (x_t (2))) *
-                           Eigen::AngleAxis<float> (static_cast<float> (x_t (3)), Eigen::Vector3f::UnitX ()) *
-                           Eigen::AngleAxis<float> (static_cast<float> (x_t (4)), Eigen::Vector3f::UnitY ()) *
-                           Eigen::AngleAxis<float> (static_cast<float> (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix ();
+  // Convert x_t into matrix form
+  convertTransform(x_t, final_transformation_);
 
   // New transformed point cloud
-  transformPointCloud (*input_, trans_cloud, final_transformation_);
+  transformPointCloud(*input_, trans_cloud, final_transformation_);
 
-  // Updates score, gradient and hessian.  Hessian calculation is unessisary but testing showed that most step calculations use the
-  // initial step suggestion and recalculation the reusable portions of the hessian would intail more computation time.
-  score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, true);
+  // Updates score, gradient and hessian.  Hessian calculation is unessisary but testing
+  // showed that most step calculations use the initial step suggestion and
+  // recalculation the reusable portions of the hessian would intail more computation
+  // time.
+  score = computeDerivatives(score_gradient, hessian, trans_cloud, x_t, true);
 
   // Calculate phi(alpha_t)
   double phi_t = -score;
   // Calculate phi'(alpha_t)
-  double d_phi_t = -(score_gradient.dot (step_dir));
+  double d_phi_t = -(score_gradient.dot(step_dir));
 
   // Calculate psi(alpha_t)
-  double psi_t = auxilaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu);
+  double psi_t = auxilaryFunction_PsiMT(a_t, phi_t, phi_0, d_phi_0, mu);
   // Calculate psi'(alpha_t)
-  double d_psi_t = auxilaryFunction_dPsiMT (d_phi_t, d_phi_0, mu);
-
-  // Iterate until max number of iterations, interval convergance or a value satisfies the sufficient decrease, Equation 1.1, and curvature condition, Equation 1.2 [More, Thuente 1994]
-  while (!interval_converged && step_iterations < max_step_iterations && !(psi_t <= 0 /*Sufficient Decrease*/ && d_phi_t <= -nu * d_phi_0 /*Curvature Condition*/))
-  {
+  double d_psi_t = auxilaryFunction_dPsiMT(d_phi_t, d_phi_0, mu);
+
+  // Iterate until max number of iterations, interval convergance or a value satisfies
+  // the sufficient decrease, Equation 1.1, and curvature condition, Equation 1.2 [More,
+  // Thuente 1994]
+  while (!interval_converged && step_iterations < max_step_iterations &&
+         !(psi_t <= 0 /*Sufficient Decrease*/ &&
+           d_phi_t <= -nu * d_phi_0 /*Curvature Condition*/)) {
     // Use auxiliary function if interval I is not closed
-    if (open_interval)
-    {
-      a_t = trialValueSelectionMT (a_l, f_l, g_l,
-                                   a_u, f_u, g_u,
-                                   a_t, psi_t, d_psi_t);
+    if (open_interval) {
+      a_t = trialValueSelectionMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, psi_t, d_psi_t);
     }
-    else
-    {
-      a_t = trialValueSelectionMT (a_l, f_l, g_l,
-                                   a_u, f_u, g_u,
-                                   a_t, phi_t, d_phi_t);
+    else {
+      a_t = trialValueSelectionMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, phi_t, d_phi_t);
     }
 
-    a_t = std::min (a_t, step_max);
-    a_t = std::max (a_t, step_min);
+    a_t = std::min(a_t, step_max);
+    a_t = std::max(a_t, step_min);
 
     x_t = x + step_dir * a_t;
 
-    final_transformation_ = (Eigen::Translation<float, 3> (static_cast<float> (x_t (0)), static_cast<float> (x_t (1)), static_cast<float> (x_t (2))) *
-                             Eigen::AngleAxis<float> (static_cast<float> (x_t (3)), Eigen::Vector3f::UnitX ()) *
-                             Eigen::AngleAxis<float> (static_cast<float> (x_t (4)), Eigen::Vector3f::UnitY ()) *
-                             Eigen::AngleAxis<float> (static_cast<float> (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix ();
+    // Convert x_t into matrix form
+    convertTransform(x_t, final_transformation_);
 
     // New transformed point cloud
     // Done on final cloud to prevent wasted computation
-    transformPointCloud (*input_, trans_cloud, final_transformation_);
+    transformPointCloud(*input_, trans_cloud, final_transformation_);
 
     // Updates score, gradient. Values stored to prevent wasted computation.
-    score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, false);
+    score = computeDerivatives(score_gradient, hessian, trans_cloud, x_t, false);
 
     // Calculate phi(alpha_t+)
     phi_t = -score;
     // Calculate phi'(alpha_t+)
-    d_phi_t = -(score_gradient.dot (step_dir));
+    d_phi_t = -(score_gradient.dot(step_dir));
 
     // Calculate psi(alpha_t+)
-    psi_t = auxilaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu);
+    psi_t = auxilaryFunction_PsiMT(a_t, phi_t, phi_0, d_phi_0, mu);
     // Calculate psi'(alpha_t+)
-    d_psi_t = auxilaryFunction_dPsiMT (d_phi_t, d_phi_0, mu);
+    d_psi_t = auxilaryFunction_dPsiMT(d_phi_t, d_phi_0, mu);
 
     // Check if I is now a closed interval
-    if (open_interval && (psi_t <= 0 && d_psi_t >= 0))
-    {
+    if (open_interval && (psi_t <= 0 && d_psi_t >= 0)) {
       open_interval = false;
 
       // Converts f_l and g_l from psi to phi
@@ -724,19 +776,16 @@ NormalDistributionsTransform<PointSource, PointTarget>::computeStepLengthMT (con
       g_u += mu * d_phi_0;
     }
 
-    if (open_interval)
-    {
+    if (open_interval) {
       // Update interval end points using Updating Algorithm [More, Thuente 1994]
-      interval_converged = updateIntervalMT (a_l, f_l, g_l,
-                                             a_u, f_u, g_u,
-                                             a_t, psi_t, d_psi_t);
+      interval_converged =
+          updateIntervalMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, psi_t, d_psi_t);
     }
-    else
-    {
-      // Update interval end points using Modified Updating Algorithm [More, Thuente 1994]
-      interval_converged = updateIntervalMT (a_l, f_l, g_l,
-                                             a_u, f_u, g_u,
-                                             a_t, phi_t, d_phi_t);
+    else {
+      // Update interval end points using Modified Updating Algorithm [More, Thuente
+      // 1994]
+      interval_converged =
+          updateIntervalMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, phi_t, d_phi_t);
     }
 
     step_iterations++;
@@ -745,13 +794,13 @@ NormalDistributionsTransform<PointSource, PointTarget>::computeStepLengthMT (con
   // If inner loop was run then hessian needs to be calculated.
   // Hessian is unnessisary for step length determination but gradients are required
   // so derivative and transform data is stored for the next iteration.
-  if (step_iterations)
-    computeHessian (hessian, trans_cloud, x_t);
+  if (step_iterations) {
+    computeHessian(hessian, trans_cloud);
+  }
 
-  return (a_t);
+  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 d725e1e8..6b1ac880 100644
--- a/registration/include/pcl/registration/impl/ndt_2d.hpp
+++ b/registration/include/pcl/registration/impl/ndt_2d.hpp
@@ -1,205 +1,200 @@
 /*
-* 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_
 
-#include <pcl/registration/eigen.h>
-#include <pcl/registration/boost.h>
+#include <Eigen/Eigenvalues> // for SelfAdjointEigenSolver, EigenSolver
 
 #include <cmath>
 #include <memory>
 
-namespace pcl
-{
+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<unsigned N=3, typename T=double>
-struct ValueAndDerivatives
-{
-  ValueAndDerivatives () : hessian (), grad (), value () {}
+ * (grad vector and hessian matrix), so they can be returned easily from
+ * functions
+ */
+template <unsigned N = 3, typename T = double>
+struct ValueAndDerivatives {
+  ValueAndDerivatives() : hessian(), grad(), value() {}
 
   Eigen::Matrix<T, N, N> hessian;
-  Eigen::Matrix<T, N, 1>    grad;
+  Eigen::Matrix<T, N, 1> grad;
   T value;
 
-  static ValueAndDerivatives<N,T>
-  Zero ()
+  static ValueAndDerivatives<N, T>
+  Zero()
   {
-    ValueAndDerivatives<N,T> r;
-    r.hessian = Eigen::Matrix<T, N, N>::Zero ();
-    r.grad    = Eigen::Matrix<T, N, 1>::Zero ();
-    r.value   = 0;
+    ValueAndDerivatives<N, T> r;
+    r.hessian = Eigen::Matrix<T, N, N>::Zero();
+    r.grad = Eigen::Matrix<T, N, 1>::Zero();
+    r.value = 0;
     return r;
   }
 
-  ValueAndDerivatives<N,T>&
-  operator+= (ValueAndDerivatives<N,T> const& r)
+  ValueAndDerivatives<N, T>&
+  operator+=(ValueAndDerivatives<N, T> const& r)
   {
     hessian += r.hessian;
-    grad    += r.grad;
-    value   += r.value;
+    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 (...).
-  */
+ *
+ * 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 <typename PointT>
-class NormalDist
-{
+class NormalDist {
   using PointCloud = pcl::PointCloud<PointT>;
 
 public:
-  NormalDist ()
-    : min_n_ (3), n_ (0)
-  {
-  }
+  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
-    */
+   * \param[in] i Point index to store
+   */
   void
-  addIdx (std::size_t i)
+  addIdx(std::size_t i)
   {
-    pt_indices_.push_back (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.
-    */
+  /** \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)
+  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 ();
+    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 ();
+    n_ = pt_indices_.size();
 
-    if (n_ >= min_n_)
-    {
-      mean_ = sx / static_cast<double> (n_);
+    if (n_ >= min_n_) {
+      mean_ = sx / static_cast<double>(n_);
       // Using maximum likelihood estimation as in the original paper
-      Eigen::Matrix2d covar = (sxx - 2 * (sx * mean_.transpose ())) / static_cast<double> (n_) + mean_ * mean_.transpose ();
-
-      Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> 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 ();
+      Eigen::Matrix2d covar =
+          (sxx - 2 * (sx * mean_.transpose())) / static_cast<double>(n_) +
+          mean_ * mean_.transpose();
+
+      Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> 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 ();
+        l(0, 0) = l(1, 1) * min_covar_eigvalue_mult;
+        covar = q * l * q.transpose();
       }
-      covar_inv_ = covar.inverse ();
+      covar_inv_ = covar.inverse();
     }
 
-    pt_indices_.clear ();
+    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
+  /** \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 ();
+      return ValueAndDerivatives<3, double>::Zero();
 
-    ValueAndDerivatives<3,double> r;
+    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 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));
+    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<double, 2, 3> jacobian;
-    jacobian <<
-      1, 0, -(x * sin_theta + y*cos_theta),
-      0, 1,   x * cos_theta - y*sin_theta;
+    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;
+      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)
-    );
+    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))
-        );
+        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;
   }
@@ -214,78 +209,85 @@ protected:
 };
 
 /** \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.
-  */
+ * and provide the value and derivatives of the model at any point via the
+ * test (...) function.
+ */
 template <typename PointT>
-class NDTSingleGrid: public boost::noncopyable
-{
+class NDTSingleGrid : public boost::noncopyable {
   using PointCloud = pcl::PointCloud<PointT>;
   using PointCloudConstPtr = typename PointCloud::ConstPtr;
   using NormalDist = pcl::ndt2d::NormalDist<PointT>;
 
 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])
+  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++;
-    }
+    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 ());
+    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);
+        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
+  /** \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);
+    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 ();
+      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
-    */
+   * \param[in] p a point
+   */
   NormalDist*
-  normalDistForPoint (PointT const& p) const
+  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<int> ();
+      idxf[i] = (p.getVector3fMap()[i] - min_[i]) / step_[i];
+    Eigen::Vector2i idxi = idxf.cast<int>();
     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<NormalDist*> (&normal_distributions_.coeffRef (idxi[0], idxi[1]));
+    return const_cast<NormalDist*>(&normal_distributions_.coeffRef(idxi[0], idxi[1]));
   }
 
   Eigen::Vector2f min_;
@@ -297,49 +299,52 @@ protected:
 };
 
 /** \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.
-  */
+ * 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 <typename PointT>
-class NDT2D: public boost::noncopyable
-{
+class NDT2D : public boost::noncopyable {
   using PointCloud = pcl::PointCloud<PointT>;
   using PointCloudConstPtr = typename PointCloud::ConstPtr;
   using SingleGrid = NDTSingleGrid<PointT>;
 
 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)
+   * \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));
+    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
+  /** \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);
+    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;
   }
 
@@ -350,21 +355,21 @@ protected:
 } // namespace ndt2d
 } // namespace pcl
 
-
-namespace Eigen
-{
+namespace Eigen {
 
 /* This NumTraits specialisation is necessary because NormalDist is used as
-* the element type of an Eigen Matrix.
-*/
-template<typename PointT>
-struct NumTraits<pcl::ndt2d::NormalDist<PointT> >
-{
+ * the element type of an Eigen Matrix.
+ */
+template <typename PointT>
+struct NumTraits<pcl::ndt2d::NormalDist<PointT>> {
   using Real = double;
   using Literal = double;
-  static Real dummy_precision () { return 1.0; }
-  enum
+  static Real
+  dummy_precision()
   {
+    return 1.0;
+  }
+  enum {
     IsComplex = 0,
     IsInteger = 0,
     IsSigned = 0,
@@ -377,123 +382,133 @@ struct NumTraits<pcl::ndt2d::NormalDist<PointT> >
 
 } // namespace Eigen
 
+namespace pcl {
 
-namespace pcl
-{
-
-template <typename PointSource, typename PointTarget> void
-NormalDistributionsTransform2D<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
+template <typename PointSource, typename PointTarget>
+void
+NormalDistributionsTransform2D<PointSource, PointTarget>::computeTransformation(
+    PointCloudSource& output, const Eigen::Matrix4f& guess)
 {
   PointCloudSource intm_cloud = output;
 
   nr_iterations_ = 0;
   converged_ = false;
 
-  if (guess != Eigen::Matrix4f::Identity ())
-  {
+  if (guess != Eigen::Matrix4f::Identity()) {
     transformation_ = guess;
-    transformPointCloud (output, intm_cloud, transformation_);
+    transformPointCloud(output, intm_cloud, transformation_);
   }
 
   // build Normal Distribution Transform of target cloud:
-  ndt2d::NDT2D<PointTarget> target_ndt (target_, grid_centre_, grid_extent_, grid_step_);
+  ndt2d::NDT2D<PointTarget> target_ndt(target_, grid_centre_, grid_extent_, grid_step_);
 
   // can't seem to use .block<> () member function on transformation_
   // directly... gcc bug?
   Eigen::Matrix4f& transformation = transformation_;
 
-
   // work with x translation, y translation and z rotation: extending to 3D
   // would be some tricky maths, but not impossible.
-  const Eigen::Matrix3f initial_rot (transformation.block<3,3> (0,0));
-  const Eigen::Vector3f rot_x (initial_rot*Eigen::Vector3f::UnitX ());
-  const double z_rotation = std::atan2 (rot_x[1], rot_x[0]);
+  const Eigen::Matrix3f initial_rot(transformation.block<3, 3>(0, 0));
+  const Eigen::Vector3f rot_x(initial_rot * Eigen::Vector3f::UnitX());
+  const double z_rotation = std::atan2(rot_x[1], rot_x[0]);
 
-  Eigen::Vector3d xytheta_transformation (
-    transformation (0,3),
-    transformation (1,3),
-    z_rotation
-  );
+  Eigen::Vector3d xytheta_transformation(
+      transformation(0, 3), transformation(1, 3), z_rotation);
 
-  while (!converged_)
-  {
-    const double cos_theta = std::cos (xytheta_transformation[2]);
-    const double sin_theta = std::sin (xytheta_transformation[2]);
+  while (!converged_) {
+    const double cos_theta = std::cos(xytheta_transformation[2]);
+    const double sin_theta = std::sin(xytheta_transformation[2]);
     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);
+    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]
-    );
+    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]);
 
-    if (score.value != 0)
-    {
+    if (score.value != 0) {
       // test for positive definiteness, and adjust to ensure it if necessary:
       Eigen::EigenSolver<Eigen::Matrix3d> solver;
-      solver.compute (score.hessian, false);
+      solver.compute(score.hessian, false);
       double min_eigenvalue = 0;
-      for (int i = 0; i <3; i++)
-        if (solver.eigenvalues ()[i].real () < min_eigenvalue)
-            min_eigenvalue = solver.eigenvalues ()[i].real ();
+      for (int i = 0; i < 3; i++)
+        if (solver.eigenvalues()[i].real() < min_eigenvalue)
+          min_eigenvalue = solver.eigenvalues()[i].real();
 
       // ensure "safe" positive definiteness: this is a detail missing
       // from the original paper
-      if (min_eigenvalue < 0)
-      {
+      if (min_eigenvalue < 0) {
         double lambda = 1.1 * min_eigenvalue - 1;
-        score.hessian += Eigen::Vector3d (-lambda, -lambda, -lambda).asDiagonal ();
-        solver.compute (score.hessian, false);
-        PCL_DEBUG ("[pcl::NormalDistributionsTransform2D::computeTransformation] adjust hessian: %f: new eigenvalues:%f %f %f\n",
-            float (lambda),
-            solver.eigenvalues ()[0].real (),
-            solver.eigenvalues ()[1].real (),
-            solver.eigenvalues ()[2].real ()
-        );
+        score.hessian += Eigen::Vector3d(-lambda, -lambda, -lambda).asDiagonal();
+        solver.compute(score.hessian, false);
+        PCL_DEBUG("[pcl::NormalDistributionsTransform2D::computeTransformation] adjust "
+                  "hessian: %f: new eigenvalues:%f %f %f\n",
+                  float(lambda),
+                  solver.eigenvalues()[0].real(),
+                  solver.eigenvalues()[1].real(),
+                  solver.eigenvalues()[2].real());
       }
-      assert (solver.eigenvalues ()[0].real () >= 0 &&
-              solver.eigenvalues ()[1].real () >= 0 &&
-              solver.eigenvalues ()[2].real () >= 0);
+      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);
+      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<float> (xytheta_transformation[2]), Eigen::Vector3f::UnitZ ()));
-      transformation.block<3,1> (0,3).matrix () = Eigen::Vector3f (static_cast<float> (xytheta_transformation[0]), static_cast<float> (xytheta_transformation[1]), 0.0f);
-
-      //std::cout << "new transformation:\n" << transformation << std::endl;
+      transformation.block<3, 3>(0, 0).matrix() = Eigen::Matrix3f(Eigen::AngleAxisf(
+          static_cast<float>(xytheta_transformation[2]), Eigen::Vector3f::UnitZ()));
+      transformation.block<3, 1>(0, 3).matrix() =
+          Eigen::Vector3f(static_cast<float>(xytheta_transformation[0]),
+                          static_cast<float>(xytheta_transformation[1]),
+                          0.0f);
+
+      // std::cout << "new transformation:\n" << transformation << std::endl;
     }
-    else
-    {
-      PCL_ERROR ("[pcl::NormalDistributionsTransform2D::computeTransformation] no overlap: try increasing the size or reducing the step of the grid\n");
+    else {
+      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);
+    transformPointCloud(output, intm_cloud, transformation);
 
     nr_iterations_++;
 
     if (update_visualizer_)
-      update_visualizer_ (output, *indices_, *target_, *indices_);
+      update_visualizer_(output, *indices_, *target_, *indices_);
 
-    //std::cout << "eps=" << std::abs ((transformation - previous_transformation_).sum ()) << std::endl;
+    // std::cout << "eps=" << std::abs ((transformation - previous_transformation_).sum
+    // ()) << std::endl;
 
-    Eigen::Matrix4f transformation_delta = transformation.inverse() * previous_transformation_;
-    double cos_angle = 0.5 * (transformation_delta.coeff (0, 0) + transformation_delta.coeff (1, 1) + transformation_delta.coeff (2, 2) - 1);
-    double translation_sqr = transformation_delta.coeff (0, 3) * transformation_delta.coeff (0, 3) +
-                               transformation_delta.coeff (1, 3) * transformation_delta.coeff (1, 3) +
-                               transformation_delta.coeff (2, 3) * transformation_delta.coeff (2, 3);
+    Eigen::Matrix4f transformation_delta =
+        transformation.inverse() * previous_transformation_;
+    double cos_angle =
+        0.5 * (transformation_delta.coeff(0, 0) + transformation_delta.coeff(1, 1) +
+               transformation_delta.coeff(2, 2) - 1);
+    double translation_sqr =
+        transformation_delta.coeff(0, 3) * transformation_delta.coeff(0, 3) +
+        transformation_delta.coeff(1, 3) * transformation_delta.coeff(1, 3) +
+        transformation_delta.coeff(2, 3) * transformation_delta.coeff(2, 3);
 
     if (nr_iterations_ >= max_iterations_ ||
-        ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) && (transformation_rotation_epsilon_ > 0 && cos_angle >= transformation_rotation_epsilon_)) ||
-        ((transformation_epsilon_ <= 0)                                             && (transformation_rotation_epsilon_ > 0 && cos_angle >= transformation_rotation_epsilon_)) ||
-        ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) && (transformation_rotation_epsilon_ <= 0)))
-    {
+        ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) &&
+         (transformation_rotation_epsilon_ > 0 &&
+          cos_angle >= transformation_rotation_epsilon_)) ||
+        ((transformation_epsilon_ <= 0) &&
+         (transformation_rotation_epsilon_ > 0 &&
+          cos_angle >= transformation_rotation_epsilon_)) ||
+        ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) &&
+         (transformation_rotation_epsilon_ <= 0))) {
       converged_ = true;
     }
   }
@@ -503,5 +518,4 @@ NormalDistributionsTransform2D<PointSource, PointTarget>::computeTransformation
 
 } // namespace pcl
 
-#endif    // PCL_NDT_2D_IMPL_H_
-
+#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 1f2de87a..094308b3 100644
--- a/registration/include/pcl/registration/impl/pairwise_graph_registration.hpp
+++ b/registration/include/pcl/registration/impl/pairwise_graph_registration.hpp
@@ -41,48 +41,53 @@
 #ifndef PCL_REGISTRATION_IMPL_PAIRWISE_GRAPH_REGISTRATION_HPP_
 #define PCL_REGISTRATION_IMPL_PAIRWISE_GRAPH_REGISTRATION_HPP_
 
+namespace pcl {
 
-namespace pcl
+template <typename GraphT, typename PointT>
+void
+PairwiseGraphRegistration<GraphT, PointT>::computeRegistration()
 {
-
-template <typename GraphT, typename PointT> void
-PairwiseGraphRegistration<GraphT, PointT>::computeRegistration ()
-{
-  if (!registration_method_)
-  {
-    PCL_ERROR ("[pcl::PairwiseGraphRegistration::computeRegistration] No registration method set!\n");
+  if (!registration_method_) {
+    PCL_ERROR("[pcl::PairwiseGraphRegistration::computeRegistration] No registration "
+              "method set!\n");
     return;
   }
 
-  typename std::vector<GraphHandlerVertex>::iterator last_vx_it = last_vertices_.begin ();
-  if (last_aligned_vertex_ == boost::graph_traits<GraphT>::null_vertex ())
-  {
+  typename std::vector<GraphHandlerVertex>::iterator last_vx_it =
+      last_vertices_.begin();
+  if (last_aligned_vertex_ == boost::graph_traits<GraphT>::null_vertex()) {
     last_aligned_vertex_ = *last_vx_it;
     ++last_vx_it;
   }
 
   pcl::PointCloud<PointT> fake_cloud;
-  registration_method_->setInputTarget (boost::get_cloud<PointT> (last_aligned_vertex_, *(graph_handler_->getGraph ())));
-  for(; last_vx_it < last_vertices_.end (); ++last_vx_it)
-  {
-    registration_method_->setInputCloud (boost::get_cloud<PointT> (*last_vx_it, *(graph_handler_->getGraph ())));
+  registration_method_->setInputTarget(
+      boost::get_cloud<PointT>(last_aligned_vertex_, *(graph_handler_->getGraph())));
+  for (; last_vx_it < last_vertices_.end(); ++last_vx_it) {
+    registration_method_->setInputCloud(
+        boost::get_cloud<PointT>(*last_vx_it, *(graph_handler_->getGraph())));
 
-    const Eigen::Matrix4f last_aligned_vertex_pose = boost::get_pose (last_aligned_vertex_, *(graph_handler_->getGraph ()));
-    if (!incremental_)
-    {
-      const Eigen::Matrix4f guess = last_aligned_vertex_pose.transpose () * boost::get_pose (*last_vx_it, *(graph_handler_->getGraph ()));
-      registration_method_->align (fake_cloud, guess);
-    } else
-      registration_method_->align (fake_cloud);
+    const Eigen::Matrix4f last_aligned_vertex_pose =
+        boost::get_pose(last_aligned_vertex_, *(graph_handler_->getGraph()));
+    if (!incremental_) {
+      const Eigen::Matrix4f guess =
+          last_aligned_vertex_pose.transpose() *
+          boost::get_pose(*last_vx_it, *(graph_handler_->getGraph()));
+      registration_method_->align(fake_cloud, guess);
+    }
+    else
+      registration_method_->align(fake_cloud);
 
-    const Eigen::Matrix4f global_ref_final_tr = last_aligned_vertex_pose * registration_method_->getFinalTransformation ();
-    boost::set_estimate<PointT> (*last_vx_it, global_ref_final_tr, *(graph_handler_->getGraph ()));
+    const Eigen::Matrix4f global_ref_final_tr =
+        last_aligned_vertex_pose * registration_method_->getFinalTransformation();
+    boost::set_estimate<PointT>(
+        *last_vx_it, global_ref_final_tr, *(graph_handler_->getGraph()));
     last_aligned_vertex_ = *last_vx_it;
-    registration_method_->setInputTarget (boost::get_cloud<PointT> (last_aligned_vertex_, *(graph_handler_->getGraph ())));
+    registration_method_->setInputTarget(
+        boost::get_cloud<PointT>(last_aligned_vertex_, *(graph_handler_->getGraph())));
   }
 }
 
 } // namespace pcl
 
-#endif //PCL_REGISTRATION_IMPL_PAIRWISE_GRAPH_REGISTRATION_HPP_
-
+#endif // PCL_REGISTRATION_IMPL_PAIRWISE_GRAPH_REGISTRATION_HPP_
diff --git a/registration/include/pcl/registration/impl/ppf_registration.hpp b/registration/include/pcl/registration/impl/ppf_registration.hpp
index a81c00a5..42c42545 100644
--- a/registration/include/pcl/registration/impl/ppf_registration.hpp
+++ b/registration/include/pcl/registration/impl/ppf_registration.hpp
@@ -39,38 +39,42 @@
  *
  */
 
-
 #ifndef PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
 #define PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
 
-#include <pcl/registration/ppf_registration.h>
-#include <pcl/features/ppf.h>
 #include <pcl/common/transforms.h>
-
 #include <pcl/features/pfh.h>
+#include <pcl/features/pfh_tools.h> // for computePairFeatures
+#include <pcl/features/ppf.h>
+#include <pcl/registration/ppf_registration.h>
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget> void
-pcl::PPFRegistration<PointSource, PointTarget>::setInputTarget (const PointCloudTargetConstPtr &cloud)
+template <typename PointSource, typename PointTarget>
+void
+pcl::PPFRegistration<PointSource, PointTarget>::setInputTarget(
+    const PointCloudTargetConstPtr& cloud)
 {
-  Registration<PointSource, PointTarget>::setInputTarget (cloud);
+  Registration<PointSource, PointTarget>::setInputTarget(cloud);
 
-  scene_search_tree_ = typename pcl::KdTreeFLANN<PointTarget>::Ptr (new pcl::KdTreeFLANN<PointTarget>);
-  scene_search_tree_->setInputCloud (target_);
+  scene_search_tree_ =
+      typename pcl::KdTreeFLANN<PointTarget>::Ptr(new pcl::KdTreeFLANN<PointTarget>);
+  scene_search_tree_->setInputCloud(target_);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget> void
-pcl::PPFRegistration<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
+template <typename PointSource, typename PointTarget>
+void
+pcl::PPFRegistration<PointSource, PointTarget>::computeTransformation(
+    PointCloudSource& output, const Eigen::Matrix4f& guess)
 {
-  if (!search_method_)
-  {
-    PCL_ERROR("[pcl::PPFRegistration::computeTransformation] Search method not set - skipping computeTransformation!\n");
+  if (!search_method_) {
+    PCL_ERROR("[pcl::PPFRegistration::computeTransformation] Search method not set - "
+              "skipping computeTransformation!\n");
     return;
   }
 
-  if (guess != Eigen::Matrix4f::Identity ())
-  {
-    PCL_ERROR("[pcl::PPFRegistration::computeTransformation] setting initial transform (guess) not implemented!\n");
+  if (guess != Eigen::Matrix4f::Identity()) {
+    PCL_ERROR("[pcl::PPFRegistration::computeTransformation] setting initial transform "
+              "(guess) not implemented!\n");
   }
 
   const auto aux_size = static_cast<std::size_t>(
@@ -79,208 +83,253 @@ pcl::PPFRegistration<PointSource, PointTarget>::computeTransformation (PointClou
   const std::vector<unsigned int> tmp_vec(aux_size, 0);
   std::vector<std::vector<unsigned int>> accumulator_array(input_->size(), tmp_vec);
 
-  PCL_INFO ("Accumulator array size: %u x %u.\n", accumulator_array.size (), accumulator_array.back ().size ());
+  PCL_INFO("Accumulator array size: %u x %u.\n",
+           accumulator_array.size(),
+           accumulator_array.back().size());
 
   PoseWithVotesList voted_poses;
-  // Consider every <scene_reference_point_sampling_rate>-th point as the reference point => fix s_r
+  // Consider every <scene_reference_point_sampling_rate>-th point as the reference
+  // point => fix s_r
   float f1, f2, f3, f4;
-  for (std::size_t scene_reference_index = 0; scene_reference_index < target_->size (); scene_reference_index += scene_reference_point_sampling_rate_)
-  {
-    Eigen::Vector3f scene_reference_point = (*target_)[scene_reference_index].getVector3fMap (),
-        scene_reference_normal = (*target_)[scene_reference_index].getNormalVector3fMap ();
-
-    float rotation_angle_sg = std::acos (scene_reference_normal.dot (Eigen::Vector3f::UnitX ()));
-    bool parallel_to_x_sg = (scene_reference_normal.y() == 0.0f && scene_reference_normal.z() == 0.0f);
-    Eigen::Vector3f rotation_axis_sg = (parallel_to_x_sg)?(Eigen::Vector3f::UnitY ()):(scene_reference_normal.cross (Eigen::Vector3f::UnitX ()). normalized());
-    Eigen::AngleAxisf rotation_sg (rotation_angle_sg, rotation_axis_sg);
-    Eigen::Affine3f transform_sg (Eigen::Translation3f ( rotation_sg * ((-1) * scene_reference_point)) * rotation_sg);
+  for (index_t scene_reference_index = 0;
+       scene_reference_index < static_cast<index_t>(target_->size());
+       scene_reference_index += scene_reference_point_sampling_rate_) {
+    Eigen::Vector3f scene_reference_point =
+                        (*target_)[scene_reference_index].getVector3fMap(),
+                    scene_reference_normal =
+                        (*target_)[scene_reference_index].getNormalVector3fMap();
+
+    float rotation_angle_sg =
+        std::acos(scene_reference_normal.dot(Eigen::Vector3f::UnitX()));
+    bool parallel_to_x_sg =
+        (scene_reference_normal.y() == 0.0f && scene_reference_normal.z() == 0.0f);
+    Eigen::Vector3f rotation_axis_sg =
+        (parallel_to_x_sg)
+            ? (Eigen::Vector3f::UnitY())
+            : (scene_reference_normal.cross(Eigen::Vector3f::UnitX()).normalized());
+    Eigen::AngleAxisf rotation_sg(rotation_angle_sg, rotation_axis_sg);
+    Eigen::Affine3f transform_sg(
+        Eigen::Translation3f(rotation_sg * ((-1) * scene_reference_point)) *
+        rotation_sg);
 
     // For every other point in the scene => now have pair (s_r, s_i) fixed
-    std::vector<int> indices;
+    pcl::Indices indices;
     std::vector<float> distances;
-    scene_search_tree_->radiusSearch ((*target_)[scene_reference_index],
-                                     search_method_->getModelDiameter () /2,
+    scene_search_tree_->radiusSearch((*target_)[scene_reference_index],
+                                     search_method_->getModelDiameter() / 2,
                                      indices,
                                      distances);
-    for(const std::size_t &scene_point_index : indices)
-//    for(std::size_t i = 0; i < target_->size (); ++i)
+    for (const auto& scene_point_index : indices)
+    //    for(std::size_t i = 0; i < target_->size (); ++i)
     {
-      //size_t scene_point_index = i;
-      if (scene_reference_index != scene_point_index)
-      {
-        if (/*pcl::computePPFPairFeature*/pcl::computePairFeatures ((*target_)[scene_reference_index].getVector4fMap (),
-                                        (*target_)[scene_reference_index].getNormalVector4fMap (),
-                                        (*target_)[scene_point_index].getVector4fMap (),
-                                        (*target_)[scene_point_index].getNormalVector4fMap (),
-                                        f1, f2, f3, f4))
-        {
-          std::vector<std::pair<std::size_t, std::size_t> > nearest_indices;
-          search_method_->nearestNeighborSearch (f1, f2, f3, f4, nearest_indices);
+      // size_t scene_point_index = i;
+      if (scene_reference_index != scene_point_index) {
+        if (/*pcl::computePPFPairFeature*/ pcl::computePairFeatures(
+            (*target_)[scene_reference_index].getVector4fMap(),
+            (*target_)[scene_reference_index].getNormalVector4fMap(),
+            (*target_)[scene_point_index].getVector4fMap(),
+            (*target_)[scene_point_index].getNormalVector4fMap(),
+            f1,
+            f2,
+            f3,
+            f4)) {
+          std::vector<std::pair<std::size_t, std::size_t>> nearest_indices;
+          search_method_->nearestNeighborSearch(f1, f2, f3, f4, nearest_indices);
 
           // Compute alpha_s angle
-          Eigen::Vector3f scene_point = (*target_)[scene_point_index].getVector3fMap ();
+          Eigen::Vector3f scene_point = (*target_)[scene_point_index].getVector3fMap();
 
           Eigen::Vector3f scene_point_transformed = transform_sg * scene_point;
-          float alpha_s = std::atan2 ( -scene_point_transformed(2), scene_point_transformed(1));
-          if (std::sin (alpha_s) * scene_point_transformed(2) < 0.0f)
+          float alpha_s =
+              std::atan2(-scene_point_transformed(2), scene_point_transformed(1));
+          if (std::sin(alpha_s) * scene_point_transformed(2) < 0.0f)
             alpha_s *= (-1);
           alpha_s *= (-1);
 
           // Go through point pairs in the model with the same discretized feature
-          for (const auto &nearest_index : nearest_indices)
-          {
+          for (const auto& nearest_index : nearest_indices) {
             std::size_t model_reference_index = nearest_index.first;
             std::size_t model_point_index = nearest_index.second;
             // Calculate angle alpha = alpha_m - alpha_s
-            float alpha = search_method_->alpha_m_[model_reference_index][model_point_index] - alpha_s;
-            unsigned int alpha_discretized = static_cast<unsigned int> (std::floor (alpha) + std::floor (M_PI / search_method_->getAngleDiscretizationStep ()));
-            accumulator_array[model_reference_index][alpha_discretized] ++;
+            float alpha =
+                search_method_->alpha_m_[model_reference_index][model_point_index] -
+                alpha_s;
+            unsigned int alpha_discretized = static_cast<unsigned int>(
+                std::floor(alpha) +
+                std::floor(M_PI / search_method_->getAngleDiscretizationStep()));
+            accumulator_array[model_reference_index][alpha_discretized]++;
           }
         }
-        else PCL_ERROR ("[pcl::PPFRegistration::computeTransformation] Computing pair feature vector between points %u and %u went wrong.\n", scene_reference_index, scene_point_index);
+        else
+          PCL_ERROR("[pcl::PPFRegistration::computeTransformation] Computing pair "
+                    "feature vector between points %u and %u went wrong.\n",
+                    scene_reference_index,
+                    scene_point_index);
       }
     }
 
     std::size_t max_votes_i = 0, max_votes_j = 0;
     unsigned int max_votes = 0;
 
-    for (std::size_t i = 0; i < accumulator_array.size (); ++i)
-      for (std::size_t j = 0; j < accumulator_array.back ().size (); ++j)
-      {
-        if (accumulator_array[i][j] > max_votes)
-        {
+    for (std::size_t i = 0; i < accumulator_array.size(); ++i)
+      for (std::size_t j = 0; j < accumulator_array.back().size(); ++j) {
+        if (accumulator_array[i][j] > max_votes) {
           max_votes = accumulator_array[i][j];
           max_votes_i = i;
           max_votes_j = j;
         }
-        // Reset accumulator_array for the next set of iterations with a new scene reference point
+        // Reset accumulator_array for the next set of iterations with a new scene
+        // reference point
         accumulator_array[i][j] = 0;
       }
 
-    Eigen::Vector3f model_reference_point = (*input_)[max_votes_i].getVector3fMap (),
-        model_reference_normal = (*input_)[max_votes_i].getNormalVector3fMap ();
-    float rotation_angle_mg = std::acos (model_reference_normal.dot (Eigen::Vector3f::UnitX ()));
-    bool parallel_to_x_mg = (model_reference_normal.y() == 0.0f && model_reference_normal.z() == 0.0f);
-    Eigen::Vector3f rotation_axis_mg = (parallel_to_x_mg)?(Eigen::Vector3f::UnitY ()):(model_reference_normal.cross (Eigen::Vector3f::UnitX ()). normalized());
-    Eigen::AngleAxisf rotation_mg (rotation_angle_mg, rotation_axis_mg);
-    Eigen::Affine3f transform_mg (Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg);
-    Eigen::Affine3f max_transform = 
-      transform_sg.inverse () * 
-      Eigen::AngleAxisf ((static_cast<float> (max_votes_j) - std::floor (static_cast<float> (M_PI) / search_method_->getAngleDiscretizationStep ())) * search_method_->getAngleDiscretizationStep (), Eigen::Vector3f::UnitX ()) * 
-      transform_mg;
-
-    voted_poses.push_back (PoseWithVotes (max_transform, max_votes));
+    Eigen::Vector3f model_reference_point = (*input_)[max_votes_i].getVector3fMap(),
+                    model_reference_normal =
+                        (*input_)[max_votes_i].getNormalVector3fMap();
+    float rotation_angle_mg =
+        std::acos(model_reference_normal.dot(Eigen::Vector3f::UnitX()));
+    bool parallel_to_x_mg =
+        (model_reference_normal.y() == 0.0f && model_reference_normal.z() == 0.0f);
+    Eigen::Vector3f rotation_axis_mg =
+        (parallel_to_x_mg)
+            ? (Eigen::Vector3f::UnitY())
+            : (model_reference_normal.cross(Eigen::Vector3f::UnitX()).normalized());
+    Eigen::AngleAxisf rotation_mg(rotation_angle_mg, rotation_axis_mg);
+    Eigen::Affine3f transform_mg(
+        Eigen::Translation3f(rotation_mg * ((-1) * model_reference_point)) *
+        rotation_mg);
+    Eigen::Affine3f max_transform =
+        transform_sg.inverse() *
+        Eigen::AngleAxisf((static_cast<float>(max_votes_j) -
+                           std::floor(static_cast<float>(M_PI) /
+                                      search_method_->getAngleDiscretizationStep())) *
+                              search_method_->getAngleDiscretizationStep(),
+                          Eigen::Vector3f::UnitX()) *
+        transform_mg;
+
+    voted_poses.push_back(PoseWithVotes(max_transform, max_votes));
   }
-  PCL_DEBUG ("Done with the Hough Transform ...\n");
+  PCL_DEBUG("Done with the Hough Transform ...\n");
 
   // Cluster poses for filtering out outliers and obtaining more precise results
   PoseWithVotesList results;
-  clusterPoses (voted_poses, results);
+  clusterPoses(voted_poses, results);
 
-  pcl::transformPointCloud (*input_, output, results.front ().pose);
+  pcl::transformPointCloud(*input_, output, results.front().pose);
 
-  transformation_ = final_transformation_ = results.front ().pose.matrix ();
+  transformation_ = final_transformation_ = results.front().pose.matrix();
   converged_ = true;
 }
 
-
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget> void
-pcl::PPFRegistration<PointSource, PointTarget>::clusterPoses (typename pcl::PPFRegistration<PointSource, PointTarget>::PoseWithVotesList &poses,
-                                                              typename pcl::PPFRegistration<PointSource, PointTarget>::PoseWithVotesList &result)
+template <typename PointSource, typename PointTarget>
+void
+pcl::PPFRegistration<PointSource, PointTarget>::clusterPoses(
+    typename pcl::PPFRegistration<PointSource, PointTarget>::PoseWithVotesList& poses,
+    typename pcl::PPFRegistration<PointSource, PointTarget>::PoseWithVotesList& result)
 {
-  PCL_INFO ("Clustering poses ...\n");
+  PCL_INFO("Clustering poses ...\n");
   // Start off by sorting the poses by the number of votes
-  sort(poses.begin (), poses.end (), poseWithVotesCompareFunction);
+  sort(poses.begin(), poses.end(), poseWithVotesCompareFunction);
 
   std::vector<PoseWithVotesList> clusters;
-  std::vector<std::pair<std::size_t, unsigned int> > cluster_votes;
-  for (std::size_t poses_i = 0; poses_i < poses.size(); ++ poses_i)
-  {
+  std::vector<std::pair<std::size_t, unsigned int>> cluster_votes;
+  for (std::size_t poses_i = 0; poses_i < poses.size(); ++poses_i) {
     bool found_cluster = false;
-    for (std::size_t clusters_i = 0; clusters_i < clusters.size(); ++ clusters_i)
-    {
-      if (posesWithinErrorBounds (poses[poses_i].pose, clusters[clusters_i].front ().pose))
-      {
+    for (std::size_t clusters_i = 0; clusters_i < clusters.size(); ++clusters_i) {
+      if (posesWithinErrorBounds(poses[poses_i].pose,
+                                 clusters[clusters_i].front().pose)) {
         found_cluster = true;
-        clusters[clusters_i].push_back (poses[poses_i]);
+        clusters[clusters_i].push_back(poses[poses_i]);
         cluster_votes[clusters_i].second += poses[poses_i].votes;
         break;
       }
     }
 
-    if (!found_cluster)
-    {
+    if (!found_cluster) {
       // Create a new cluster with the current pose
       PoseWithVotesList new_cluster;
-      new_cluster.push_back (poses[poses_i]);
-      clusters.push_back (new_cluster);
-      cluster_votes.push_back (std::pair<std::size_t, unsigned int> (clusters.size () - 1, poses[poses_i].votes));
+      new_cluster.push_back(poses[poses_i]);
+      clusters.push_back(new_cluster);
+      cluster_votes.push_back(std::pair<std::size_t, unsigned int>(
+          clusters.size() - 1, poses[poses_i].votes));
     }
- }
+  }
 
   // Sort clusters by total number of votes
-  std::sort (cluster_votes.begin (), cluster_votes.end (), clusterVotesCompareFunction);
+  std::sort(cluster_votes.begin(), cluster_votes.end(), clusterVotesCompareFunction);
   // Compute pose average and put them in result vector
-  /// @todo some kind of threshold for determining whether a cluster has enough votes or not...
-  /// now just taking the first three clusters
-  result.clear ();
-  std::size_t max_clusters = (clusters.size () < 3) ? clusters.size () : 3;
-  for (std::size_t cluster_i = 0; cluster_i < max_clusters; ++ cluster_i)
-  {
-    PCL_INFO ("Winning cluster has #votes: %d and #poses voted: %d.\n", cluster_votes[cluster_i].second, clusters[cluster_votes[cluster_i].first].size ());
-    Eigen::Vector3f translation_average (0.0, 0.0, 0.0);
-    Eigen::Vector4f rotation_average (0.0, 0.0, 0.0, 0.0);
-    for (typename PoseWithVotesList::iterator v_it = clusters[cluster_votes[cluster_i].first].begin (); v_it != clusters[cluster_votes[cluster_i].first].end (); ++ v_it)
-    {
-      translation_average += v_it->pose.translation ();
-      /// averaging rotations by just averaging the quaternions in 4D space - reference "On Averaging Rotations" by CLAUS GRAMKOW
-      rotation_average += Eigen::Quaternionf (v_it->pose.rotation ()).coeffs ();
+  /// @todo some kind of threshold for determining whether a cluster has enough votes or
+  /// not... now just taking the first three clusters
+  result.clear();
+  std::size_t max_clusters = (clusters.size() < 3) ? clusters.size() : 3;
+  for (std::size_t cluster_i = 0; cluster_i < max_clusters; ++cluster_i) {
+    PCL_INFO("Winning cluster has #votes: %d and #poses voted: %d.\n",
+             cluster_votes[cluster_i].second,
+             clusters[cluster_votes[cluster_i].first].size());
+    Eigen::Vector3f translation_average(0.0, 0.0, 0.0);
+    Eigen::Vector4f rotation_average(0.0, 0.0, 0.0, 0.0);
+    for (typename PoseWithVotesList::iterator v_it =
+             clusters[cluster_votes[cluster_i].first].begin();
+         v_it != clusters[cluster_votes[cluster_i].first].end();
+         ++v_it) {
+      translation_average += v_it->pose.translation();
+      /// averaging rotations by just averaging the quaternions in 4D space - reference
+      /// "On Averaging Rotations" by CLAUS GRAMKOW
+      rotation_average += Eigen::Quaternionf(v_it->pose.rotation()).coeffs();
     }
 
-    translation_average /= static_cast<float> (clusters[cluster_votes[cluster_i].first].size ());
-    rotation_average /= static_cast<float> (clusters[cluster_votes[cluster_i].first].size ());
+    translation_average /=
+        static_cast<float>(clusters[cluster_votes[cluster_i].first].size());
+    rotation_average /=
+        static_cast<float>(clusters[cluster_votes[cluster_i].first].size());
 
     Eigen::Affine3f transform_average;
-    transform_average.translation ().matrix () = translation_average;
-    transform_average.linear ().matrix () = Eigen::Quaternionf (rotation_average).normalized().toRotationMatrix ();
+    transform_average.translation().matrix() = translation_average;
+    transform_average.linear().matrix() =
+        Eigen::Quaternionf(rotation_average).normalized().toRotationMatrix();
 
-    result.push_back (PoseWithVotes (transform_average, cluster_votes[cluster_i].second));
+    result.push_back(PoseWithVotes(transform_average, cluster_votes[cluster_i].second));
   }
 }
 
-
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget> bool
-pcl::PPFRegistration<PointSource, PointTarget>::posesWithinErrorBounds (Eigen::Affine3f &pose1,
-                                                                        Eigen::Affine3f &pose2)
+template <typename PointSource, typename PointTarget>
+bool
+pcl::PPFRegistration<PointSource, PointTarget>::posesWithinErrorBounds(
+    Eigen::Affine3f& pose1, Eigen::Affine3f& pose2)
 {
-  float position_diff = (pose1.translation () - pose2.translation ()).norm ();
-  Eigen::AngleAxisf rotation_diff_mat ((pose1.rotation ().inverse ().lazyProduct (pose2.rotation ()).eval()));
+  float position_diff = (pose1.translation() - pose2.translation()).norm();
+  Eigen::AngleAxisf rotation_diff_mat(
+      (pose1.rotation().inverse().lazyProduct(pose2.rotation()).eval()));
 
-  float rotation_diff_angle = std::abs (rotation_diff_mat.angle ());
+  float rotation_diff_angle = std::abs(rotation_diff_mat.angle());
 
-  return (position_diff < clustering_position_diff_threshold_ && rotation_diff_angle < clustering_rotation_diff_threshold_);
+  return (position_diff < clustering_position_diff_threshold_ &&
+          rotation_diff_angle < clustering_rotation_diff_threshold_);
 }
 
-
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget> bool
-pcl::PPFRegistration<PointSource, PointTarget>::poseWithVotesCompareFunction (const typename pcl::PPFRegistration<PointSource, PointTarget>::PoseWithVotes &a,
-                                                                              const typename pcl::PPFRegistration<PointSource, PointTarget>::PoseWithVotes &b )
+template <typename PointSource, typename PointTarget>
+bool
+pcl::PPFRegistration<PointSource, PointTarget>::poseWithVotesCompareFunction(
+    const typename pcl::PPFRegistration<PointSource, PointTarget>::PoseWithVotes& a,
+    const typename pcl::PPFRegistration<PointSource, PointTarget>::PoseWithVotes& b)
 {
   return (a.votes > b.votes);
 }
 
-
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget> bool
-pcl::PPFRegistration<PointSource, PointTarget>::clusterVotesCompareFunction (const std::pair<std::size_t, unsigned int> &a,
-                                                                             const std::pair<std::size_t, unsigned int> &b)
+template <typename PointSource, typename PointTarget>
+bool
+pcl::PPFRegistration<PointSource, PointTarget>::clusterVotesCompareFunction(
+    const std::pair<std::size_t, unsigned int>& a,
+    const std::pair<std::size_t, unsigned int>& b)
 {
   return (a.second > b.second);
 }
 
-//#define PCL_INSTANTIATE_PPFRegistration(PointSource,PointTarget) template class PCL_EXPORTS pcl::PPFRegistration<PointSource, PointTarget>;
+//#define PCL_INSTANTIATE_PPFRegistration(PointSource,PointTarget) template class
+// PCL_EXPORTS pcl::PPFRegistration<PointSource, PointTarget>;
 
 #endif // PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
diff --git a/registration/include/pcl/registration/impl/pyramid_feature_matching.hpp b/registration/include/pcl/registration/impl/pyramid_feature_matching.hpp
index 9f33da90..ad5f4a72 100644
--- a/registration/include/pcl/registration/impl/pyramid_feature_matching.hpp
+++ b/registration/include/pcl/registration/impl/pyramid_feature_matching.hpp
@@ -42,187 +42,213 @@
 #ifndef PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_
 #define PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_
 
-#include <pcl/pcl_macros.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
 #include <pcl/console/print.h>
+#include <pcl/pcl_macros.h>
 
+namespace pcl {
 
-namespace pcl
-{
-
-template <typename PointFeature> float
-PyramidFeatureHistogram<PointFeature>::comparePyramidFeatureHistograms (const PyramidFeatureHistogramPtr &pyramid_a,
-                                                                        const PyramidFeatureHistogramPtr &pyramid_b)
+template <typename PointFeature>
+float
+PyramidFeatureHistogram<PointFeature>::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)
-  {
-    PCL_ERROR ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of dimensions: %u vs %u\n", pyramid_a->nr_dimensions, pyramid_b->nr_dimensions);
+  if (pyramid_a->nr_dimensions != pyramid_b->nr_dimensions) {
+    PCL_ERROR("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two "
+              "given pyramids have different numbers of dimensions: %u vs %u\n",
+              pyramid_a->nr_dimensions,
+              pyramid_b->nr_dimensions);
     return -1;
   }
-  if (pyramid_a->nr_levels != pyramid_b->nr_levels)
-  {
-    PCL_ERROR ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of levels: %u vs %u\n", pyramid_a->nr_levels, pyramid_b->nr_levels);
+  if (pyramid_a->nr_levels != pyramid_b->nr_levels) {
+    PCL_ERROR("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two "
+              "given pyramids have different numbers of levels: %u vs %u\n",
+              pyramid_a->nr_levels,
+              pyramid_b->nr_levels);
     return -1;
   }
 
-
   // calculate for level 0 first
-  if (pyramid_a->hist_levels[0].hist.size () != pyramid_b->hist_levels[0].hist.size ())
-  {
-    PCL_ERROR ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of bins on level 0: %u vs %u\n", pyramid_a->hist_levels[0].hist.size (), pyramid_b->hist_levels[0].hist.size ());
+  if (pyramid_a->hist_levels[0].hist.size() != pyramid_b->hist_levels[0].hist.size()) {
+    PCL_ERROR("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two "
+              "given pyramids have different numbers of bins on level 0: %u vs %u\n",
+              pyramid_a->hist_levels[0].hist.size(),
+              pyramid_b->hist_levels[0].hist.size());
     return -1;
   }
   float match_count_level = 0.0f;
-  for (std::size_t bin_i = 0; bin_i < pyramid_a->hist_levels[0].hist.size (); ++bin_i)
-  {
+  for (std::size_t bin_i = 0; bin_i < pyramid_a->hist_levels[0].hist.size(); ++bin_i) {
     if (pyramid_a->hist_levels[0].hist[bin_i] < pyramid_b->hist_levels[0].hist[bin_i])
-      match_count_level += static_cast<float> (pyramid_a->hist_levels[0].hist[bin_i]);
+      match_count_level += static_cast<float>(pyramid_a->hist_levels[0].hist[bin_i]);
     else
-      match_count_level += static_cast<float> (pyramid_b->hist_levels[0].hist[bin_i]);
+      match_count_level += static_cast<float>(pyramid_b->hist_levels[0].hist[bin_i]);
   }
 
-
   float match_count = match_count_level;
-  for (std::size_t level_i = 1; level_i < pyramid_a->nr_levels; ++level_i)
-  {
-    if (pyramid_a->hist_levels[level_i].hist.size () != pyramid_b->hist_levels[level_i].hist.size ())
-    {
-      PCL_ERROR ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of bins on level %u: %u vs %u\n", level_i, pyramid_a->hist_levels[level_i].hist.size (), pyramid_b->hist_levels[level_i].hist.size ());
+  for (std::size_t level_i = 1; level_i < pyramid_a->nr_levels; ++level_i) {
+    if (pyramid_a->hist_levels[level_i].hist.size() !=
+        pyramid_b->hist_levels[level_i].hist.size()) {
+      PCL_ERROR(
+          "[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two "
+          "given pyramids have different numbers of bins on level %u: %u vs %u\n",
+          level_i,
+          pyramid_a->hist_levels[level_i].hist.size(),
+          pyramid_b->hist_levels[level_i].hist.size());
       return -1;
     }
 
     float match_count_prev_level = match_count_level;
     match_count_level = 0.0f;
-    for (std::size_t bin_i = 0; bin_i < pyramid_a->hist_levels[level_i].hist.size (); ++bin_i)
-    {
-      if (pyramid_a->hist_levels[level_i].hist[bin_i] < pyramid_b->hist_levels[level_i].hist[bin_i])
-        match_count_level += static_cast<float> (pyramid_a->hist_levels[level_i].hist[bin_i]);
+    for (std::size_t bin_i = 0; bin_i < pyramid_a->hist_levels[level_i].hist.size();
+         ++bin_i) {
+      if (pyramid_a->hist_levels[level_i].hist[bin_i] <
+          pyramid_b->hist_levels[level_i].hist[bin_i])
+        match_count_level +=
+            static_cast<float>(pyramid_a->hist_levels[level_i].hist[bin_i]);
       else
-        match_count_level += static_cast<float> (pyramid_b->hist_levels[level_i].hist[bin_i]);
+        match_count_level +=
+            static_cast<float>(pyramid_b->hist_levels[level_i].hist[bin_i]);
     }
 
-    float level_normalization_factor = powf (2.0f, static_cast<float> (level_i));
-    match_count += (match_count_level - match_count_prev_level) / level_normalization_factor;
+    float level_normalization_factor = powf(2.0f, static_cast<float>(level_i));
+    match_count +=
+        (match_count_level - match_count_prev_level) / level_normalization_factor;
   }
 
-
   // include self-similarity factors
-  float self_similarity_a = static_cast<float> (pyramid_a->nr_features),
-        self_similarity_b = static_cast<float> (pyramid_b->nr_features);
-  PCL_DEBUG ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] Self similarity measures: %f, %f\n", self_similarity_a, self_similarity_b);
-  match_count /= std::sqrt (self_similarity_a * self_similarity_b);
+  float self_similarity_a = static_cast<float>(pyramid_a->nr_features),
+        self_similarity_b = static_cast<float>(pyramid_b->nr_features);
+  PCL_DEBUG("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] Self "
+            "similarity measures: %f, %f\n",
+            self_similarity_a,
+            self_similarity_b);
+  match_count /= std::sqrt(self_similarity_a * self_similarity_b);
 
   return match_count;
 }
 
-
 template <typename PointFeature>
-PyramidFeatureHistogram<PointFeature>::PyramidFeatureHistogram () :
-  nr_dimensions (0), nr_levels (0), nr_features (0),
-  feature_representation_ (new DefaultPointRepresentation<PointFeature>),
-  is_computed_ (false),
-  hist_levels ()
-{
-}
-
+PyramidFeatureHistogram<PointFeature>::PyramidFeatureHistogram()
+: nr_dimensions(0)
+, nr_levels(0)
+, nr_features(0)
+, feature_representation_(new DefaultPointRepresentation<PointFeature>)
+, is_computed_(false)
+, hist_levels()
+{}
 
-template <typename PointFeature> void
-PyramidFeatureHistogram<PointFeature>::PyramidFeatureHistogramLevel::initializeHistogramLevel ()
+template <typename PointFeature>
+void
+PyramidFeatureHistogram<
+    PointFeature>::PyramidFeatureHistogramLevel::initializeHistogramLevel()
 {
   std::size_t total_vector_size = 1;
-  for (std::vector<std::size_t>::iterator dim_it = bins_per_dimension.begin (); dim_it != bins_per_dimension.end (); ++dim_it)
+  for (std::vector<std::size_t>::iterator dim_it = bins_per_dimension.begin();
+       dim_it != bins_per_dimension.end();
+       ++dim_it)
     total_vector_size *= *dim_it;
 
-  hist.resize (total_vector_size, 0);
+  hist.resize(total_vector_size, 0);
 }
 
-
-template <typename PointFeature> bool
-PyramidFeatureHistogram<PointFeature>::initializeHistogram ()
+template <typename PointFeature>
+bool
+PyramidFeatureHistogram<PointFeature>::initializeHistogram()
 {
   // a few consistency checks before starting the computations
-  if (!PCLBase<PointFeature>::initCompute ())
-  {
-    PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] PCLBase initCompute failed\n");
+  if (!PCLBase<PointFeature>::initCompute()) {
+    PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] PCLBase initCompute "
+              "failed\n");
     return false;
   }
 
-  if (dimension_range_input_.empty ())
-  {
-    PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Input dimension range was not set\n");
+  if (dimension_range_input_.empty()) {
+    PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] Input dimension "
+              "range was not set\n");
     return false;
   }
 
-  if (dimension_range_target_.empty ())
-  {
-    PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Target dimension range was not set\n");
+  if (dimension_range_target_.empty()) {
+    PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] Target dimension "
+              "range was not set\n");
     return false;
   }
 
-  if (dimension_range_input_.size () != dimension_range_target_.size ())
-  {
-    PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Input and target dimension ranges do not agree in size: %u vs %u\n",
-               dimension_range_input_.size (), dimension_range_target_.size ());
+  if (dimension_range_input_.size() != dimension_range_target_.size()) {
+    PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] Input and target "
+              "dimension ranges do not agree in size: %u vs %u\n",
+              dimension_range_input_.size(),
+              dimension_range_target_.size());
     return false;
   }
 
-
-  nr_dimensions = dimension_range_target_.size ();
-  nr_features = input_->size ();
+  nr_dimensions = dimension_range_target_.size();
+  nr_features = input_->size();
   float D = 0.0f;
-  for (std::vector<std::pair<float, float> >::iterator range_it = dimension_range_target_.begin (); range_it != dimension_range_target_.end (); ++range_it)
-  {
+  for (std::vector<std::pair<float, float>>::iterator range_it =
+           dimension_range_target_.begin();
+       range_it != dimension_range_target_.end();
+       ++range_it) {
     float aux = range_it->first - range_it->second;
     D += aux * aux;
   }
-  D = std::sqrt (D);
-  nr_levels = static_cast<std::size_t> (std::ceil (std::log2(D)));
-  PCL_DEBUG ("[pcl::PyramidFeatureHistogram::initializeHistogram] Pyramid will have %u levels with a hyper-parallelepiped diagonal size of %f\n", nr_levels, D);
-
-
-  hist_levels.resize (nr_levels);
-  for (std::size_t level_i = 0; level_i < nr_levels; ++level_i)
-  {
-    std::vector<std::size_t> bins_per_dimension (nr_dimensions);
-    std::vector<float> bin_step (nr_dimensions);
-    for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i) 
-    {
-      bins_per_dimension[dim_i] = 
-        static_cast<std::size_t> (std::ceil ((dimension_range_target_[dim_i].second - dimension_range_target_[dim_i].first) / (powf (2.0f, static_cast<float> (level_i)) * std::sqrt (static_cast<float> (nr_dimensions)))));
-      bin_step[dim_i] = powf (2.0f, static_cast<float> (level_i)) * std::sqrt (static_cast<float> (nr_dimensions));
+  D = std::sqrt(D);
+  nr_levels = static_cast<std::size_t>(std::ceil(std::log2(D)));
+  PCL_DEBUG("[pcl::PyramidFeatureHistogram::initializeHistogram] Pyramid will have %u "
+            "levels with a hyper-parallelepiped diagonal size of %f\n",
+            nr_levels,
+            D);
+
+  hist_levels.resize(nr_levels);
+  for (std::size_t level_i = 0; level_i < nr_levels; ++level_i) {
+    std::vector<std::size_t> bins_per_dimension(nr_dimensions);
+    std::vector<float> bin_step(nr_dimensions);
+    for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i) {
+      bins_per_dimension[dim_i] = static_cast<std::size_t>(
+          std::ceil((dimension_range_target_[dim_i].second -
+                     dimension_range_target_[dim_i].first) /
+                    (powf(2.0f, static_cast<float>(level_i)) *
+                     std::sqrt(static_cast<float>(nr_dimensions)))));
+      bin_step[dim_i] = powf(2.0f, static_cast<float>(level_i)) *
+                        std::sqrt(static_cast<float>(nr_dimensions));
     }
-    hist_levels[level_i] = PyramidFeatureHistogramLevel (bins_per_dimension, bin_step);
+    hist_levels[level_i] = PyramidFeatureHistogramLevel(bins_per_dimension, bin_step);
 
-    PCL_DEBUG ("[pcl::PyramidFeatureHistogram::initializeHistogram] Created vector of size %u at level %u\nwith #bins per dimension:", hist_levels.back ().hist.size (), level_i);
+    PCL_DEBUG("[pcl::PyramidFeatureHistogram::initializeHistogram] Created vector of "
+              "size %u at level %u\nwith #bins per dimension:",
+              hist_levels.back().hist.size(),
+              level_i);
     for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
-      PCL_DEBUG ("%u ", bins_per_dimension[dim_i]);
-    PCL_DEBUG ("\n");
+      PCL_DEBUG("%u ", bins_per_dimension[dim_i]);
+    PCL_DEBUG("\n");
   }
 
   return true;
 }
 
-
-template <typename PointFeature> unsigned int&
-PyramidFeatureHistogram<PointFeature>::at (std::vector<std::size_t> &access,
-                                           std::size_t &level)
+template <typename PointFeature>
+unsigned int&
+PyramidFeatureHistogram<PointFeature>::at(std::vector<std::size_t>& access,
+                                          std::size_t& level)
 {
-  if (access.size () != nr_dimensions)
-  {
-    PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Cannot access histogram position because the access point does not have the right number of dimensions\n");
-    return hist_levels.front ().hist.front ();
+  if (access.size() != nr_dimensions) {
+    PCL_ERROR(
+        "[pcl::PyramidFeatureHistogram::at] Cannot access histogram position because "
+        "the access point does not have the right number of dimensions\n");
+    return hist_levels.front().hist.front();
   }
-  if (level >= hist_levels.size ())
-  {
-    PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
-    return hist_levels.front ().hist.front ();
+  if (level >= hist_levels.size()) {
+    PCL_ERROR(
+        "[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
+    return hist_levels.front().hist.front();
   }
 
   std::size_t vector_position = 0;
   std::size_t dim_accumulator = 1;
 
-  for (int i = static_cast<int> (access.size ()) - 1; i >= 0; --i)
-  {
+  for (int i = static_cast<int>(access.size()) - 1; i >= 0; --i) {
     vector_position += access[i] * dim_accumulator;
     dim_accumulator *= hist_levels[level].bins_per_dimension[i];
   }
@@ -230,72 +256,81 @@ PyramidFeatureHistogram<PointFeature>::at (std::vector<std::size_t> &access,
   return hist_levels[level].hist[vector_position];
 }
 
-
-template <typename PointFeature> unsigned int&
-PyramidFeatureHistogram<PointFeature>::at (std::vector<float> &feature,
-                                           std::size_t &level)
+template <typename PointFeature>
+unsigned int&
+PyramidFeatureHistogram<PointFeature>::at(std::vector<float>& feature,
+                                          std::size_t& level)
 {
-  if (feature.size () != nr_dimensions)
-  {
-    PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] The given feature vector does not match the feature dimensions of the pyramid histogram: %u vs %u\n", feature.size (), nr_dimensions);
-    return hist_levels.front ().hist.front ();
+  if (feature.size() != nr_dimensions) {
+    PCL_ERROR("[pcl::PyramidFeatureHistogram::at] The given feature vector does not "
+              "match the feature dimensions of the pyramid histogram: %u vs %u\n",
+              feature.size(),
+              nr_dimensions);
+    return hist_levels.front().hist.front();
   }
-  if (level >= hist_levels.size ())
-  {
-    PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
-    return hist_levels.front ().hist.front ();
+  if (level >= hist_levels.size()) {
+    PCL_ERROR(
+        "[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
+    return hist_levels.front().hist.front();
   }
 
   std::vector<std::size_t> access;
   for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
-    access.push_back (static_cast<std::size_t> (std::floor ((feature[dim_i] - dimension_range_target_[dim_i].first) / hist_levels[level].bin_step[dim_i])));
+    access.push_back(static_cast<std::size_t>(
+        std::floor((feature[dim_i] - dimension_range_target_[dim_i].first) /
+                   hist_levels[level].bin_step[dim_i])));
 
-  return at (access, level);
+  return at(access, level);
 }
 
-
-template <typename PointFeature> void
-PyramidFeatureHistogram<PointFeature>::convertFeatureToVector (const PointFeature &feature,
-                                                               std::vector<float> &feature_vector)
+template <typename PointFeature>
+void
+PyramidFeatureHistogram<PointFeature>::convertFeatureToVector(
+    const PointFeature& feature, std::vector<float>& feature_vector)
 {
   // convert feature to vector representation
-  feature_vector.resize (feature_representation_->getNumberOfDimensions ());
-  feature_representation_->vectorize (feature, feature_vector);
+  feature_vector.resize(feature_representation_->getNumberOfDimensions());
+  feature_representation_->vectorize(feature, feature_vector);
 
   // adapt the values from the input range to the target range
-  for (std::size_t i = 0; i < feature_vector.size (); ++i)
-    feature_vector[i] = (feature_vector[i] - dimension_range_input_[i].first) / (dimension_range_input_[i].second - dimension_range_input_[i].first) *
-    (dimension_range_target_[i].second - dimension_range_target_[i].first) + dimension_range_target_[i].first;
+  for (std::size_t i = 0; i < feature_vector.size(); ++i)
+    feature_vector[i] =
+        (feature_vector[i] - dimension_range_input_[i].first) /
+            (dimension_range_input_[i].second - dimension_range_input_[i].first) *
+            (dimension_range_target_[i].second - dimension_range_target_[i].first) +
+        dimension_range_target_[i].first;
 }
 
-
-template <typename PointFeature> void
-PyramidFeatureHistogram<PointFeature>::compute ()
+template <typename PointFeature>
+void
+PyramidFeatureHistogram<PointFeature>::compute()
 {
-  if (!initializeHistogram ())
+  if (!initializeHistogram())
     return;
 
-  for (const auto& point: *input_)
-  {
+  for (const auto& point : *input_) {
     std::vector<float> feature_vector;
-    convertFeatureToVector (point, feature_vector);
-    addFeature (feature_vector);
+    // NaN is converted to very high number that gives out of bound exception.
+    if (!pcl::isFinite(point))
+      continue;
+    convertFeatureToVector(point, feature_vector);
+    addFeature(feature_vector);
   }
 
   is_computed_ = true;
 }
 
-
-template <typename PointFeature> void
-PyramidFeatureHistogram<PointFeature>::addFeature (std::vector<float> &feature)
+template <typename PointFeature>
+void
+PyramidFeatureHistogram<PointFeature>::addFeature(std::vector<float>& feature)
 {
   for (std::size_t level_i = 0; level_i < nr_levels; ++level_i)
-    at (feature, level_i) ++;
+    at(feature, level_i)++;
 }
 
 } // namespace pcl
 
-#define PCL_INSTANTIATE_PyramidFeatureHistogram(PointFeature) template class PCL_EXPORTS pcl::PyramidFeatureHistogram<PointFeature>;
+#define PCL_INSTANTIATE_PyramidFeatureHistogram(PointFeature)                          \
+  template class PCL_EXPORTS pcl::PyramidFeatureHistogram<PointFeature>;
 
 #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 1614bfda..1abc953f 100644
--- a/registration/include/pcl/registration/impl/registration.hpp
+++ b/registration/include/pcl/registration/impl/registration.hpp
@@ -40,104 +40,116 @@
 
 #pragma once
 
-namespace pcl
+namespace pcl {
+
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
+Registration<PointSource, PointTarget, Scalar>::setInputSource(
+    const PointCloudSourceConstPtr& cloud)
 {
+  if (cloud->points.empty()) {
+    PCL_ERROR("[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n",
+              getClassName().c_str());
+    return;
+  }
+  source_cloud_updated_ = true;
+  PCLBase<PointSource>::setInputCloud(cloud);
+}
 
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
-Registration<PointSource, PointTarget, Scalar>::setInputTarget (const PointCloudTargetConstPtr &cloud)
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
+Registration<PointSource, PointTarget, Scalar>::setInputTarget(
+    const PointCloudTargetConstPtr& cloud)
 {
-  if (cloud->points.empty ())
-  {
-    PCL_ERROR ("[pcl::%s::setInputTarget] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
+  if (cloud->points.empty()) {
+    PCL_ERROR("[pcl::%s::setInputTarget] Invalid or empty point cloud dataset given!\n",
+              getClassName().c_str());
     return;
   }
   target_ = cloud;
   target_cloud_updated_ = true;
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> bool
-Registration<PointSource, PointTarget, Scalar>::initCompute ()
+template <typename PointSource, typename PointTarget, typename Scalar>
+bool
+Registration<PointSource, PointTarget, Scalar>::initCompute()
 {
-  if (!target_)
-  {
-    PCL_ERROR ("[pcl::registration::%s::compute] No input target dataset was given!\n", getClassName ().c_str ());
+  if (!target_) {
+    PCL_ERROR("[pcl::registration::%s::compute] No input target dataset was given!\n",
+              getClassName().c_str());
     return (false);
   }
 
   // Only update target kd-tree if a new target cloud was set
-  if (target_cloud_updated_ && !force_no_recompute_)
-  {
-    tree_->setInputCloud (target_);
+  if (target_cloud_updated_ && !force_no_recompute_) {
+    tree_->setInputCloud(target_);
     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_);
+  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.
+  // 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<PointSource>::initCompute ());
+  return (PCLBase<PointSource>::initCompute());
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> bool
-Registration<PointSource, PointTarget, Scalar>::initComputeReciprocal ()
+template <typename PointSource, typename PointTarget, typename Scalar>
+bool
+Registration<PointSource, PointTarget, Scalar>::initComputeReciprocal()
 {
-  if (!input_)
-  {
-    PCL_ERROR ("[pcl::registration::%s::compute] No input source dataset was given!\n", getClassName ().c_str ());
+  if (!input_) {
+    PCL_ERROR("[pcl::registration::%s::compute] No input source dataset was given!\n",
+              getClassName().c_str());
     return (false);
   }
 
-  if (source_cloud_updated_ && !force_no_recompute_reciprocal_)
-  {
-    tree_reciprocal_->setInputCloud (input_);
+  if (source_cloud_updated_ && !force_no_recompute_reciprocal_) {
+    tree_reciprocal_->setInputCloud(input_);
     source_cloud_updated_ = false;
   }
   return (true);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline double
-Registration<PointSource, PointTarget, Scalar>::getFitnessScore (
-    const std::vector<float> &distances_a,
-    const std::vector<float> &distances_b)
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline double
+Registration<PointSource, PointTarget, Scalar>::getFitnessScore(
+    const std::vector<float>& distances_a, const std::vector<float>& distances_b)
 {
-  unsigned int nr_elem = static_cast<unsigned int> (std::min (distances_a.size (), distances_b.size ()));
-  Eigen::VectorXf map_a = Eigen::VectorXf::Map (&distances_a[0], nr_elem);
-  Eigen::VectorXf map_b = Eigen::VectorXf::Map (&distances_b[0], nr_elem);
-  return (static_cast<double> ((map_a - map_b).sum ()) / static_cast<double> (nr_elem));
+  unsigned int nr_elem =
+      static_cast<unsigned int>(std::min(distances_a.size(), distances_b.size()));
+  Eigen::VectorXf map_a = Eigen::VectorXf::Map(&distances_a[0], nr_elem);
+  Eigen::VectorXf map_b = Eigen::VectorXf::Map(&distances_b[0], nr_elem);
+  return (static_cast<double>((map_a - map_b).sum()) / static_cast<double>(nr_elem));
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline double
-Registration<PointSource, PointTarget, Scalar>::getFitnessScore (double max_range)
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline double
+Registration<PointSource, PointTarget, Scalar>::getFitnessScore(double max_range)
 {
   double fitness_score = 0.0;
 
   // Transform the input dataset using the final transformation
   PointCloudSource input_transformed;
-  transformPointCloud (*input_, input_transformed, final_transformation_);
+  transformPointCloud(*input_, input_transformed, final_transformation_);
 
-  std::vector<int> nn_indices (1);
-  std::vector<float> nn_dists (1);
+  pcl::Indices nn_indices(1);
+  std::vector<float> nn_dists(1);
 
   // For each point in the source dataset
   int nr = 0;
-  for (const auto& point: input_transformed)
-  {
+  for (const auto& point : input_transformed) {
     // Find its nearest neighbor in the target
-    tree_->nearestKSearch (point, 1, nn_indices, nn_dists);
+    tree_->nearestKSearch(point, 1, nn_indices, nn_dists);
 
     // Deal with occlusions (incomplete targets)
-    if (nn_dists[0] <= max_range)
-    {
+    if (nn_dists[0] <= max_range) {
       // Add to the fitness score
       fitness_score += nn_dists[0];
       nr++;
@@ -146,62 +158,60 @@ Registration<PointSource, PointTarget, Scalar>::getFitnessScore (double max_rang
 
   if (nr > 0)
     return (fitness_score / nr);
-  return (std::numeric_limits<double>::max ());
-
+  return (std::numeric_limits<double>::max());
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
-Registration<PointSource, PointTarget, Scalar>::align (PointCloudSource &output)
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
+Registration<PointSource, PointTarget, Scalar>::align(PointCloudSource& output)
 {
-  align (output, Matrix4::Identity ());
+  align(output, Matrix4::Identity());
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
-Registration<PointSource, PointTarget, Scalar>::align (PointCloudSource &output, const Matrix4& guess)
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
+Registration<PointSource, PointTarget, Scalar>::align(PointCloudSource& output,
+                                                      const Matrix4& guess)
 {
-  if (!initCompute ())
+  if (!initCompute())
     return;
 
   // Resize the output dataset
-  output.resize (indices_->size ());
+  output.resize(indices_->size());
   // Copy the header
-  output.header   = input_->header;
+  output.header = input_->header;
   // Check if the output will be computed for all points or only a subset
-  if (indices_->size () != input_->size ())
-  {
-    output.width    = indices_->size ();
-    output.height   = 1;
+  if (indices_->size() != input_->size()) {
+    output.width = indices_->size();
+    output.height = 1;
   }
-  else
-  {
-    output.width    = static_cast<std::uint32_t> (input_->width);
-    output.height   = input_->height;
+  else {
+    output.width = static_cast<std::uint32_t>(input_->width);
+    output.height = input_->height;
   }
   output.is_dense = input_->is_dense;
 
   // Copy the point data to output
-  for (std::size_t i = 0; i < indices_->size (); ++i)
+  for (std::size_t i = 0; i < indices_->size(); ++i)
     output[i] = (*input_)[(*indices_)[i]];
 
   // Set the internal point representation of choice unless otherwise noted
-  if (point_representation_ && !force_no_recompute_) 
-    tree_->setPointRepresentation (point_representation_);
+  if (point_representation_ && !force_no_recompute_)
+    tree_->setPointRepresentation(point_representation_);
 
   // Perform the actual transformation computation
   converged_ = false;
-  final_transformation_ = transformation_ = previous_transformation_ = Matrix4::Identity ();
+  final_transformation_ = transformation_ = previous_transformation_ =
+      Matrix4::Identity();
 
-  // Right before we estimate the transformation, we set all the point.data[3] values to 1 to aid the rigid 
-  // transformation
-  for (std::size_t i = 0; i < indices_->size (); ++i)
+  // Right before we estimate the transformation, we set all the point.data[3] values to
+  // 1 to aid the rigid transformation
+  for (std::size_t i = 0; i < indices_->size(); ++i)
     output[i].data[3] = 1.0;
 
-  computeTransformation (output, guess);
+  computeTransformation(output, guess);
 
-  deinitCompute ();
+  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 6159f28f..6f853a06 100644
--- a/registration/include/pcl/registration/impl/sample_consensus_prerejective.hpp
+++ b/registration/include/pcl/registration/impl/sample_consensus_prerejective.hpp
@@ -41,42 +41,44 @@
 #ifndef PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_
 #define PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_
 
+namespace pcl {
 
-namespace pcl
+template <typename PointSource, typename PointTarget, typename FeatureT>
+void
+SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::setSourceFeatures(
+    const FeatureCloudConstPtr& features)
 {
-
-template <typename PointSource, typename PointTarget, typename FeatureT> void
-SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::setSourceFeatures (const FeatureCloudConstPtr &features)
-{
-  if (features == nullptr || features->empty ())
-  {
-    PCL_ERROR ("[pcl::%s::setSourceFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
+  if (features == nullptr || features->empty()) {
+    PCL_ERROR(
+        "[pcl::%s::setSourceFeatures] Invalid or empty point cloud dataset given!\n",
+        getClassName().c_str());
     return;
   }
   input_features_ = features;
 }
 
-
-template <typename PointSource, typename PointTarget, typename FeatureT> void
-SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::setTargetFeatures (const FeatureCloudConstPtr &features)
+template <typename PointSource, typename PointTarget, typename FeatureT>
+void
+SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::setTargetFeatures(
+    const FeatureCloudConstPtr& features)
 {
-  if (features == nullptr || features->empty ())
-  {
-    PCL_ERROR ("[pcl::%s::setTargetFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
+  if (features == nullptr || features->empty()) {
+    PCL_ERROR(
+        "[pcl::%s::setTargetFeatures] Invalid or empty point cloud dataset given!\n",
+        getClassName().c_str());
     return;
   }
   target_features_ = features;
-  feature_tree_->setInputCloud (target_features_);
+  feature_tree_->setInputCloud(target_features_);
 }
 
-
-template <typename PointSource, typename PointTarget, typename FeatureT> void
-SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::selectSamples (
-    const PointCloudSource &cloud, int nr_samples, std::vector<int> &sample_indices)
+template <typename PointSource, typename PointTarget, typename FeatureT>
+void
+SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::selectSamples(
+    const PointCloudSource& cloud, int nr_samples, pcl::Indices& sample_indices)
 {
-  if (nr_samples > static_cast<int> (cloud.size ()))
-  {
-    PCL_ERROR ("[pcl::%s::selectSamples] ", getClassName ().c_str ());
+  if (nr_samples > static_cast<int>(cloud.size())) {
+    PCL_ERROR("[pcl::%s::selectSamples] ", getClassName().c_str());
     PCL_ERROR("The number of samples (%d) must not be greater than the number of "
               "points (%zu)!\n",
               nr_samples,
@@ -84,26 +86,24 @@ SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::selectSamples (
     return;
   }
 
-  sample_indices.resize (nr_samples);
+  sample_indices.resize(nr_samples);
   int temp_sample;
 
   // Draw random samples until n samples is reached
-  for (int i = 0; i < nr_samples; i++)
-  {
+  for (int i = 0; i < nr_samples; i++) {
     // Select a random number
-    sample_indices[i] = getRandomIndex (static_cast<int> (cloud.size ()) - i);
+    sample_indices[i] = getRandomIndex(static_cast<int>(cloud.size()) - i);
 
     // Run trough list of numbers, starting at the lowest, to avoid duplicates
-    for (int j = 0; j < i; j++)
-    {
-      // Move value up if it is higher than previous selections to ensure true randomness
-      if (sample_indices[i] >= sample_indices[j])
-      {
+    for (int j = 0; j < i; j++) {
+      // Move value up if it is higher than previous selections to ensure true
+      // randomness
+      if (sample_indices[i] >= sample_indices[j]) {
         sample_indices[i]++;
       }
-      else
-      {
-        // The new number is lower, place it at the correct point and break for a sorted list
+      else {
+        // The new number is lower, place it at the correct point and break for a sorted
+        // list
         temp_sample = sample_indices[i];
         for (int k = i; k > j; k--)
           sample_indices[k] = sample_indices[k - 1];
@@ -115,119 +115,124 @@ SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::selectSamples (
   }
 }
 
-
-template <typename PointSource, typename PointTarget, typename FeatureT> void 
-SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::findSimilarFeatures (
-        const std::vector<int> &sample_indices,
-        std::vector<std::vector<int> >& similar_features,
-        std::vector<int> &corresponding_indices)
+template <typename PointSource, typename PointTarget, typename FeatureT>
+void
+SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::findSimilarFeatures(
+    const pcl::Indices& sample_indices,
+    std::vector<pcl::Indices>& similar_features,
+    pcl::Indices& corresponding_indices)
 {
   // Allocate results
-  corresponding_indices.resize (sample_indices.size ());
-  std::vector<float> nn_distances (k_correspondences_);
+  corresponding_indices.resize(sample_indices.size());
+  std::vector<float> nn_distances(k_correspondences_);
 
   // Loop over the sampled features
-  for (std::size_t i = 0; i < sample_indices.size (); ++i)
-  {
+  for (std::size_t i = 0; i < sample_indices.size(); ++i) {
     // Current feature index
-    const int idx = sample_indices[i];
+    const auto& 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);
+    // 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);
 
     // Select one at random and add it to corresponding_indices
     if (k_correspondences_ == 1)
       corresponding_indices[i] = similar_features[idx][0];
     else
-      corresponding_indices[i] = similar_features[idx][getRandomIndex (k_correspondences_)];
+      corresponding_indices[i] =
+          similar_features[idx][getRandomIndex(k_correspondences_)];
   }
 }
 
-
-template <typename PointSource, typename PointTarget, typename FeatureT> void
-SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
+template <typename PointSource, typename PointTarget, typename FeatureT>
+void
+SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::computeTransformation(
+    PointCloudSource& output, const Eigen::Matrix4f& guess)
 {
   // Some sanity checks first
-  if (!input_features_)
-  {
-    PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
-    PCL_ERROR ("No source features were given! Call setSourceFeatures before aligning.\n");
+  if (!input_features_) {
+    PCL_ERROR("[pcl::%s::computeTransformation] ", getClassName().c_str());
+    PCL_ERROR(
+        "No source features were given! Call setSourceFeatures before aligning.\n");
     return;
   }
-  if (!target_features_)
-  {
-    PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
-    PCL_ERROR ("No target features were given! Call setTargetFeatures before aligning.\n");
+  if (!target_features_) {
+    PCL_ERROR("[pcl::%s::computeTransformation] ", getClassName().c_str());
+    PCL_ERROR(
+        "No target features were given! Call setTargetFeatures before aligning.\n");
     return;
   }
 
-  if (input_->size () != input_features_->size ())
-  {
-    PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
-    PCL_ERROR ("The source points and source feature points need to be in a one-to-one relationship! Current input cloud sizes: %ld vs %ld.\n",
-               input_->size (), input_features_->size ());
+  if (input_->size() != input_features_->size()) {
+    PCL_ERROR("[pcl::%s::computeTransformation] ", getClassName().c_str());
+    PCL_ERROR("The source points and source feature points need to be in a one-to-one "
+              "relationship! Current input cloud sizes: %ld vs %ld.\n",
+              input_->size(),
+              input_features_->size());
     return;
   }
 
-  if (target_->size () != target_features_->size ())
-  {
-    PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
-    PCL_ERROR ("The target points and target feature points need to be in a one-to-one relationship! Current input cloud sizes: %ld vs %ld.\n",
-               target_->size (), target_features_->size ());
+  if (target_->size() != target_features_->size()) {
+    PCL_ERROR("[pcl::%s::computeTransformation] ", getClassName().c_str());
+    PCL_ERROR("The target points and target feature points need to be in a one-to-one "
+              "relationship! Current input cloud sizes: %ld vs %ld.\n",
+              target_->size(),
+              target_features_->size());
     return;
   }
 
-  if (inlier_fraction_ < 0.0f || inlier_fraction_ > 1.0f)
-  {
-    PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
-    PCL_ERROR ("Illegal inlier fraction %f, must be in [0,1]!\n",
-               inlier_fraction_);
+  if (inlier_fraction_ < 0.0f || inlier_fraction_ > 1.0f) {
+    PCL_ERROR("[pcl::%s::computeTransformation] ", getClassName().c_str());
+    PCL_ERROR("Illegal inlier fraction %f, must be in [0,1]!\n", inlier_fraction_);
     return;
   }
 
-  const float similarity_threshold = correspondence_rejector_poly_->getSimilarityThreshold ();
-  if (similarity_threshold < 0.0f || similarity_threshold >= 1.0f)
-  {
-    PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
-    PCL_ERROR ("Illegal prerejection similarity threshold %f, must be in [0,1[!\n",
-               similarity_threshold);
+  const float similarity_threshold =
+      correspondence_rejector_poly_->getSimilarityThreshold();
+  if (similarity_threshold < 0.0f || similarity_threshold >= 1.0f) {
+    PCL_ERROR("[pcl::%s::computeTransformation] ", getClassName().c_str());
+    PCL_ERROR("Illegal prerejection similarity threshold %f, must be in [0,1[!\n",
+              similarity_threshold);
     return;
   }
 
-  if (k_correspondences_ <= 0)
-  {
-    PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
-    PCL_ERROR ("Illegal correspondence randomness %d, must be > 0!\n",
-            k_correspondences_);
+  if (k_correspondences_ <= 0) {
+    PCL_ERROR("[pcl::%s::computeTransformation] ", getClassName().c_str());
+    PCL_ERROR("Illegal correspondence randomness %d, must be > 0!\n",
+              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_);
+  // 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<float>::max ();
+  inliers_.clear();
+  float lowest_error = std::numeric_limits<float>::max();
   converged_ = false;
 
   // Temporaries
-  std::vector<int> inliers;
+  pcl::Indices 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<float> (inliers.size ()) / static_cast<float> (input_->size ());
+  if (!guess.isApprox(Eigen::Matrix4f::Identity(), 0.01f)) {
+    getFitness(inliers, error);
+    inlier_fraction =
+        static_cast<float>(inliers.size()) / static_cast<float>(input_->size());
 
-    if (inlier_fraction >= inlier_fraction_ && error < lowest_error)
-    {
+    if (inlier_fraction >= inlier_fraction_ && error < lowest_error) {
       inliers_ = inliers;
       lowest_error = error;
       converged_ = true;
@@ -235,30 +240,30 @@ SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::computeTransfor
   }
 
   // Feature correspondence cache
-  std::vector<std::vector<int> > similar_features (input_->size ());
+  std::vector<pcl::Indices> similar_features(input_->size());
 
   // Start
-  for (int i = 0; i < max_iterations_; ++i)
-  {
+  for (int i = 0; i < max_iterations_; ++i) {
     // Temporary containers
-    std::vector<int> sample_indices;
-    std::vector<int> corresponding_indices;
+    pcl::Indices sample_indices;
+    pcl::Indices corresponding_indices;
 
     // Draw nr_samples_ random samples
-    selectSamples (*input_, nr_samples_, sample_indices);
+    selectSamples(*input_, nr_samples_, sample_indices);
 
     // Find corresponding features in the target cloud
-    findSimilarFeatures (sample_indices, similar_features, corresponding_indices);
+    findSimilarFeatures(sample_indices, similar_features, corresponding_indices);
 
     // Apply prerejection
-    if (!correspondence_rejector_poly_->thresholdPolygon (sample_indices, corresponding_indices))
-    {
+    if (!correspondence_rejector_poly_->thresholdPolygon(sample_indices,
+                                                         corresponding_indices)) {
       ++num_rejections;
       continue;
     }
 
     // Estimate the transform from the correspondences, write to transformation_
-    transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_);
+    transformation_estimation_->estimateRigidTransformation(
+        *input_, sample_indices, *target_, corresponding_indices, transformation_);
 
     // Take a backup of previous result
     const Matrix4 final_transformation_prev = final_transformation_;
@@ -267,17 +272,17 @@ SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::computeTransfor
     final_transformation_ = transformation_;
 
     // Transform the input and compute the error (uses input_ and final_transformation_)
-    getFitness (inliers, error);
+    getFitness(inliers, error);
 
     // Restore previous result
     final_transformation_ = final_transformation_prev;
 
     // If the new fit is better, update results
-    inlier_fraction = static_cast<float> (inliers.size ()) / static_cast<float> (input_->size ());
+    inlier_fraction =
+        static_cast<float>(inliers.size()) / static_cast<float>(input_->size());
 
     // Update result if pose hypothesis is better
-    if (inlier_fraction >= inlier_fraction_ && error < lowest_error)
-    {
+    if (inlier_fraction >= inlier_fraction_ && error < lowest_error) {
       inliers_ = inliers;
       lowest_error = error;
       converged_ = true;
@@ -287,20 +292,24 @@ SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::computeTransfor
 
   // Apply the final transformation
   if (converged_)
-    transformPointCloud (*input_, output, final_transformation_);
+    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_);
+  PCL_DEBUG("[pcl::%s::computeTransformation] Rejected %i out of %i generated pose "
+            "hypotheses.\n",
+            getClassName().c_str(),
+            num_rejections,
+            max_iterations_);
 }
 
-
-template <typename PointSource, typename PointTarget, typename FeatureT> void
-SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::getFitness (std::vector<int>& inliers, float& fitness_score)
+template <typename PointSource, typename PointTarget, typename FeatureT>
+void
+SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::getFitness(
+    pcl::Indices& inliers, float& fitness_score)
 {
   // Initialize variables
-  inliers.clear ();
-  inliers.reserve (input_->size ());
+  inliers.clear();
+  inliers.reserve(input_->size());
   fitness_score = 0.0f;
 
   // Use squared distance for comparison with NN search results
@@ -308,22 +317,20 @@ SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::getFitness (std
 
   // Transform the input dataset using the final transformation
   PointCloudSource input_transformed;
-  input_transformed.resize (input_->size ());
-  transformPointCloud (*input_, input_transformed, final_transformation_);
+  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.size (); ++i)
-  {
+  for (std::size_t i = 0; i < input_transformed.size(); ++i) {
     // Find its nearest neighbor in the target
-    std::vector<int> nn_indices (1);
-    std::vector<float> nn_dists (1);
-    tree_->nearestKSearch (input_transformed[i], 1, nn_indices, nn_dists);
+    pcl::Indices nn_indices(1);
+    std::vector<float> nn_dists(1);
+    tree_->nearestKSearch(input_transformed[i], 1, nn_indices, nn_dists);
 
     // Check if point is an inlier
-    if (nn_dists[0] < max_range)
-    {
+    if (nn_dists[0] < max_range) {
       // Update inliers
-      inliers.push_back (static_cast<int> (i));
+      inliers.push_back(i);
 
       // Update fitness score
       fitness_score += nn_dists[0];
@@ -331,13 +338,12 @@ SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::getFitness (std
   }
 
   // Calculate MSE
-  if (!inliers.empty ())
-    fitness_score /= static_cast<float> (inliers.size ());
+  if (!inliers.empty())
+    fitness_score /= static_cast<float>(inliers.size());
   else
-    fitness_score = std::numeric_limits<float>::max ();
+    fitness_score = std::numeric_limits<float>::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 1d79ad07..c2aae92c 100644
--- a/registration/include/pcl/registration/impl/transformation_estimation_2D.hpp
+++ b/registration/include/pcl/registration/impl/transformation_estimation_2D.hpp
@@ -38,22 +38,19 @@
 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_HPP_
 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_HPP_
 
+namespace pcl {
 
-namespace pcl
-{
-
-namespace registration
-{
+namespace registration {
 
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
-TransformationEstimation2D<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
+TransformationEstimation2D<PointSource, PointTarget, Scalar>::
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  const auto nr_points = cloud_src.size ();
-  if (cloud_tgt.size () != nr_points)
-  {
+  const auto nr_points = cloud_src.size();
+  if (cloud_tgt.size() != nr_points) {
     PCL_ERROR("[pcl::TransformationEstimation2D::estimateRigidTransformation] Number "
               "or points in source (%zu) differs than target (%zu)!\n",
               static_cast<std::size_t>(nr_points),
@@ -61,21 +58,20 @@ TransformationEstimation2D<PointSource, PointTarget, Scalar>::estimateRigidTrans
     return;
   }
 
-  ConstCloudIterator<PointSource> source_it (cloud_src);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> void
-TransformationEstimation2D<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const std::vector<int> &indices_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
+TransformationEstimation2D<PointSource, PointTarget, Scalar>::
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::Indices& indices_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  if (indices_src.size () != cloud_tgt.size ())
-  {
+  if (indices_src.size() != cloud_tgt.size()) {
     PCL_ERROR("[pcl::Transformation2D::estimateRigidTransformation] Number or points "
               "in source (%zu) differs than target (%zu)!\n",
               indices_src.size(),
@@ -83,99 +79,109 @@ TransformationEstimation2D<PointSource, PointTarget, Scalar>::estimateRigidTrans
     return;
   }
 
-  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
-TransformationEstimation2D<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const std::vector<int> &indices_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    const std::vector<int> &indices_tgt,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
+TransformationEstimation2D<PointSource, PointTarget, Scalar>::
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::Indices& indices_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                const pcl::Indices& indices_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  if (indices_src.size () != indices_tgt.size ())
-  {
-    PCL_ERROR ("[pcl::TransformationEstimation2D::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ());
+  if (indices_src.size() != indices_tgt.size()) {
+    PCL_ERROR("[pcl::TransformationEstimation2D::estimateRigidTransformation] Number "
+              "or points in source (%lu) differs than target (%lu)!\n",
+              indices_src.size(),
+              indices_tgt.size());
     return;
   }
 
-  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt, indices_tgt);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt, indices_tgt);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> void
-TransformationEstimation2D<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    const pcl::Correspondences &correspondences,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
+TransformationEstimation2D<PointSource, PointTarget, Scalar>::
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                const pcl::Correspondences& correspondences,
+                                Matrix4& transformation_matrix) const
 {
-  ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src, correspondences, true);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt, correspondences, false);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
-TransformationEstimation2D<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
-    ConstCloudIterator<PointSource>& source_it,
-    ConstCloudIterator<PointTarget>& target_it,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
+TransformationEstimation2D<PointSource, PointTarget, Scalar>::
+    estimateRigidTransformation(ConstCloudIterator<PointSource>& source_it,
+                                ConstCloudIterator<PointTarget>& target_it,
+                                Matrix4& transformation_matrix) const
 {
-  source_it.reset (); target_it.reset ();
+  source_it.reset();
+  target_it.reset();
 
   Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
   // Estimate the centroids of source, target
-  compute3DCentroid (source_it, centroid_src);
-  compute3DCentroid (target_it, centroid_tgt);
-  source_it.reset (); target_it.reset ();
+  compute3DCentroid(source_it, centroid_src);
+  compute3DCentroid(target_it, centroid_tgt);
+  source_it.reset();
+  target_it.reset();
 
   // ignore z component
   centroid_src[2] = 0.0f;
   centroid_tgt[2] = 0.0f;
   // Subtract the centroids from source, target
-  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean, cloud_tgt_demean;
-  demeanPointCloud (source_it, centroid_src, cloud_src_demean);
-  demeanPointCloud (target_it, centroid_tgt, cloud_tgt_demean);
-
-  getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);
+  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean,
+      cloud_tgt_demean;
+  demeanPointCloud(source_it, centroid_src, cloud_src_demean);
+  demeanPointCloud(target_it, centroid_tgt, cloud_tgt_demean);
+
+  getTransformationFromCorrelation(cloud_src_demean,
+                                   centroid_src,
+                                   cloud_tgt_demean,
+                                   centroid_tgt,
+                                   transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> void
-TransformationEstimation2D<PointSource, PointTarget, Scalar>::getTransformationFromCorrelation (
-    const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_src_demean,
-    const Eigen::Matrix<Scalar, 4, 1> &centroid_src,
-    const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean,
-    const Eigen::Matrix<Scalar, 4, 1> &centroid_tgt,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
+TransformationEstimation2D<PointSource, PointTarget, Scalar>::
+    getTransformationFromCorrelation(
+        const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_src_demean,
+        const Eigen::Matrix<Scalar, 4, 1>& centroid_src,
+        const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_tgt_demean,
+        const Eigen::Matrix<Scalar, 4, 1>& centroid_tgt,
+        Matrix4& transformation_matrix) const
 {
-  transformation_matrix.setIdentity ();
+  transformation_matrix.setIdentity();
 
   // Assemble the correlation matrix H = source * target'
-  Eigen::Matrix<Scalar, 3, 3> H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner (3, 3);
+  Eigen::Matrix<Scalar, 3, 3> 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)));
+  float angle = std::atan2((H(0, 1) - H(1, 0)), (H(0, 0) + H(1, 1)));
 
-  Eigen::Matrix<Scalar, 3, 3> R (Eigen::Matrix<Scalar, 3, 3>::Identity ());
-  R (0, 0) = R (1, 1) = std::cos (angle);
-  R (0, 1) = -std::sin (angle);
-  R (1, 0) = std::sin (angle);
+  Eigen::Matrix<Scalar, 3, 3> R(Eigen::Matrix<Scalar, 3, 3>::Identity());
+  R(0, 0) = R(1, 1) = std::cos(angle);
+  R(0, 1) = -std::sin(angle);
+  R(1, 0) = std::sin(angle);
 
   // Return the correct transformation
-  transformation_matrix.topLeftCorner (3, 3).matrix () = R;
-  const Eigen::Matrix<Scalar, 3, 1> Rc (R * centroid_src.head (3).matrix ());
-  transformation_matrix.block (0, 3, 3, 1).matrix () = centroid_tgt.head (3) - Rc;
+  transformation_matrix.topLeftCorner(3, 3).matrix() = R;
+  const Eigen::Matrix<Scalar, 3, 1> Rc(R * centroid_src.head(3).matrix());
+  transformation_matrix.block(0, 3, 3, 1).matrix() = centroid_tgt.head(3) - Rc;
 }
 
 } // namespace registration
 } // namespace pcl
 
-#endif    // PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_HPP_
-
+#endif // PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_HPP_
diff --git a/registration/include/pcl/registration/impl/transformation_estimation_3point.hpp b/registration/include/pcl/registration/impl/transformation_estimation_3point.hpp
index 0821fb6a..cceac553 100644
--- a/registration/include/pcl/registration/impl/transformation_estimation_3point.hpp
+++ b/registration/include/pcl/registration/impl/transformation_estimation_3point.hpp
@@ -41,14 +41,14 @@
 #include <pcl/registration/transformation_estimation_3point.h>
 
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
-pcl::registration::TransformationEstimation3Point<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
+pcl::registration::TransformationEstimation3Point<PointSource, PointTarget, Scalar>::
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  if (cloud_src.size () != 3 || cloud_tgt.size () != 3)
-  {
+  if (cloud_src.size() != 3 || cloud_tgt.size() != 3) {
     PCL_ERROR("[pcl::TransformationEstimation3Point::estimateRigidTransformation] "
               "Number of points in source (%zu) and target (%zu) must be 3!\n",
               static_cast<std::size_t>(cloud_src.size()),
@@ -56,21 +56,21 @@ pcl::registration::TransformationEstimation3Point<PointSource, PointTarget, Scal
     return;
   }
 
-  ConstCloudIterator<PointSource> source_it (cloud_src);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename Scalar> void
-pcl::registration::TransformationEstimation3Point<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const std::vector<int> &indices_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
+pcl::registration::TransformationEstimation3Point<PointSource, PointTarget, Scalar>::
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::Indices& indices_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  if (indices_src.size () != 3 || cloud_tgt.size () != 3)
-  {
+  if (indices_src.size() != 3 || cloud_tgt.size() != 3) {
     PCL_ERROR(
         "[pcl::TransformationEstimation3Point::estimateRigidTransformation] Number of "
         "indices in source (%zu) and points in target (%zu) must be 3!\n",
@@ -79,108 +79,119 @@ pcl::registration::TransformationEstimation3Point<PointSource, PointTarget, Scal
     return;
   }
 
-  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
-pcl::registration::TransformationEstimation3Point<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const std::vector<int> &indices_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    const std::vector<int> &indices_tgt,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
+pcl::registration::TransformationEstimation3Point<PointSource, PointTarget, Scalar>::
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::Indices& indices_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                const pcl::Indices& indices_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  if (indices_src.size () != 3 || indices_tgt.size () != 3)
-  {
-    PCL_ERROR ("[pcl::TransformationEstimation3Point::estimateRigidTransformation] Number of indices in source (%lu) and target (%lu) must be 3!\n", 
-      indices_src.size (), indices_tgt.size ());
+  if (indices_src.size() != 3 || indices_tgt.size() != 3) {
+    PCL_ERROR("[pcl::TransformationEstimation3Point::estimateRigidTransformation] "
+              "Number of indices in source (%lu) and target (%lu) must be 3!\n",
+              indices_src.size(),
+              indices_tgt.size());
     return;
   }
 
-  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt, indices_tgt);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt, indices_tgt);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename Scalar> void
-pcl::registration::TransformationEstimation3Point<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    const pcl::Correspondences &correspondences,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
+pcl::registration::TransformationEstimation3Point<PointSource, PointTarget, Scalar>::
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                const pcl::Correspondences& correspondences,
+                                Matrix4& transformation_matrix) const
 {
-  if (correspondences.size () != 3)
-  {
-    PCL_ERROR ("[pcl::TransformationEstimation3Point::estimateRigidTransformation] Number of correspondences (%lu) must be 3!\n", 
-      correspondences.size ());
+  if (correspondences.size() != 3) {
+    PCL_ERROR("[pcl::TransformationEstimation3Point::estimateRigidTransformation] "
+              "Number of correspondences (%lu) must be 3!\n",
+              correspondences.size());
     return;
   }
 
-  ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src, correspondences, true);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt, correspondences, false);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
-pcl::registration::TransformationEstimation3Point<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
-    ConstCloudIterator<PointSource>& source_it,
-    ConstCloudIterator<PointTarget>& target_it,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
+pcl::registration::TransformationEstimation3Point<PointSource, PointTarget, Scalar>::
+    estimateRigidTransformation(ConstCloudIterator<PointSource>& source_it,
+                                ConstCloudIterator<PointTarget>& target_it,
+                                Matrix4& transformation_matrix) const
 {
-  transformation_matrix.setIdentity ();
-  source_it.reset ();
-  target_it.reset ();
-
-  Eigen::Matrix <Scalar, 4, 1> source_mean, target_mean;    
-  pcl::compute3DCentroid (source_it, source_mean);
-  pcl::compute3DCentroid (target_it, target_mean);
-
-  source_it.reset ();
-  target_it.reset ();
-
-  Eigen::Matrix <Scalar, Eigen::Dynamic, Eigen::Dynamic> source_demean, target_demean;
-  pcl::demeanPointCloud (source_it, source_mean, source_demean, 3);
-  pcl::demeanPointCloud (target_it, target_mean, target_demean, 3);
-  
-  source_it.reset ();
-  target_it.reset ();
-    
-  Eigen::Matrix <Scalar, 3, 1> s1 = source_demean.col (1).head (3) - source_demean.col (0).head (3); 
-  s1.normalize ();
-
-  Eigen::Matrix <Scalar, 3, 1> s2 = source_demean.col (2).head (3) - source_demean.col (0).head (3); 
-  s2 -= s2.dot (s1) * s1;
-  s2.normalize ();
-  
-  Eigen::Matrix <Scalar, 3, 3> source_rot;
-  source_rot.col (0) = s1;
-  source_rot.col (1) = s2;
-  source_rot.col (2) = s1.cross (s2);
-    
-  Eigen::Matrix <Scalar, 3, 1> t1 = target_demean.col (1).head (3) - target_demean.col (0).head (3); 
-  t1.normalize ();
-
-  Eigen::Matrix <Scalar, 3, 1> t2 = target_demean.col (2).head (3) - target_demean.col (0).head (3); 
-  t2 -= t2.dot (t1) * t1;
-  t2.normalize ();
-  
-  Eigen::Matrix <Scalar, 3, 3> target_rot;
-  target_rot.col (0) = t1;
-  target_rot.col (1) = t2;
-  target_rot.col (2) = t1.cross (t2);
-
-  //Eigen::Matrix <Scalar, 3, 3> R = source_rot * target_rot.transpose ();
-  Eigen::Matrix <Scalar, 3, 3> R = target_rot * source_rot.transpose ();
-  transformation_matrix.topLeftCorner (3, 3) = R;
-  //transformation_matrix.block (0, 3, 3, 1) = source_mean.head (3) - R * target_mean.head (3);
-  transformation_matrix.block (0, 3, 3, 1) = target_mean.head (3) - R * source_mean.head (3);
+  transformation_matrix.setIdentity();
+  source_it.reset();
+  target_it.reset();
+
+  Eigen::Matrix<Scalar, 4, 1> source_mean, target_mean;
+  pcl::compute3DCentroid(source_it, source_mean);
+  pcl::compute3DCentroid(target_it, target_mean);
+
+  source_it.reset();
+  target_it.reset();
+
+  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> source_demean, target_demean;
+  pcl::demeanPointCloud(source_it, source_mean, source_demean, 3);
+  pcl::demeanPointCloud(target_it, target_mean, target_demean, 3);
+
+  source_it.reset();
+  target_it.reset();
+
+  Eigen::Matrix<Scalar, 3, 1> s1 =
+      source_demean.col(1).head(3) - source_demean.col(0).head(3);
+  s1.normalize();
+
+  Eigen::Matrix<Scalar, 3, 1> s2 =
+      source_demean.col(2).head(3) - source_demean.col(0).head(3);
+  s2 -= s2.dot(s1) * s1;
+  s2.normalize();
+
+  Eigen::Matrix<Scalar, 3, 3> source_rot;
+  source_rot.col(0) = s1;
+  source_rot.col(1) = s2;
+  source_rot.col(2) = s1.cross(s2);
+
+  Eigen::Matrix<Scalar, 3, 1> t1 =
+      target_demean.col(1).head(3) - target_demean.col(0).head(3);
+  t1.normalize();
+
+  Eigen::Matrix<Scalar, 3, 1> t2 =
+      target_demean.col(2).head(3) - target_demean.col(0).head(3);
+  t2 -= t2.dot(t1) * t1;
+  t2.normalize();
+
+  Eigen::Matrix<Scalar, 3, 3> target_rot;
+  target_rot.col(0) = t1;
+  target_rot.col(1) = t2;
+  target_rot.col(2) = t1.cross(t2);
+
+  // Eigen::Matrix <Scalar, 3, 3> R = source_rot * target_rot.transpose ();
+  Eigen::Matrix<Scalar, 3, 3> R = target_rot * source_rot.transpose();
+  transformation_matrix.topLeftCorner(3, 3) = R;
+  // transformation_matrix.block (0, 3, 3, 1) = source_mean.head (3) - R *
+  // target_mean.head (3);
+  transformation_matrix.block(0, 3, 3, 1) =
+      target_mean.head(3) - R * source_mean.head(3);
 }
 
-//#define PCL_INSTANTIATE_TransformationEstimation3Point(T,U) template class PCL_EXPORTS pcl::registration::TransformationEstimation3Point<T,U>;
+//#define PCL_INSTANTIATE_TransformationEstimation3Point(T,U) template class PCL_EXPORTS
+// pcl::registration::TransformationEstimation3Point<T,U>;
 
 #endif // PCL_REGISTRATION_IMPL_TRANSFORMATION_ESTIMATION_3POINT_H_
diff --git a/registration/include/pcl/registration/impl/transformation_estimation_dq.hpp b/registration/include/pcl/registration/impl/transformation_estimation_dq.hpp
index a7493d96..fce6079d 100644
--- a/registration/include/pcl/registration/impl/transformation_estimation_dq.hpp
+++ b/registration/include/pcl/registration/impl/transformation_estimation_dq.hpp
@@ -41,23 +41,24 @@
 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_DQ_HPP_
 
 #include <pcl/common/eigen.h>
+PCL_DEPRECATED_HEADER(1,
+                      15,
+                      "TransformationEstimationDQ has been renamed to "
+                      "TransformationEstimationDualQuaternion.");
 
+namespace pcl {
 
-namespace pcl
-{
-
-namespace registration
-{
+namespace registration {
 
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
-TransformationEstimationDQ<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
+TransformationEstimationDQ<PointSource, PointTarget, Scalar>::
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  const auto nr_points = cloud_src.size ();
-  if (cloud_tgt.size () != nr_points)
-  {
+  const auto nr_points = cloud_src.size();
+  if (cloud_tgt.size() != nr_points) {
     PCL_ERROR("[pcl::TransformationEstimationDQ::estimateRigidTransformation] Number "
               "or points in source (%zu) differs than target (%zu)!\n",
               static_cast<std::size_t>(nr_points),
@@ -65,21 +66,20 @@ TransformationEstimationDQ<PointSource, PointTarget, Scalar>::estimateRigidTrans
     return;
   }
 
-  ConstCloudIterator<PointSource> source_it (cloud_src);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> void
-TransformationEstimationDQ<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const std::vector<int> &indices_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
+TransformationEstimationDQ<PointSource, PointTarget, Scalar>::
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::Indices& indices_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  if (indices_src.size () != cloud_tgt.size ())
-  {
+  if (indices_src.size() != cloud_tgt.size()) {
     PCL_ERROR("[pcl::TransformationDQ::estimateRigidTransformation] Number or points "
               "in source (%zu) differs than target (%zu)!\n",
               indices_src.size(),
@@ -87,22 +87,21 @@ TransformationEstimationDQ<PointSource, PointTarget, Scalar>::estimateRigidTrans
     return;
   }
 
-  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
-TransformationEstimationDQ<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const std::vector<int> &indices_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    const std::vector<int> &indices_tgt,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
+TransformationEstimationDQ<PointSource, PointTarget, Scalar>::
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::Indices& indices_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                const pcl::Indices& indices_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  if (indices_src.size () != indices_tgt.size ())
-  {
+  if (indices_src.size() != indices_tgt.size()) {
     PCL_ERROR("[pcl::TransformationEstimationDQ::estimateRigidTransformation] Number "
               "or points in source (%zu) differs than target (%zu)!\n",
               indices_src.size(),
@@ -110,70 +109,70 @@ TransformationEstimationDQ<PointSource, PointTarget, Scalar>::estimateRigidTrans
     return;
   }
 
-  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt, indices_tgt);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt, indices_tgt);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> void
-TransformationEstimationDQ<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    const pcl::Correspondences &correspondences,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
+TransformationEstimationDQ<PointSource, PointTarget, Scalar>::
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                const pcl::Correspondences& correspondences,
+                                Matrix4& transformation_matrix) const
 {
-  ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src, correspondences, true);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt, correspondences, false);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
-TransformationEstimationDQ<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
-    ConstCloudIterator<PointSource>& source_it,
-    ConstCloudIterator<PointTarget>& target_it,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
+TransformationEstimationDQ<PointSource, PointTarget, Scalar>::
+    estimateRigidTransformation(ConstCloudIterator<PointSource>& source_it,
+                                ConstCloudIterator<PointTarget>& target_it,
+                                Matrix4& transformation_matrix) const
 {
-  const int npts = static_cast <int> (source_it.size ());
+  const int npts = static_cast<int>(source_it.size());
 
-  transformation_matrix.setIdentity ();
+  transformation_matrix.setIdentity();
 
   // dual quaternion optimization
-  Eigen::Matrix<Scalar,4,4> C1 = Eigen::Matrix<Scalar,4,4>::Zero();
-  Eigen::Matrix<Scalar,4,4> C2 = Eigen::Matrix<Scalar,4,4>::Zero();
-  Scalar *c1 = C1.data();
-  Scalar *c2 = C2.data();
-
-  for( int i=0; i<npts; i++ ) {
-    const PointSource &a = *source_it;
-    const PointTarget &b = *target_it;
-    const Scalar axbx = a.x*b.x;
-    const Scalar ayby = a.y*b.y;
-    const Scalar azbz = a.z*b.z;
-    const Scalar axby = a.x*b.y;
-    const Scalar aybx = a.y*b.x;
-    const Scalar axbz = a.x*b.z;
-    const Scalar azbx = a.z*b.x;
-    const Scalar aybz = a.y*b.z;
-    const Scalar azby = a.z*b.y;
+  Eigen::Matrix<Scalar, 4, 4> C1 = Eigen::Matrix<Scalar, 4, 4>::Zero();
+  Eigen::Matrix<Scalar, 4, 4> C2 = Eigen::Matrix<Scalar, 4, 4>::Zero();
+  Scalar* c1 = C1.data();
+  Scalar* c2 = C2.data();
+
+  for (int i = 0; i < npts; i++) {
+    const PointSource& a = *source_it;
+    const PointTarget& b = *target_it;
+    const Scalar axbx = a.x * b.x;
+    const Scalar ayby = a.y * b.y;
+    const Scalar azbz = a.z * b.z;
+    const Scalar axby = a.x * b.y;
+    const Scalar aybx = a.y * b.x;
+    const Scalar axbz = a.x * b.z;
+    const Scalar azbx = a.z * b.x;
+    const Scalar aybz = a.y * b.z;
+    const Scalar azby = a.z * b.y;
     c1[0] += axbx - azbz - ayby;
     c1[5] += ayby - azbz - axbx;
-    c1[10]+= azbz - axbx - ayby;
-    c1[15]+= axbx + ayby + azbz;
+    c1[10] += azbz - axbx - ayby;
+    c1[15] += axbx + ayby + azbz;
     c1[1] += axby + aybx;
     c1[2] += axbz + azbx;
     c1[3] += aybz - azby;
     c1[6] += azby + aybz;
     c1[7] += azbx - axbz;
-    c1[11]+= axby - aybx;
+    c1[11] += axby - aybx;
 
     c2[1] += a.z + b.z;
     c2[2] -= a.y + b.y;
     c2[3] += a.x - b.x;
     c2[6] += a.x + b.x;
     c2[7] += a.y - b.y;
-    c2[11]+= a.z - b.z;
+    c2[11] += a.z - b.z;
     source_it++;
     target_it++;
   }
@@ -181,46 +180,46 @@ TransformationEstimationDQ<PointSource, PointTarget, Scalar>::estimateRigidTrans
   c1[4] = c1[1];
   c1[8] = c1[2];
   c1[9] = c1[6];
-  c1[12]= c1[3];
-  c1[13]= c1[7];
-  c1[14]= c1[11];
+  c1[12] = c1[3];
+  c1[13] = c1[7];
+  c1[14] = c1[11];
   c2[4] = -c2[1];
   c2[8] = -c2[2];
-  c2[12]= -c2[3];
+  c2[12] = -c2[3];
   c2[9] = -c2[6];
-  c2[13]= -c2[7];
-  c2[14]= -c2[11];
+  c2[13] = -c2[7];
+  c2[14] = -c2[11];
 
   C1 *= -2.0f;
   C2 *= 2.0f;
 
-  const Eigen::Matrix<Scalar,4,4> A = (0.25f/float(npts))*C2.transpose()*C2 - C1;
+  const Eigen::Matrix<Scalar, 4, 4> A =
+      (0.25f / float(npts)) * C2.transpose() * C2 - C1;
 
-  const Eigen::EigenSolver< Eigen::Matrix<Scalar,4,4> > es(A);
+  const Eigen::EigenSolver<Eigen::Matrix<Scalar, 4, 4>> es(A);
 
   ptrdiff_t i;
   es.eigenvalues().real().maxCoeff(&i);
-  const Eigen::Matrix<Scalar,4,1> qmat = es.eigenvectors().col(i).real();
-  const Eigen::Matrix<Scalar,4,1> smat = -(0.5f/float(npts))*C2*qmat;
+  const Eigen::Matrix<Scalar, 4, 1> qmat = es.eigenvectors().col(i).real();
+  const Eigen::Matrix<Scalar, 4, 1> smat = -(0.5f / float(npts)) * C2 * qmat;
 
-  const Eigen::Quaternion<Scalar> q( qmat(3), qmat(0), qmat(1), qmat(2) );
-  const Eigen::Quaternion<Scalar> s( smat(3), smat(0), smat(1), smat(2) );
+  const Eigen::Quaternion<Scalar> q(qmat(3), qmat(0), qmat(1), qmat(2));
+  const Eigen::Quaternion<Scalar> s(smat(3), smat(0), smat(1), smat(2));
 
-  const Eigen::Quaternion<Scalar> t = s*q.conjugate();
+  const Eigen::Quaternion<Scalar> t = s * q.conjugate();
 
-  const Eigen::Matrix<Scalar,3,3> R( q.toRotationMatrix() );
+  const Eigen::Matrix<Scalar, 3, 3> R(q.toRotationMatrix());
 
-  for( int i=0; i<3; ++i )
-    for( int j=0; j<3; ++j)
-      transformation_matrix(i,j) = R(i,j);
+  for (int i = 0; i < 3; ++i)
+    for (int j = 0; j < 3; ++j)
+      transformation_matrix(i, j) = R(i, j);
 
-  transformation_matrix(0,3) = -t.x();
-  transformation_matrix(1,3) = -t.y();
-  transformation_matrix(2,3) = -t.z();
+  transformation_matrix(0, 3) = -t.x();
+  transformation_matrix(1, 3) = -t.y();
+  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 1f59835c..927a324a 100644
--- a/registration/include/pcl/registration/impl/transformation_estimation_dual_quaternion.hpp
+++ b/registration/include/pcl/registration/impl/transformation_estimation_dual_quaternion.hpp
@@ -42,22 +42,21 @@
 
 #include <pcl/common/eigen.h>
 
+#include <Eigen/Eigenvalues> // for EigenSolver
 
-namespace pcl
-{
+namespace pcl {
 
-namespace registration
-{
+namespace registration {
 
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
-TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
+TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar>::
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  const auto nr_points = cloud_src.size ();
-  if (cloud_tgt.size () != nr_points)
-  {
+  const auto nr_points = cloud_src.size();
+  if (cloud_tgt.size() != nr_points) {
     PCL_ERROR(
         "[pcl::TransformationEstimationDualQuaternion::estimateRigidTransformation] "
         "Number or points in source (%zu) differs than target (%zu)!\n",
@@ -66,21 +65,20 @@ TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar>::estima
     return;
   }
 
-  ConstCloudIterator<PointSource> source_it (cloud_src);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> void
-TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const std::vector<int> &indices_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
+TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar>::
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::Indices& indices_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  if (indices_src.size () != cloud_tgt.size ())
-  {
+  if (indices_src.size() != cloud_tgt.size()) {
     PCL_ERROR("[pcl::TransformationDQ::estimateRigidTransformation] Number or points "
               "in source (%zu) differs than target (%zu)!\n",
               indices_src.size(),
@@ -88,63 +86,65 @@ TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar>::estima
     return;
   }
 
-  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
-TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const std::vector<int> &indices_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    const std::vector<int> &indices_tgt,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
+TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar>::
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::Indices& indices_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                const pcl::Indices& indices_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  if (indices_src.size () != indices_tgt.size ())
-  {
-    PCL_ERROR ("[pcl::TransformationEstimationDualQuaternion::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ());
+  if (indices_src.size() != indices_tgt.size()) {
+    PCL_ERROR(
+        "[pcl::TransformationEstimationDualQuaternion::estimateRigidTransformation] "
+        "Number or points in source (%lu) differs than target (%lu)!\n",
+        indices_src.size(),
+        indices_tgt.size());
     return;
   }
 
-  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt, indices_tgt);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt, indices_tgt);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> void
-TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    const pcl::Correspondences &correspondences,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
+TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar>::
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                const pcl::Correspondences& correspondences,
+                                Matrix4& transformation_matrix) const
 {
-  ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src, correspondences, true);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt, correspondences, false);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
-TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
-    ConstCloudIterator<PointSource>& source_it,
-    ConstCloudIterator<PointTarget>& target_it,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
+TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar>::
+    estimateRigidTransformation(ConstCloudIterator<PointSource>& source_it,
+                                ConstCloudIterator<PointTarget>& target_it,
+                                Matrix4& transformation_matrix) const
 {
-  const int npts = static_cast<int> (source_it.size ());
+  const int npts = static_cast<int>(source_it.size());
 
-  transformation_matrix.setIdentity ();
+  transformation_matrix.setIdentity();
 
   // dual quaternion optimization
-  Eigen::Matrix<double, 4, 4> C1 = Eigen::Matrix<double, 4, 4>::Zero ();
-  Eigen::Matrix<double, 4, 4> C2 = Eigen::Matrix<double, 4, 4>::Zero ();
-  double* c1 = C1.data ();
-  double* c2 = C2.data ();
+  Eigen::Matrix<double, 4, 4> C1 = Eigen::Matrix<double, 4, 4>::Zero();
+  Eigen::Matrix<double, 4, 4> C2 = Eigen::Matrix<double, 4, 4>::Zero();
+  double* c1 = C1.data();
+  double* c2 = C2.data();
 
-  for (int i = 0; i < npts; ++i)
-  {
+  for (int i = 0; i < npts; ++i) {
     const PointSource& a = *source_it;
     const PointTarget& b = *target_it;
     const double axbx = a.x * b.x;
@@ -156,70 +156,70 @@ TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar>::estima
     const double azbx = a.z * b.x;
     const double aybz = a.y * b.z;
     const double azby = a.z * b.y;
-    c1[0]  += axbx - azbz - ayby;
-    c1[5]  += ayby - azbz - axbx;
+    c1[0] += axbx - azbz - ayby;
+    c1[5] += ayby - azbz - axbx;
     c1[10] += azbz - axbx - ayby;
     c1[15] += axbx + ayby + azbz;
-    c1[1]  += axby + aybx;
-    c1[2]  += axbz + azbx;
-    c1[3]  += aybz - azby;
-    c1[6]  += azby + aybz;
-    c1[7]  += azbx - axbz;
+    c1[1] += axby + aybx;
+    c1[2] += axbz + azbx;
+    c1[3] += aybz - azby;
+    c1[6] += azby + aybz;
+    c1[7] += azbx - axbz;
     c1[11] += axby - aybx;
 
-    c2[1]  += a.z + b.z;
-    c2[2]  -= a.y + b.y;
-    c2[3]  += a.x - b.x;
-    c2[6]  += a.x + b.x;
-    c2[7]  += a.y - b.y;
+    c2[1] += a.z + b.z;
+    c2[2] -= a.y + b.y;
+    c2[3] += a.x - b.x;
+    c2[6] += a.x + b.x;
+    c2[7] += a.y - b.y;
     c2[11] += a.z - b.z;
     source_it++;
     target_it++;
   }
 
-  c1[4]  = c1[1];
-  c1[8]  = c1[2];
-  c1[9]  = c1[6];
+  c1[4] = c1[1];
+  c1[8] = c1[2];
+  c1[9] = c1[6];
   c1[12] = c1[3];
   c1[13] = c1[7];
   c1[14] = c1[11];
-  c2[4]  = -c2[1];
-  c2[8]  = -c2[2];
+  c2[4] = -c2[1];
+  c2[8] = -c2[2];
   c2[12] = -c2[3];
-  c2[9]  = -c2[6];
+  c2[9] = -c2[6];
   c2[13] = -c2[7];
   c2[14] = -c2[11];
 
   C1 *= -2.0;
   C2 *= 2.0;
 
-  const Eigen::Matrix<double, 4, 4> A = (0.25 / double (npts)) * C2.transpose () * C2 - C1;
+  const Eigen::Matrix<double, 4, 4> A =
+      (0.25 / double(npts)) * C2.transpose() * C2 - C1;
 
-  const Eigen::EigenSolver<Eigen::Matrix<double, 4, 4> > es (A);
+  const Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>> es(A);
 
   ptrdiff_t i;
-  es.eigenvalues ().real ().maxCoeff (&i);
-  const Eigen::Matrix<double, 4, 1> qmat = es.eigenvectors ().col (i).real ();
-  const Eigen::Matrix<double, 4, 1> smat = - (0.5 / double (npts)) * C2 * qmat;
+  es.eigenvalues().real().maxCoeff(&i);
+  const Eigen::Matrix<double, 4, 1> qmat = es.eigenvectors().col(i).real();
+  const Eigen::Matrix<double, 4, 1> smat = -(0.5 / double(npts)) * C2 * qmat;
 
-  const Eigen::Quaternion<double> q (qmat (3), qmat (0), qmat (1), qmat (2));
-  const Eigen::Quaternion<double> s (smat (3), smat (0), smat (1), smat (2));
+  const Eigen::Quaternion<double> q(qmat(3), qmat(0), qmat(1), qmat(2));
+  const Eigen::Quaternion<double> s(smat(3), smat(0), smat(1), smat(2));
 
-  const Eigen::Quaternion<double> t = s * q.conjugate ();
+  const Eigen::Quaternion<double> t = s * q.conjugate();
 
-  const Eigen::Matrix<double, 3, 3> R (q.toRotationMatrix ());
+  const Eigen::Matrix<double, 3, 3> R(q.toRotationMatrix());
 
   for (int i = 0; i < 3; ++i)
     for (int j = 0; j < 3; ++j)
-      transformation_matrix (i, j) = R (i, j);
+      transformation_matrix(i, j) = R(i, j);
 
-  transformation_matrix (0, 3) = - t.x ();
-  transformation_matrix (1, 3) = - t.y ();
-  transformation_matrix (2, 3) = - t.z ();
+  transformation_matrix(0, 3) = -t.x();
+  transformation_matrix(1, 3) = -t.y();
+  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_lm.hpp b/registration/include/pcl/registration/impl/transformation_estimation_lm.hpp
index ce9aacb3..fe01a34a 100644
--- a/registration/include/pcl/registration/impl/transformation_estimation_lm.hpp
+++ b/registration/include/pcl/registration/impl/transformation_estimation_lm.hpp
@@ -40,89 +40,93 @@
 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_HPP_
 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_HPP_
 
-#include <pcl/registration/warp_point_rigid.h>
 #include <pcl/registration/warp_point_rigid_6d.h>
-#include <pcl/registration/distances.h>
-#include <unsupported/Eigen/NonLinearOptimization>
 
+#include <unsupported/Eigen/NonLinearOptimization>
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointSource, typename PointTarget, typename MatScalar>
-pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::TransformationEstimationLM ()
-  : tmp_src_ ()
-  , tmp_tgt_ ()
-  , tmp_idx_src_ ()
-  , tmp_idx_tgt_ ()
-  , warp_point_ (new WarpPointRigid6D<PointSource, PointTarget, MatScalar>)
-{
-};
+pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::
+    TransformationEstimationLM()
+: tmp_src_()
+, tmp_tgt_()
+, tmp_idx_src_()
+, tmp_idx_tgt_()
+, warp_point_(new WarpPointRigid6D<PointSource, PointTarget, MatScalar>){};
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename MatScalar> void
-pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename MatScalar>
+void
+pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                Matrix4& transformation_matrix) const
 {
 
   // <cloud_src,cloud_src> is the source dataset
-  if (cloud_src.size () != cloud_tgt.size ())
-  {
-    PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] ");
+  if (cloud_src.size() != cloud_tgt.size()) {
+    PCL_ERROR("[pcl::registration::TransformationEstimationLM::"
+              "estimateRigidTransformation] ");
     PCL_ERROR("Number or points in source (%zu) differs than target (%zu)!\n",
               static_cast<std::size_t>(cloud_src.size()),
               static_cast<std::size_t>(cloud_tgt.size()));
     return;
   }
-  if (cloud_src.size () < 4)     // need at least 4 samples
+  if (cloud_src.size() < 4) // need at least 4 samples
   {
-    PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] ");
+    PCL_ERROR("[pcl::registration::TransformationEstimationLM::"
+              "estimateRigidTransformation] ");
     PCL_ERROR("Need at least 4 points to estimate a transform! Source and target have "
               "%zu points!\n",
               static_cast<std::size_t>(cloud_src.size()));
     return;
   }
 
-  int n_unknowns = warp_point_->getDimension ();
-  VectorX x (n_unknowns);
-  x.setZero ();
-  
+  int n_unknowns = warp_point_->getDimension();
+  VectorX x(n_unknowns);
+  x.setZero();
+
   // Set temporary pointers
   tmp_src_ = &cloud_src;
   tmp_tgt_ = &cloud_tgt;
 
-  OptimizationFunctor functor (static_cast<int> (cloud_src.size ()), this);
-  Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
-  //Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, double> lm (num_diff);
-  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, MatScalar> lm (num_diff);
-  int info = lm.minimize (x);
+  OptimizationFunctor functor(static_cast<int>(cloud_src.size()), this);
+  Eigen::NumericalDiff<OptimizationFunctor> num_diff(functor);
+  // Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, double> lm
+  // (num_diff);
+  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, MatScalar> lm(
+      num_diff);
+  int info = lm.minimize(x);
 
   // Compute the norm of the residuals
-  PCL_DEBUG ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation]");
-  PCL_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
-  PCL_DEBUG ("Final solution: [%f", x[0]);
-  for (int i = 1; i < n_unknowns; ++i) 
-    PCL_DEBUG (" %f", x[i]);
-  PCL_DEBUG ("]\n");
+  PCL_DEBUG(
+      "[pcl::registration::TransformationEstimationLM::estimateRigidTransformation]");
+  PCL_DEBUG("LM solver finished with exit code %i, having a residual norm of %g. \n",
+            info,
+            lm.fvec.norm());
+  PCL_DEBUG("Final solution: [%f", x[0]);
+  for (int i = 1; i < n_unknowns; ++i)
+    PCL_DEBUG(" %f", x[i]);
+  PCL_DEBUG("]\n");
 
   // Return the correct transformation
-  warp_point_->setParam (x);
-  transformation_matrix = warp_point_->getTransform ();
+  warp_point_->setParam(x);
+  transformation_matrix = warp_point_->getTransform();
 
   tmp_src_ = nullptr;
   tmp_tgt_ = nullptr;
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename MatScalar> void
-pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const std::vector<int> &indices_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename MatScalar>
+void
+pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::Indices& indices_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  if (indices_src.size () != cloud_tgt.size ())
-  {
+  if (indices_src.size() != cloud_tgt.size()) {
     PCL_ERROR(
         "[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] "
         "Number or points in source (%zu) differs than target (%zu)!\n",
@@ -132,43 +136,49 @@ pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScala
   }
 
   // <cloud_src,cloud_src> is the source dataset
-  transformation_matrix.setIdentity ();
+  transformation_matrix.setIdentity();
 
-  const auto nr_correspondences = cloud_tgt.size ();
-  std::vector<int> indices_tgt;
+  const auto nr_correspondences = cloud_tgt.size();
+  pcl::Indices indices_tgt;
   indices_tgt.resize(nr_correspondences);
   for (std::size_t i = 0; i < nr_correspondences; ++i)
     indices_tgt[i] = i;
 
-  estimateRigidTransformation(cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
+  estimateRigidTransformation(
+      cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename MatScalar> inline void
-pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const std::vector<int> &indices_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    const std::vector<int> &indices_tgt,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename MatScalar>
+inline void
+pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::Indices& indices_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                const pcl::Indices& indices_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  if (indices_src.size () != indices_tgt.size ())
-  {
-    PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ());
+  if (indices_src.size() != indices_tgt.size()) {
+    PCL_ERROR(
+        "[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] "
+        "Number or points in source (%lu) differs than target (%lu)!\n",
+        indices_src.size(),
+        indices_tgt.size());
     return;
   }
 
-  if (indices_src.size () < 4)     // need at least 4 samples
+  if (indices_src.size() < 4) // need at least 4 samples
   {
-    PCL_ERROR ("[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] ");
-    PCL_ERROR ("Need at least 4 points to estimate a transform! Source and target have %lu points!",
-               indices_src.size ());
+    PCL_ERROR("[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] ");
+    PCL_ERROR("Need at least 4 points to estimate a transform! Source and target have "
+              "%lu points!",
+              indices_src.size());
     return;
   }
 
-  int n_unknowns = warp_point_->getDimension ();  // get dimension of unknown space
-  VectorX x (n_unknowns);
-  x.setConstant (n_unknowns, 0);
+  int n_unknowns = warp_point_->getDimension(); // get dimension of unknown space
+  VectorX x(n_unknowns);
+  x.setConstant(n_unknowns, 0);
 
   // Set temporary pointers
   tmp_src_ = &cloud_src;
@@ -176,22 +186,29 @@ pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScala
   tmp_idx_src_ = &indices_src;
   tmp_idx_tgt_ = &indices_tgt;
 
-  OptimizationFunctorWithIndices functor (static_cast<int> (indices_src.size ()), this);
-  Eigen::NumericalDiff<OptimizationFunctorWithIndices> num_diff (functor);
-  //Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices> > lm (num_diff);
-  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices>, MatScalar> lm (num_diff);
-  int info = lm.minimize (x);
+  OptimizationFunctorWithIndices functor(static_cast<int>(indices_src.size()), this);
+  Eigen::NumericalDiff<OptimizationFunctorWithIndices> num_diff(functor);
+  // Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices> > lm
+  // (num_diff);
+  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices>,
+                            MatScalar>
+      lm(num_diff);
+  int info = lm.minimize(x);
 
   // Compute the norm of the residuals
-  PCL_DEBUG ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
-  PCL_DEBUG ("Final solution: [%f", x[0]);
-  for (int i = 1; i < n_unknowns; ++i) 
-    PCL_DEBUG (" %f", x[i]);
-  PCL_DEBUG ("]\n");
+  PCL_DEBUG(
+      "[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] LM "
+      "solver finished with exit code %i, having a residual norm of %g. \n",
+      info,
+      lm.fvec.norm());
+  PCL_DEBUG("Final solution: [%f", x[0]);
+  for (int i = 1; i < n_unknowns; ++i)
+    PCL_DEBUG(" %f", x[i]);
+  PCL_DEBUG("]\n");
 
   // Return the correct transformation
-  warp_point_->setParam (x);
-  transformation_matrix = warp_point_->getTransform ();
+  warp_point_->setParam(x);
+  transformation_matrix = warp_point_->getTransform();
 
   tmp_src_ = nullptr;
   tmp_tgt_ = nullptr;
@@ -199,81 +216,85 @@ pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScala
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename MatScalar> inline void
-pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    const pcl::Correspondences &correspondences,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename MatScalar>
+inline void
+pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                const pcl::Correspondences& correspondences,
+                                Matrix4& transformation_matrix) const
 {
-  const auto nr_correspondences = correspondences.size ();
-  std::vector<int> indices_src (nr_correspondences);
-  std::vector<int> indices_tgt (nr_correspondences);
-  for (std::size_t i = 0; i < nr_correspondences; ++i)
-  {
+  const auto nr_correspondences = correspondences.size();
+  pcl::Indices indices_src(nr_correspondences);
+  pcl::Indices indices_tgt(nr_correspondences);
+  for (std::size_t i = 0; i < nr_correspondences; ++i) {
     indices_src[i] = correspondences[i].index_query;
     indices_tgt[i] = correspondences[i].index_match;
   }
 
-  estimateRigidTransformation (cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
+  estimateRigidTransformation(
+      cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename MatScalar> int 
-pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::OptimizationFunctor::operator () (
-    const VectorX &x, VectorX &fvec) const
+template <typename PointSource, typename PointTarget, typename MatScalar>
+int
+pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::
+    OptimizationFunctor::operator()(const VectorX& x, VectorX& fvec) const
 {
-  const PointCloud<PointSource> & src_points = *estimator_->tmp_src_;
-  const PointCloud<PointTarget> & tgt_points = *estimator_->tmp_tgt_;
+  const PointCloud<PointSource>& src_points = *estimator_->tmp_src_;
+  const PointCloud<PointTarget>& tgt_points = *estimator_->tmp_tgt_;
 
   // Initialize the warp function with the given parameters
-  estimator_->warp_point_->setParam (x);
+  estimator_->warp_point_->setParam(x);
 
-  // Transform each source point and compute its distance to the corresponding target point
-  for (int i = 0; i < values (); ++i)
-  {
-    const PointSource & p_src = src_points[i];
-    const PointTarget & p_tgt = tgt_points[i];
+  // Transform each source point and compute its distance to the corresponding target
+  // point
+  for (int i = 0; i < values(); ++i) {
+    const PointSource& p_src = src_points[i];
+    const PointTarget& p_tgt = tgt_points[i];
 
     // Transform the source point based on the current warp parameters
     Vector4 p_src_warped;
-    estimator_->warp_point_->warpPoint (p_src, p_src_warped);
+    estimator_->warp_point_->warpPoint(p_src, p_src_warped);
 
     // Estimate the distance (cost function)
-    fvec[i] = estimator_->computeDistance (p_src_warped, p_tgt);
+    fvec[i] = estimator_->computeDistance(p_src_warped, p_tgt);
   }
   return (0);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename MatScalar> int
-pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::OptimizationFunctorWithIndices::operator() (
-    const VectorX &x, VectorX &fvec) const
+template <typename PointSource, typename PointTarget, typename MatScalar>
+int
+pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::
+    OptimizationFunctorWithIndices::operator()(const VectorX& x, VectorX& fvec) const
 {
-  const PointCloud<PointSource> & src_points = *estimator_->tmp_src_;
-  const PointCloud<PointTarget> & tgt_points = *estimator_->tmp_tgt_;
-  const std::vector<int> & src_indices = *estimator_->tmp_idx_src_;
-  const std::vector<int> & tgt_indices = *estimator_->tmp_idx_tgt_;
+  const PointCloud<PointSource>& src_points = *estimator_->tmp_src_;
+  const PointCloud<PointTarget>& tgt_points = *estimator_->tmp_tgt_;
+  const pcl::Indices& src_indices = *estimator_->tmp_idx_src_;
+  const pcl::Indices& tgt_indices = *estimator_->tmp_idx_tgt_;
 
   // Initialize the warp function with the given parameters
-  estimator_->warp_point_->setParam (x);
+  estimator_->warp_point_->setParam(x);
 
-  // Transform each source point and compute its distance to the corresponding target point
-  for (int i = 0; i < values (); ++i)
-  {
-    const PointSource & p_src = src_points[src_indices[i]];
-    const PointTarget & p_tgt = tgt_points[tgt_indices[i]];
+  // Transform each source point and compute its distance to the corresponding target
+  // point
+  for (int i = 0; i < values(); ++i) {
+    const PointSource& p_src = src_points[src_indices[i]];
+    const PointTarget& p_tgt = tgt_points[tgt_indices[i]];
 
     // Transform the source point based on the current warp parameters
     Vector4 p_src_warped;
-    estimator_->warp_point_->warpPoint (p_src, p_src_warped);
-    
+    estimator_->warp_point_->warpPoint(p_src, p_src_warped);
+
     // Estimate the distance (cost function)
-    fvec[i] = estimator_->computeDistance (p_src_warped, p_tgt);
+    fvec[i] = estimator_->computeDistance(p_src_warped, p_tgt);
   }
   return (0);
 }
 
-//#define PCL_INSTANTIATE_TransformationEstimationLM(T,U) template class PCL_EXPORTS pcl::registration::TransformationEstimationLM<T,U>;
+//#define PCL_INSTANTIATE_TransformationEstimationLM(T,U) template class PCL_EXPORTS
+// pcl::registration::TransformationEstimationLM<T,U>;
 
 #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_HPP_ */
diff --git a/registration/include/pcl/registration/impl/transformation_estimation_point_to_plane_lls.hpp b/registration/include/pcl/registration/impl/transformation_estimation_point_to_plane_lls.hpp
index df5186c8..18591d32 100644
--- a/registration/include/pcl/registration/impl/transformation_estimation_point_to_plane_lls.hpp
+++ b/registration/include/pcl/registration/impl/transformation_estimation_point_to_plane_lls.hpp
@@ -43,22 +43,19 @@
 
 #include <pcl/cloud_iterator.h>
 
+namespace pcl {
 
-namespace pcl
-{
-
-namespace registration
-{
+namespace registration {
 
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
 TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
-estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
-                             const pcl::PointCloud<PointTarget> &cloud_tgt,
-                             Matrix4 &transformation_matrix) const
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  const auto nr_points = cloud_src.size ();
-  if (cloud_tgt.size () != nr_points)
-  {
+  const auto nr_points = cloud_src.size();
+  if (cloud_tgt.size() != nr_points) {
     PCL_ERROR(
         "[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] "
         "Number or points in source (%zu) differs than target (%zu)!\n",
@@ -67,22 +64,21 @@ estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
     return;
   }
 
-  ConstCloudIterator<PointSource> source_it (cloud_src);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> void
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
 TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
-estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
-                             const std::vector<int> &indices_src,
-                             const pcl::PointCloud<PointTarget> &cloud_tgt,
-                             Matrix4 &transformation_matrix) const
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::Indices& indices_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  const auto nr_points = indices_src.size ();
-  if (cloud_tgt.size () != nr_points)
-  {
+  const auto nr_points = indices_src.size();
+  if (cloud_tgt.size() != nr_points) {
     PCL_ERROR(
         "[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] "
         "Number or points in source (%zu) differs than target (%zu)!\n",
@@ -91,23 +87,22 @@ estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
     return;
   }
 
-  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
 TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
-estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
-                             const std::vector<int> &indices_src,
-                             const pcl::PointCloud<PointTarget> &cloud_tgt,
-                             const std::vector<int> &indices_tgt,
-                             Matrix4 &transformation_matrix) const
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::Indices& indices_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                const pcl::Indices& indices_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  const auto nr_points = indices_src.size ();
-  if (indices_tgt.size () != nr_points)
-  {
+  const auto nr_points = indices_src.size();
+  if (indices_tgt.size() != nr_points) {
     PCL_ERROR(
         "[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] "
         "Number or points in source (%zu) differs than target (%zu)!\n",
@@ -116,93 +111,97 @@ estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
     return;
   }
 
-  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt, indices_tgt);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt, indices_tgt);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
 TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
-estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
-                             const pcl::PointCloud<PointTarget> &cloud_tgt,
-                             const pcl::Correspondences &correspondences,
-                             Matrix4 &transformation_matrix) const
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                const pcl::Correspondences& correspondences,
+                                Matrix4& transformation_matrix) const
 {
-  ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src, correspondences, true);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt, correspondences, false);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
 TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
-constructTransformationMatrix (const double & alpha, const double & beta, const double & gamma,
-                               const double & tx,    const double & ty,   const double & tz,
-                               Matrix4 &transformation_matrix) const
+    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
-  transformation_matrix = Eigen::Matrix<Scalar, 4, 4>::Zero ();
-  transformation_matrix (0, 0) = static_cast<Scalar> ( std::cos (gamma) * std::cos (beta));
-  transformation_matrix (0, 1) = static_cast<Scalar> (-sin (gamma) * std::cos (alpha) + std::cos (gamma) * sin (beta) * sin (alpha));
-  transformation_matrix (0, 2) = static_cast<Scalar> ( sin (gamma) * sin (alpha) + std::cos (gamma) * sin (beta) * std::cos (alpha));
-  transformation_matrix (1, 0) = static_cast<Scalar> ( sin (gamma) * std::cos (beta));
-  transformation_matrix (1, 1) = static_cast<Scalar> ( std::cos (gamma) * std::cos (alpha) + sin (gamma) * sin (beta) * sin (alpha));
-  transformation_matrix (1, 2) = static_cast<Scalar> (-std::cos (gamma) * sin (alpha) + sin (gamma) * sin (beta) * std::cos (alpha));
-  transformation_matrix (2, 0) = static_cast<Scalar> (-sin (beta));
-  transformation_matrix (2, 1) = static_cast<Scalar> ( std::cos (beta) * sin (alpha));
-  transformation_matrix (2, 2) = static_cast<Scalar> ( std::cos (beta) * std::cos (alpha));
-
-  transformation_matrix (0, 3) = static_cast<Scalar> (tx);
-  transformation_matrix (1, 3) = static_cast<Scalar> (ty);
-  transformation_matrix (2, 3) = static_cast<Scalar> (tz);
-  transformation_matrix (3, 3) = static_cast<Scalar> (1);
+  transformation_matrix = Eigen::Matrix<Scalar, 4, 4>::Zero();
+  transformation_matrix(0, 0) = static_cast<Scalar>(std::cos(gamma) * std::cos(beta));
+  transformation_matrix(0, 1) = static_cast<Scalar>(
+      -sin(gamma) * std::cos(alpha) + std::cos(gamma) * sin(beta) * sin(alpha));
+  transformation_matrix(0, 2) = static_cast<Scalar>(
+      sin(gamma) * sin(alpha) + std::cos(gamma) * sin(beta) * std::cos(alpha));
+  transformation_matrix(1, 0) = static_cast<Scalar>(sin(gamma) * std::cos(beta));
+  transformation_matrix(1, 1) = static_cast<Scalar>(
+      std::cos(gamma) * std::cos(alpha) + sin(gamma) * sin(beta) * sin(alpha));
+  transformation_matrix(1, 2) = static_cast<Scalar>(
+      -std::cos(gamma) * sin(alpha) + sin(gamma) * sin(beta) * std::cos(alpha));
+  transformation_matrix(2, 0) = static_cast<Scalar>(-sin(beta));
+  transformation_matrix(2, 1) = static_cast<Scalar>(std::cos(beta) * sin(alpha));
+  transformation_matrix(2, 2) = static_cast<Scalar>(std::cos(beta) * std::cos(alpha));
+
+  transformation_matrix(0, 3) = static_cast<Scalar>(tx);
+  transformation_matrix(1, 3) = static_cast<Scalar>(ty);
+  transformation_matrix(2, 3) = static_cast<Scalar>(tz);
+  transformation_matrix(3, 3) = static_cast<Scalar>(1);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
 TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
-estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it, ConstCloudIterator<PointTarget>& target_it, Matrix4 &transformation_matrix) const
+    estimateRigidTransformation(ConstCloudIterator<PointSource>& source_it,
+                                ConstCloudIterator<PointTarget>& target_it,
+                                Matrix4& transformation_matrix) const
 {
   using Vector6d = Eigen::Matrix<double, 6, 1>;
   using Matrix6d = Eigen::Matrix<double, 6, 6>;
 
   Matrix6d ATA;
   Vector6d ATb;
-  ATA.setZero ();
-  ATb.setZero ();
+  ATA.setZero();
+  ATb.setZero();
 
   // Approximate as a linear least squares problem
-  while (source_it.isValid () && target_it.isValid ())
-  {
-    if (!std::isfinite (source_it->x) ||
-        !std::isfinite (source_it->y) ||
-        !std::isfinite (source_it->z) ||
-        !std::isfinite (target_it->x) ||
-        !std::isfinite (target_it->y) ||
-        !std::isfinite (target_it->z) ||
-        !std::isfinite (target_it->normal_x) ||
-        !std::isfinite (target_it->normal_y) ||
-        !std::isfinite (target_it->normal_z))
-    {
+  while (source_it.isValid() && target_it.isValid()) {
+    if (!std::isfinite(source_it->x) || !std::isfinite(source_it->y) ||
+        !std::isfinite(source_it->z) || !std::isfinite(target_it->x) ||
+        !std::isfinite(target_it->y) || !std::isfinite(target_it->z) ||
+        !std::isfinite(target_it->normal_x) || !std::isfinite(target_it->normal_y) ||
+        !std::isfinite(target_it->normal_z)) {
       ++target_it;
       ++source_it;
       continue;
     }
 
-    const float & sx = source_it->x;
-    const float & sy = source_it->y;
-    const float & sz = source_it->z;
-    const float & dx = target_it->x;
-    const float & dy = target_it->y;
-    const float & dz = target_it->z;
-    const float & nx = target_it->normal[0];
-    const float & ny = target_it->normal[1];
-    const float & nz = target_it->normal[2];
+    const float& sx = source_it->x;
+    const float& sy = source_it->y;
+    const float& sz = source_it->z;
+    const float& dx = target_it->x;
+    const float& dy = target_it->y;
+    const float& dz = target_it->z;
+    const float& nx = target_it->normal[0];
+    const float& ny = target_it->normal[1];
+    const float& nz = target_it->normal[2];
 
-    double a = nz*sy - ny*sz;
-    double b = nx*sz - nz*sx;
-    double c = ny*sx - nx*sy;
+    double a = nz * sy - ny * sz;
+    double b = nx * sz - nz * sx;
+    double c = ny * sx - nx * sy;
 
     //    0  1  2  3  4  5
     //    6  7  8  9 10 11
@@ -211,65 +210,65 @@ estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it, ConstCl
     //   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;
-    ATA.coeffRef (3) += a * nx;
-    ATA.coeffRef (4) += a * ny;
-    ATA.coeffRef (5) += a * nz;
-    ATA.coeffRef (7) += b * b;
-    ATA.coeffRef (8) += b * c;
-    ATA.coeffRef (9) += b * nx;
-    ATA.coeffRef (10) += b * ny;
-    ATA.coeffRef (11) += b * nz;
-    ATA.coeffRef (14) += c * c;
-    ATA.coeffRef (15) += c * nx;
-    ATA.coeffRef (16) += c * ny;
-    ATA.coeffRef (17) += c * nz;
-    ATA.coeffRef (21) += nx * nx;
-    ATA.coeffRef (22) += nx * ny;
-    ATA.coeffRef (23) += nx * nz;
-    ATA.coeffRef (28) += ny * ny;
-    ATA.coeffRef (29) += ny * nz;
-    ATA.coeffRef (35) += nz * nz;
-
-    double d = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
-    ATb.coeffRef (0) += a * d;
-    ATb.coeffRef (1) += b * d;
-    ATb.coeffRef (2) += c * d;
-    ATb.coeffRef (3) += nx * d;
-    ATb.coeffRef (4) += ny * d;
-    ATb.coeffRef (5) += nz * d;
+    ATA.coeffRef(0) += a * a;
+    ATA.coeffRef(1) += a * b;
+    ATA.coeffRef(2) += a * c;
+    ATA.coeffRef(3) += a * nx;
+    ATA.coeffRef(4) += a * ny;
+    ATA.coeffRef(5) += a * nz;
+    ATA.coeffRef(7) += b * b;
+    ATA.coeffRef(8) += b * c;
+    ATA.coeffRef(9) += b * nx;
+    ATA.coeffRef(10) += b * ny;
+    ATA.coeffRef(11) += b * nz;
+    ATA.coeffRef(14) += c * c;
+    ATA.coeffRef(15) += c * nx;
+    ATA.coeffRef(16) += c * ny;
+    ATA.coeffRef(17) += c * nz;
+    ATA.coeffRef(21) += nx * nx;
+    ATA.coeffRef(22) += nx * ny;
+    ATA.coeffRef(23) += nx * nz;
+    ATA.coeffRef(28) += ny * ny;
+    ATA.coeffRef(29) += ny * nz;
+    ATA.coeffRef(35) += nz * nz;
+
+    double d = nx * dx + ny * dy + nz * dz - nx * sx - ny * sy - nz * sz;
+    ATb.coeffRef(0) += a * d;
+    ATb.coeffRef(1) += b * d;
+    ATb.coeffRef(2) += c * d;
+    ATb.coeffRef(3) += nx * d;
+    ATb.coeffRef(4) += ny * d;
+    ATb.coeffRef(5) += nz * d;
 
     ++target_it;
     ++source_it;
   }
 
-  ATA.coeffRef (6) = ATA.coeff (1);
-  ATA.coeffRef (12) = ATA.coeff (2);
-  ATA.coeffRef (13) = ATA.coeff (8);
-  ATA.coeffRef (18) = ATA.coeff (3);
-  ATA.coeffRef (19) = ATA.coeff (9);
-  ATA.coeffRef (20) = ATA.coeff (15);
-  ATA.coeffRef (24) = ATA.coeff (4);
-  ATA.coeffRef (25) = ATA.coeff (10);
-  ATA.coeffRef (26) = ATA.coeff (16);
-  ATA.coeffRef (27) = ATA.coeff (22);
-  ATA.coeffRef (30) = ATA.coeff (5);
-  ATA.coeffRef (31) = ATA.coeff (11);
-  ATA.coeffRef (32) = ATA.coeff (17);
-  ATA.coeffRef (33) = ATA.coeff (23);
-  ATA.coeffRef (34) = ATA.coeff (29);
+  ATA.coeffRef(6) = ATA.coeff(1);
+  ATA.coeffRef(12) = ATA.coeff(2);
+  ATA.coeffRef(13) = ATA.coeff(8);
+  ATA.coeffRef(18) = ATA.coeff(3);
+  ATA.coeffRef(19) = ATA.coeff(9);
+  ATA.coeffRef(20) = ATA.coeff(15);
+  ATA.coeffRef(24) = ATA.coeff(4);
+  ATA.coeffRef(25) = ATA.coeff(10);
+  ATA.coeffRef(26) = ATA.coeff(16);
+  ATA.coeffRef(27) = ATA.coeff(22);
+  ATA.coeffRef(30) = ATA.coeff(5);
+  ATA.coeffRef(31) = ATA.coeff(11);
+  ATA.coeffRef(32) = ATA.coeff(17);
+  ATA.coeffRef(33) = ATA.coeff(23);
+  ATA.coeffRef(34) = ATA.coeff(29);
 
   // Solve A*x = b
-  Vector6d x = static_cast<Vector6d> (ATA.inverse () * ATb);
+  Vector6d x = static_cast<Vector6d>(ATA.inverse() * ATb);
 
   // Construct the transformation matrix from x
-  constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
+  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 72f405a1..bc57d251 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
@@ -42,22 +42,19 @@
 
 #include <pcl/cloud_iterator.h>
 
+namespace pcl {
 
-namespace pcl
-{
-
-namespace registration
-{
+namespace registration {
 
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
 TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
-estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
-                             const pcl::PointCloud<PointTarget> &cloud_tgt,
-                             Matrix4 &transformation_matrix) const
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  const auto nr_points = cloud_src.size ();
-  if (cloud_tgt.size () != nr_points)
-  {
+  const auto nr_points = cloud_src.size();
+  if (cloud_tgt.size() != nr_points) {
     PCL_ERROR("[pcl::TransformationEstimationPointToPlaneLLSWeighted::"
               "estimateRigidTransformation] Number or points in source (%zu) differs "
               "than target (%zu)!\n",
@@ -66,29 +63,29 @@ estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
     return;
   }
 
-  if (weights_.size () != nr_points)
-  {
-    PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n");
+  if (weights_.size() != nr_points) {
+    PCL_ERROR("[pcl::TransformationEstimationPointToPlaneLLSWeighted::"
+              "estimateRigidTransformation] Number or weights from the number of "
+              "correspondences! Use setWeights () to set them.\n");
     return;
   }
 
-  ConstCloudIterator<PointSource> source_it (cloud_src);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
-  typename std::vector<Scalar>::const_iterator weights_it = weights_.begin ();
-  estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt);
+  typename std::vector<Scalar>::const_iterator weights_it = weights_.begin();
+  estimateRigidTransformation(source_it, target_it, weights_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> void
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
 TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
-estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
-                             const std::vector<int> &indices_src,
-                             const pcl::PointCloud<PointTarget> &cloud_tgt,
-                             Matrix4 &transformation_matrix) const
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::Indices& indices_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  const std::size_t nr_points = indices_src.size ();
-  if (cloud_tgt.size () != nr_points)
-  {
+  const std::size_t nr_points = indices_src.size();
+  if (cloud_tgt.size() != nr_points) {
     PCL_ERROR("[pcl::TransformationEstimationPointToPlaneLLSWeighted::"
               "estimateRigidTransformation] Number or points in source (%zu) differs "
               "than target (%zu)!\n",
@@ -97,136 +94,143 @@ estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
     return;
   }
 
-  if (weights_.size () != nr_points)
-  {
-    PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n");
+  if (weights_.size() != nr_points) {
+    PCL_ERROR("[pcl::TransformationEstimationPointToPlaneLLSWeighted::"
+              "estimateRigidTransformation] Number or weights from the number of "
+              "correspondences! Use setWeights () to set them.\n");
     return;
   }
 
-
-  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
-  typename std::vector<Scalar>::const_iterator weights_it = weights_.begin ();
-  estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt);
+  typename std::vector<Scalar>::const_iterator weights_it = weights_.begin();
+  estimateRigidTransformation(source_it, target_it, weights_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
 TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
-estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
-                             const std::vector<int> &indices_src,
-                             const pcl::PointCloud<PointTarget> &cloud_tgt,
-                             const std::vector<int> &indices_tgt,
-                             Matrix4 &transformation_matrix) const
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::Indices& indices_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                const pcl::Indices& indices_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  const std::size_t nr_points = indices_src.size ();
-  if (indices_tgt.size () != nr_points)
-  {
-    PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ());
+  const std::size_t nr_points = indices_src.size();
+  if (indices_tgt.size() != nr_points) {
+    PCL_ERROR("[pcl::TransformationEstimationPointToPlaneLLSWeighted::"
+              "estimateRigidTransformation] Number or points in source (%lu) differs "
+              "than target (%lu)!\n",
+              indices_src.size(),
+              indices_tgt.size());
     return;
   }
 
-  if (weights_.size () != nr_points)
-  {
-    PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n");
+  if (weights_.size() != nr_points) {
+    PCL_ERROR("[pcl::TransformationEstimationPointToPlaneLLSWeighted::"
+              "estimateRigidTransformation] Number or weights from the number of "
+              "correspondences! Use setWeights () to set them.\n");
     return;
   }
 
-  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt, indices_tgt);
-  typename std::vector<Scalar>::const_iterator weights_it = weights_.begin ();
-  estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt, indices_tgt);
+  typename std::vector<Scalar>::const_iterator weights_it = weights_.begin();
+  estimateRigidTransformation(source_it, target_it, weights_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
 TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
-estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
-                             const pcl::PointCloud<PointTarget> &cloud_tgt,
-                             const pcl::Correspondences &correspondences,
-                             Matrix4 &transformation_matrix) const
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                const pcl::Correspondences& correspondences,
+                                Matrix4& transformation_matrix) const
 {
-  ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false);
-  std::vector<Scalar> weights (correspondences.size ());
-  for (std::size_t i = 0; i < correspondences.size (); ++i)
+  ConstCloudIterator<PointSource> source_it(cloud_src, correspondences, true);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt, correspondences, false);
+  std::vector<Scalar> weights(correspondences.size());
+  for (std::size_t i = 0; i < correspondences.size(); ++i)
     weights[i] = correspondences[i].weight;
-  typename std::vector<Scalar>::const_iterator weights_it = weights.begin ();
-  estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
+  typename std::vector<Scalar>::const_iterator weights_it = weights.begin();
+  estimateRigidTransformation(source_it, target_it, weights_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
 TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
-constructTransformationMatrix (const double & alpha, const double & beta, const double & gamma,
-                               const double & tx,    const double & ty,   const double & tz,
-                               Matrix4 &transformation_matrix) const
+    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
-  transformation_matrix = Eigen::Matrix<Scalar, 4, 4>::Zero ();
-  transformation_matrix (0, 0) = static_cast<Scalar> ( std::cos (gamma) * std::cos (beta));
-  transformation_matrix (0, 1) = static_cast<Scalar> (-sin (gamma) * std::cos (alpha) + std::cos (gamma) * sin (beta) * sin (alpha));
-  transformation_matrix (0, 2) = static_cast<Scalar> ( sin (gamma) * sin (alpha) + std::cos (gamma) * sin (beta) * std::cos (alpha));
-  transformation_matrix (1, 0) = static_cast<Scalar> ( sin (gamma) * std::cos (beta));
-  transformation_matrix (1, 1) = static_cast<Scalar> ( std::cos (gamma) * std::cos (alpha) + sin (gamma) * sin (beta) * sin (alpha));
-  transformation_matrix (1, 2) = static_cast<Scalar> (-std::cos (gamma) * sin (alpha) + sin (gamma) * sin (beta) * std::cos (alpha));
-  transformation_matrix (2, 0) = static_cast<Scalar> (-sin (beta));
-  transformation_matrix (2, 1) = static_cast<Scalar> ( std::cos (beta) * sin (alpha));
-  transformation_matrix (2, 2) = static_cast<Scalar> ( std::cos (beta) * std::cos (alpha));
-
-  transformation_matrix (0, 3) = static_cast<Scalar> (tx);
-  transformation_matrix (1, 3) = static_cast<Scalar> (ty);
-  transformation_matrix (2, 3) = static_cast<Scalar> (tz);
-  transformation_matrix (3, 3) = static_cast<Scalar> (1);
+  transformation_matrix = Eigen::Matrix<Scalar, 4, 4>::Zero();
+  transformation_matrix(0, 0) = static_cast<Scalar>(std::cos(gamma) * std::cos(beta));
+  transformation_matrix(0, 1) = static_cast<Scalar>(
+      -sin(gamma) * std::cos(alpha) + std::cos(gamma) * sin(beta) * sin(alpha));
+  transformation_matrix(0, 2) = static_cast<Scalar>(
+      sin(gamma) * sin(alpha) + std::cos(gamma) * sin(beta) * std::cos(alpha));
+  transformation_matrix(1, 0) = static_cast<Scalar>(sin(gamma) * std::cos(beta));
+  transformation_matrix(1, 1) = static_cast<Scalar>(
+      std::cos(gamma) * std::cos(alpha) + sin(gamma) * sin(beta) * sin(alpha));
+  transformation_matrix(1, 2) = static_cast<Scalar>(
+      -std::cos(gamma) * sin(alpha) + sin(gamma) * sin(beta) * std::cos(alpha));
+  transformation_matrix(2, 0) = static_cast<Scalar>(-sin(beta));
+  transformation_matrix(2, 1) = static_cast<Scalar>(std::cos(beta) * sin(alpha));
+  transformation_matrix(2, 2) = static_cast<Scalar>(std::cos(beta) * std::cos(alpha));
+
+  transformation_matrix(0, 3) = static_cast<Scalar>(tx);
+  transformation_matrix(1, 3) = static_cast<Scalar>(ty);
+  transformation_matrix(2, 3) = static_cast<Scalar>(tz);
+  transformation_matrix(3, 3) = static_cast<Scalar>(1);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
 TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
-estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it,
-                             ConstCloudIterator<PointTarget>& target_it,
-                             typename std::vector<Scalar>::const_iterator& weights_it,
-                             Matrix4 &transformation_matrix) const
+    estimateRigidTransformation(
+        ConstCloudIterator<PointSource>& source_it,
+        ConstCloudIterator<PointTarget>& target_it,
+        typename std::vector<Scalar>::const_iterator& weights_it,
+        Matrix4& transformation_matrix) const
 {
   using Vector6d = Eigen::Matrix<double, 6, 1>;
   using Matrix6d = Eigen::Matrix<double, 6, 6>;
 
   Matrix6d ATA;
   Vector6d ATb;
-  ATA.setZero ();
-  ATb.setZero ();
-
-  while (source_it.isValid () && target_it.isValid ())
-  {
-    if (!std::isfinite (source_it->x) ||
-        !std::isfinite (source_it->y) ||
-        !std::isfinite (source_it->z) ||
-        !std::isfinite (target_it->x) ||
-        !std::isfinite (target_it->y) ||
-        !std::isfinite (target_it->z) ||
-        !std::isfinite (target_it->normal_x) ||
-        !std::isfinite (target_it->normal_y) ||
-        !std::isfinite (target_it->normal_z))
-    {
-      ++ source_it;
-      ++ target_it;
-      ++ weights_it;
+  ATA.setZero();
+  ATb.setZero();
+
+  while (source_it.isValid() && target_it.isValid()) {
+    if (!std::isfinite(source_it->x) || !std::isfinite(source_it->y) ||
+        !std::isfinite(source_it->z) || !std::isfinite(target_it->x) ||
+        !std::isfinite(target_it->y) || !std::isfinite(target_it->z) ||
+        !std::isfinite(target_it->normal_x) || !std::isfinite(target_it->normal_y) ||
+        !std::isfinite(target_it->normal_z)) {
+      ++source_it;
+      ++target_it;
+      ++weights_it;
       continue;
     }
 
-    const float & sx = source_it->x;
-    const float & sy = source_it->y;
-    const float & sz = source_it->z;
-    const float & dx = target_it->x;
-    const float & dy = target_it->y;
-    const float & dz = target_it->z;
-    const float & nx = target_it->normal[0] * (*weights_it);
-    const float & ny = target_it->normal[1] * (*weights_it);
-    const float & nz = target_it->normal[2] * (*weights_it);
+    const float& sx = source_it->x;
+    const float& sy = source_it->y;
+    const float& sz = source_it->z;
+    const float& dx = target_it->x;
+    const float& dy = target_it->y;
+    const float& dz = target_it->z;
+    const float& nx = target_it->normal[0] * (*weights_it);
+    const float& ny = target_it->normal[1] * (*weights_it);
+    const float& nz = target_it->normal[2] * (*weights_it);
 
-    double a = nz*sy - ny*sz;
-    double b = nx*sz - nz*sx;
-    double c = ny*sx - nx*sy;
+    double a = nz * sy - ny * sz;
+    double b = nx * sz - nz * sx;
+    double c = ny * sx - nx * sy;
 
     //    0  1  2  3  4  5
     //    6  7  8  9 10 11
@@ -235,66 +239,67 @@ estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it,
     //   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;
-    ATA.coeffRef (3) += a * nx;
-    ATA.coeffRef (4) += a * ny;
-    ATA.coeffRef (5) += a * nz;
-    ATA.coeffRef (7) += b * b;
-    ATA.coeffRef (8) += b * c;
-    ATA.coeffRef (9) += b * nx;
-    ATA.coeffRef (10) += b * ny;
-    ATA.coeffRef (11) += b * nz;
-    ATA.coeffRef (14) += c * c;
-    ATA.coeffRef (15) += c * nx;
-    ATA.coeffRef (16) += c * ny;
-    ATA.coeffRef (17) += c * nz;
-    ATA.coeffRef (21) += nx * nx;
-    ATA.coeffRef (22) += nx * ny;
-    ATA.coeffRef (23) += nx * nz;
-    ATA.coeffRef (28) += ny * ny;
-    ATA.coeffRef (29) += ny * nz;
-    ATA.coeffRef (35) += nz * nz;
-
-    double d = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
-    ATb.coeffRef (0) += a * d;
-    ATb.coeffRef (1) += b * d;
-    ATb.coeffRef (2) += c * d;
-    ATb.coeffRef (3) += nx * d;
-    ATb.coeffRef (4) += ny * d;
-    ATb.coeffRef (5) += nz * d;
-
-    ++ source_it;
-    ++ target_it;
-    ++ weights_it;
+    ATA.coeffRef(0) += a * a;
+    ATA.coeffRef(1) += a * b;
+    ATA.coeffRef(2) += a * c;
+    ATA.coeffRef(3) += a * nx;
+    ATA.coeffRef(4) += a * ny;
+    ATA.coeffRef(5) += a * nz;
+    ATA.coeffRef(7) += b * b;
+    ATA.coeffRef(8) += b * c;
+    ATA.coeffRef(9) += b * nx;
+    ATA.coeffRef(10) += b * ny;
+    ATA.coeffRef(11) += b * nz;
+    ATA.coeffRef(14) += c * c;
+    ATA.coeffRef(15) += c * nx;
+    ATA.coeffRef(16) += c * ny;
+    ATA.coeffRef(17) += c * nz;
+    ATA.coeffRef(21) += nx * nx;
+    ATA.coeffRef(22) += nx * ny;
+    ATA.coeffRef(23) += nx * nz;
+    ATA.coeffRef(28) += ny * ny;
+    ATA.coeffRef(29) += ny * nz;
+    ATA.coeffRef(35) += nz * nz;
+
+    double d = nx * dx + ny * dy + nz * dz - nx * sx - ny * sy - nz * sz;
+    ATb.coeffRef(0) += a * d;
+    ATb.coeffRef(1) += b * d;
+    ATb.coeffRef(2) += c * d;
+    ATb.coeffRef(3) += nx * d;
+    ATb.coeffRef(4) += ny * d;
+    ATb.coeffRef(5) += nz * d;
+
+    ++source_it;
+    ++target_it;
+    ++weights_it;
   }
 
-  ATA.coeffRef (6) = ATA.coeff (1);
-  ATA.coeffRef (12) = ATA.coeff (2);
-  ATA.coeffRef (13) = ATA.coeff (8);
-  ATA.coeffRef (18) = ATA.coeff (3);
-  ATA.coeffRef (19) = ATA.coeff (9);
-  ATA.coeffRef (20) = ATA.coeff (15);
-  ATA.coeffRef (24) = ATA.coeff (4);
-  ATA.coeffRef (25) = ATA.coeff (10);
-  ATA.coeffRef (26) = ATA.coeff (16);
-  ATA.coeffRef (27) = ATA.coeff (22);
-  ATA.coeffRef (30) = ATA.coeff (5);
-  ATA.coeffRef (31) = ATA.coeff (11);
-  ATA.coeffRef (32) = ATA.coeff (17);
-  ATA.coeffRef (33) = ATA.coeff (23);
-  ATA.coeffRef (34) = ATA.coeff (29);
+  ATA.coeffRef(6) = ATA.coeff(1);
+  ATA.coeffRef(12) = ATA.coeff(2);
+  ATA.coeffRef(13) = ATA.coeff(8);
+  ATA.coeffRef(18) = ATA.coeff(3);
+  ATA.coeffRef(19) = ATA.coeff(9);
+  ATA.coeffRef(20) = ATA.coeff(15);
+  ATA.coeffRef(24) = ATA.coeff(4);
+  ATA.coeffRef(25) = ATA.coeff(10);
+  ATA.coeffRef(26) = ATA.coeff(16);
+  ATA.coeffRef(27) = ATA.coeff(22);
+  ATA.coeffRef(30) = ATA.coeff(5);
+  ATA.coeffRef(31) = ATA.coeff(11);
+  ATA.coeffRef(32) = ATA.coeff(17);
+  ATA.coeffRef(33) = ATA.coeff(23);
+  ATA.coeffRef(34) = ATA.coeff(29);
 
   // Solve A*x = b
-  Vector6d x = static_cast<Vector6d> (ATA.inverse () * ATb);
+  Vector6d x = static_cast<Vector6d>(ATA.inverse() * ATb);
 
   // Construct the transformation matrix from x
-  constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
+  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_ */
-
+#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_HPP_  \
+        */
diff --git a/registration/include/pcl/registration/impl/transformation_estimation_point_to_plane_weighted.hpp b/registration/include/pcl/registration/impl/transformation_estimation_point_to_plane_weighted.hpp
index 31e33428..730a285f 100644
--- a/registration/include/pcl/registration/impl/transformation_estimation_point_to_plane_weighted.hpp
+++ b/registration/include/pcl/registration/impl/transformation_estimation_point_to_plane_weighted.hpp
@@ -39,98 +39,107 @@
 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_HPP_
 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_HPP_
 
+#include <pcl/registration/distances.h>
 #include <pcl/registration/warp_point_rigid.h>
 #include <pcl/registration/warp_point_rigid_6d.h>
-#include <pcl/registration/distances.h>
-#include <unsupported/Eigen/NonLinearOptimization>
 
+#include <unsupported/Eigen/NonLinearOptimization>
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointSource, typename PointTarget, typename MatScalar>
-pcl::registration::TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar>::TransformationEstimationPointToPlaneWeighted ()
-  : tmp_src_ ()
-  , tmp_tgt_ ()
-  , tmp_idx_src_ ()
-  , tmp_idx_tgt_ ()
-  , warp_point_ (new WarpPointRigid6D<PointSource, PointTarget, MatScalar>)
-  , use_correspondence_weights_ (true)
-{
-};
+pcl::registration::TransformationEstimationPointToPlaneWeighted<
+    PointSource,
+    PointTarget,
+    MatScalar>::TransformationEstimationPointToPlaneWeighted()
+: tmp_src_()
+, tmp_tgt_()
+, tmp_idx_src_()
+, tmp_idx_tgt_()
+, warp_point_(new WarpPointRigid6D<PointSource, PointTarget, MatScalar>)
+, use_correspondence_weights_(true){};
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename MatScalar> void
-pcl::registration::TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename MatScalar>
+void
+pcl::registration::
+    TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar>::
+        estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                    const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                    Matrix4& transformation_matrix) const
 {
 
   // <cloud_src,cloud_src> is the source dataset
-  if (cloud_src.size () != cloud_tgt.size ())
-  {
-    PCL_ERROR ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] ");
+  if (cloud_src.size() != cloud_tgt.size()) {
+    PCL_ERROR("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
+              "estimateRigidTransformation] ");
     PCL_ERROR("Number or points in source (%zu) differs than target (%zu)!\n",
               static_cast<std::size_t>(cloud_src.size()),
               static_cast<std::size_t>(cloud_tgt.size()));
     return;
   }
-  if (cloud_src.size () < 4)     // need at least 4 samples
+  if (cloud_src.size() < 4) // need at least 4 samples
   {
-    PCL_ERROR ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] ");
+    PCL_ERROR("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
+              "estimateRigidTransformation] ");
     PCL_ERROR("Need at least 4 points to estimate a transform! Source and target have "
               "%zu points!\n",
               static_cast<std::size_t>(cloud_src.size()));
     return;
   }
 
-  if (correspondence_weights_.size () != cloud_src.size ())
-  {
-    PCL_ERROR ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] ");
+  if (correspondence_weights_.size() != cloud_src.size()) {
+    PCL_ERROR("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
+              "estimateRigidTransformation] ");
     PCL_ERROR("Number of weights (%zu) differs than number of points (%zu)!\n",
               correspondence_weights_.size(),
               static_cast<std::size_t>(cloud_src.size()));
     return;
   }
 
-  int n_unknowns = warp_point_->getDimension ();
-  VectorX x (n_unknowns);
-  x.setZero ();
-  
+  int n_unknowns = warp_point_->getDimension();
+  VectorX x(n_unknowns);
+  x.setZero();
+
   // Set temporary pointers
   tmp_src_ = &cloud_src;
   tmp_tgt_ = &cloud_tgt;
 
-  OptimizationFunctor functor (static_cast<int> (cloud_src.size ()), this);
-  Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
-  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, MatScalar> lm (num_diff);
-  int info = lm.minimize (x);
+  OptimizationFunctor functor(static_cast<int>(cloud_src.size()), this);
+  Eigen::NumericalDiff<OptimizationFunctor> num_diff(functor);
+  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, MatScalar> lm(
+      num_diff);
+  int info = lm.minimize(x);
 
   // Compute the norm of the residuals
-  PCL_DEBUG ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation]");
-  PCL_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
-  PCL_DEBUG ("Final solution: [%f", x[0]);
-  for (int i = 1; i < n_unknowns; ++i) 
-    PCL_DEBUG (" %f", x[i]);
-  PCL_DEBUG ("]\n");
+  PCL_DEBUG("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
+            "estimateRigidTransformation]");
+  PCL_DEBUG("LM solver finished with exit code %i, having a residual norm of %g. \n",
+            info,
+            lm.fvec.norm());
+  PCL_DEBUG("Final solution: [%f", x[0]);
+  for (int i = 1; i < n_unknowns; ++i)
+    PCL_DEBUG(" %f", x[i]);
+  PCL_DEBUG("]\n");
 
   // Return the correct transformation
-  warp_point_->setParam (x);
-  transformation_matrix = warp_point_->getTransform ();
+  warp_point_->setParam(x);
+  transformation_matrix = warp_point_->getTransform();
 
   tmp_src_ = NULL;
   tmp_tgt_ = NULL;
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename MatScalar> void
-pcl::registration::TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const std::vector<int> &indices_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename MatScalar>
+void
+pcl::registration::
+    TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar>::
+        estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                    const pcl::Indices& indices_src,
+                                    const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                    Matrix4& transformation_matrix) const
 {
-  if (indices_src.size () != cloud_tgt.size ())
-  {
+  if (indices_src.size() != cloud_tgt.size()) {
     PCL_ERROR("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
               "estimateRigidTransformation] Number or points in source (%zu) differs "
               "than target (%zu)!\n",
@@ -139,63 +148,69 @@ pcl::registration::TransformationEstimationPointToPlaneWeighted<PointSource, Poi
     return;
   }
 
-  if (correspondence_weights_.size () != indices_src.size ())
-  {
-    PCL_ERROR ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] ");
+  if (correspondence_weights_.size() != indices_src.size()) {
+    PCL_ERROR("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
+              "estimateRigidTransformation] ");
     PCL_ERROR("Number of weights (%zu) differs than number of points (%zu)!\n",
               correspondence_weights_.size(),
               indices_src.size());
     return;
   }
 
-
   // <cloud_src,cloud_src> is the source dataset
-  transformation_matrix.setIdentity ();
+  transformation_matrix.setIdentity();
 
-  const auto nr_correspondences = cloud_tgt.size ();
-  std::vector<int> indices_tgt;
+  const auto nr_correspondences = cloud_tgt.size();
+  pcl::Indices indices_tgt;
   indices_tgt.resize(nr_correspondences);
   for (std::size_t i = 0; i < nr_correspondences; ++i)
     indices_tgt[i] = i;
 
-  estimateRigidTransformation(cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
+  estimateRigidTransformation(
+      cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename MatScalar> inline void
-pcl::registration::TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const std::vector<int> &indices_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    const std::vector<int> &indices_tgt,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename MatScalar>
+inline void
+pcl::registration::
+    TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar>::
+        estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                    const pcl::Indices& indices_src,
+                                    const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                    const pcl::Indices& indices_tgt,
+                                    Matrix4& transformation_matrix) const
 {
-  if (indices_src.size () != indices_tgt.size ())
-  {
-    PCL_ERROR ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ());
+  if (indices_src.size() != indices_tgt.size()) {
+    PCL_ERROR("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
+              "estimateRigidTransformation] Number or points in source (%lu) differs "
+              "than target (%lu)!\n",
+              indices_src.size(),
+              indices_tgt.size());
     return;
   }
 
-  if (indices_src.size () < 4)     // need at least 4 samples
+  if (indices_src.size() < 4) // need at least 4 samples
   {
-    PCL_ERROR ("[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] ");
-    PCL_ERROR ("Need at least 4 points to estimate a transform! Source and target have %lu points!",
-               indices_src.size ());
+    PCL_ERROR("[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] ");
+    PCL_ERROR("Need at least 4 points to estimate a transform! Source and target have "
+              "%lu points!\n",
+              indices_src.size());
     return;
   }
 
-  if (correspondence_weights_.size () != indices_src.size ())
-  {
-    PCL_ERROR ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] ");
-    PCL_ERROR ("Number of weights (%lu) differs than number of points (%lu)!\n",
-               correspondence_weights_.size (), indices_src.size ());
+  if (correspondence_weights_.size() != indices_src.size()) {
+    PCL_ERROR("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
+              "estimateRigidTransformation] ");
+    PCL_ERROR("Number of weights (%lu) differs than number of points (%lu)!\n",
+              correspondence_weights_.size(),
+              indices_src.size());
     return;
   }
 
-
-  int n_unknowns = warp_point_->getDimension ();  // get dimension of unknown space
-  VectorX x (n_unknowns);
-  x.setConstant (n_unknowns, 0);
+  int n_unknowns = warp_point_->getDimension(); // get dimension of unknown space
+  VectorX x(n_unknowns);
+  x.setConstant(n_unknowns, 0);
 
   // Set temporary pointers
   tmp_src_ = &cloud_src;
@@ -203,21 +218,27 @@ pcl::registration::TransformationEstimationPointToPlaneWeighted<PointSource, Poi
   tmp_idx_src_ = &indices_src;
   tmp_idx_tgt_ = &indices_tgt;
 
-  OptimizationFunctorWithIndices functor (static_cast<int> (indices_src.size ()), this);
-  Eigen::NumericalDiff<OptimizationFunctorWithIndices> num_diff (functor);
-  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices>, MatScalar> lm (num_diff);
-  int info = lm.minimize (x);
+  OptimizationFunctorWithIndices functor(static_cast<int>(indices_src.size()), this);
+  Eigen::NumericalDiff<OptimizationFunctorWithIndices> num_diff(functor);
+  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices>,
+                            MatScalar>
+      lm(num_diff);
+  int info = lm.minimize(x);
 
   // Compute the norm of the residuals
-  PCL_DEBUG ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
-  PCL_DEBUG ("Final solution: [%f", x[0]);
-  for (int i = 1; i < n_unknowns; ++i) 
-    PCL_DEBUG (" %f", x[i]);
-  PCL_DEBUG ("]\n");
+  PCL_DEBUG("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
+            "estimateRigidTransformation] LM solver finished with exit code %i, having "
+            "a residual norm of %g. \n",
+            info,
+            lm.fvec.norm());
+  PCL_DEBUG("Final solution: [%f", x[0]);
+  for (int i = 1; i < n_unknowns; ++i)
+    PCL_DEBUG(" %f", x[i]);
+  PCL_DEBUG("]\n");
 
   // Return the correct transformation
-  warp_point_->setParam (x);
-  transformation_matrix = warp_point_->getTransform ();
+  warp_point_->setParam(x);
+  transformation_matrix = warp_point_->getTransform();
 
   tmp_src_ = NULL;
   tmp_tgt_ = NULL;
@@ -225,84 +246,94 @@ pcl::registration::TransformationEstimationPointToPlaneWeighted<PointSource, Poi
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename MatScalar> inline void
-pcl::registration::TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    const pcl::Correspondences &correspondences,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename MatScalar>
+inline void
+pcl::registration::
+    TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar>::
+        estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                    const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                    const pcl::Correspondences& correspondences,
+                                    Matrix4& transformation_matrix) const
 {
-  const int nr_correspondences = static_cast<int> (correspondences.size ());
-  std::vector<int> indices_src (nr_correspondences);
-  std::vector<int> indices_tgt (nr_correspondences);
-  for (int i = 0; i < nr_correspondences; ++i)
-  {
+  const int nr_correspondences = static_cast<int>(correspondences.size());
+  pcl::Indices indices_src(nr_correspondences);
+  pcl::Indices indices_tgt(nr_correspondences);
+  for (int i = 0; i < nr_correspondences; ++i) {
     indices_src[i] = correspondences[i].index_query;
     indices_tgt[i] = correspondences[i].index_match;
   }
 
-  if (use_correspondence_weights_)
-  {
-    correspondence_weights_.resize (nr_correspondences);
+  if (use_correspondence_weights_) {
+    correspondence_weights_.resize(nr_correspondences);
     for (std::size_t i = 0; i < nr_correspondences; ++i)
       correspondence_weights_[i] = correspondences[i].weight;
   }
 
-  estimateRigidTransformation (cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
+  estimateRigidTransformation(
+      cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename MatScalar> int 
-pcl::registration::TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar>::OptimizationFunctor::operator () (
-    const VectorX &x, VectorX &fvec) const
+template <typename PointSource, typename PointTarget, typename MatScalar>
+int
+pcl::registration::TransformationEstimationPointToPlaneWeighted<
+    PointSource,
+    PointTarget,
+    MatScalar>::OptimizationFunctor::operator()(const VectorX& x, VectorX& fvec) const
 {
-  const PointCloud<PointSource> & src_points = *estimator_->tmp_src_;
-  const PointCloud<PointTarget> & tgt_points = *estimator_->tmp_tgt_;
+  const PointCloud<PointSource>& src_points = *estimator_->tmp_src_;
+  const PointCloud<PointTarget>& tgt_points = *estimator_->tmp_tgt_;
 
   // Initialize the warp function with the given parameters
-  estimator_->warp_point_->setParam (x);
+  estimator_->warp_point_->setParam(x);
 
-  // Transform each source point and compute its distance to the corresponding target point
-  for (int i = 0; i < values (); ++i)
-  {
-    const PointSource & p_src = src_points[i];
-    const PointTarget & p_tgt = tgt_points[i];
+  // Transform each source point and compute its distance to the corresponding target
+  // point
+  for (int i = 0; i < values(); ++i) {
+    const PointSource& p_src = src_points[i];
+    const PointTarget& p_tgt = tgt_points[i];
 
     // Transform the source point based on the current warp parameters
     Vector4 p_src_warped;
-    estimator_->warp_point_->warpPoint (p_src, p_src_warped);
+    estimator_->warp_point_->warpPoint(p_src, p_src_warped);
 
     // Estimate the distance (cost function)
-    fvec[i] = estimator_->correspondence_weights_[i] * estimator_->computeDistance (p_src_warped, p_tgt);
+    fvec[i] = estimator_->correspondence_weights_[i] *
+              estimator_->computeDistance(p_src_warped, p_tgt);
   }
   return (0);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointSource, typename PointTarget, typename MatScalar> int
-pcl::registration::TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar>::OptimizationFunctorWithIndices::operator() (
-    const VectorX &x, VectorX &fvec) const
+template <typename PointSource, typename PointTarget, typename MatScalar>
+int
+pcl::registration::TransformationEstimationPointToPlaneWeighted<
+    PointSource,
+    PointTarget,
+    MatScalar>::OptimizationFunctorWithIndices::operator()(const VectorX& x,
+                                                           VectorX& fvec) const
 {
-  const PointCloud<PointSource> & src_points = *estimator_->tmp_src_;
-  const PointCloud<PointTarget> & tgt_points = *estimator_->tmp_tgt_;
-  const std::vector<int> & src_indices = *estimator_->tmp_idx_src_;
-  const std::vector<int> & tgt_indices = *estimator_->tmp_idx_tgt_;
+  const PointCloud<PointSource>& src_points = *estimator_->tmp_src_;
+  const PointCloud<PointTarget>& tgt_points = *estimator_->tmp_tgt_;
+  const pcl::Indices& src_indices = *estimator_->tmp_idx_src_;
+  const pcl::Indices& tgt_indices = *estimator_->tmp_idx_tgt_;
 
   // Initialize the warp function with the given parameters
-  estimator_->warp_point_->setParam (x);
+  estimator_->warp_point_->setParam(x);
 
-  // Transform each source point and compute its distance to the corresponding target point
-  for (int i = 0; i < values (); ++i)
-  {
-    const PointSource & p_src = src_points[src_indices[i]];
-    const PointTarget & p_tgt = tgt_points[tgt_indices[i]];
+  // Transform each source point and compute its distance to the corresponding target
+  // point
+  for (int i = 0; i < values(); ++i) {
+    const PointSource& p_src = src_points[src_indices[i]];
+    const PointTarget& p_tgt = tgt_points[tgt_indices[i]];
 
     // Transform the source point based on the current warp parameters
     Vector4 p_src_warped;
-    estimator_->warp_point_->warpPoint (p_src, p_src_warped);
-    
+    estimator_->warp_point_->warpPoint(p_src, p_src_warped);
+
     // Estimate the distance (cost function)
-    fvec[i] = estimator_->correspondence_weights_[i] * estimator_->computeDistance (p_src_warped, p_tgt);
+    fvec[i] = estimator_->correspondence_weights_[i] *
+              estimator_->computeDistance(p_src_warped, p_tgt);
   }
   return (0);
 }
diff --git a/registration/include/pcl/registration/impl/transformation_estimation_svd.hpp b/registration/include/pcl/registration/impl/transformation_estimation_svd.hpp
index d0902dc4..8c726f9c 100644
--- a/registration/include/pcl/registration/impl/transformation_estimation_svd.hpp
+++ b/registration/include/pcl/registration/impl/transformation_estimation_svd.hpp
@@ -43,22 +43,19 @@
 
 #include <pcl/common/eigen.h>
 
+namespace pcl {
 
-namespace pcl
-{
-
-namespace registration
-{
+namespace registration {
 
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
-TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
+TransformationEstimationSVD<PointSource, PointTarget, Scalar>::
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  const auto nr_points = cloud_src.size ();
-  if (cloud_tgt.size () != nr_points)
-  {
+  const auto nr_points = cloud_src.size();
+  if (cloud_tgt.size() != nr_points) {
     PCL_ERROR("[pcl::TransformationEstimationSVD::estimateRigidTransformation] Number "
               "or points in source (%zu) differs than target (%zu)!\n",
               static_cast<std::size_t>(nr_points),
@@ -66,21 +63,20 @@ TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTran
     return;
   }
 
-  ConstCloudIterator<PointSource> source_it (cloud_src);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> void
-TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const std::vector<int> &indices_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
+TransformationEstimationSVD<PointSource, PointTarget, Scalar>::
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::Indices& indices_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  if (indices_src.size () != cloud_tgt.size ())
-  {
+  if (indices_src.size() != cloud_tgt.size()) {
     PCL_ERROR("[pcl::TransformationSVD::estimateRigidTransformation] Number or points "
               "in source (%zu) differs than target (%zu)!\n",
               indices_src.size(),
@@ -88,22 +84,21 @@ TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTran
     return;
   }
 
-  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
-TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const std::vector<int> &indices_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    const std::vector<int> &indices_tgt,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
+TransformationEstimationSVD<PointSource, PointTarget, Scalar>::
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::Indices& indices_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                const pcl::Indices& indices_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  if (indices_src.size () != indices_tgt.size ())
-  {
+  if (indices_src.size() != indices_tgt.size()) {
     PCL_ERROR("[pcl::TransformationEstimationSVD::estimateRigidTransformation] Number "
               "or points in source (%zu) differs than target (%zu)!\n",
               indices_src.size(),
@@ -111,116 +106,120 @@ TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTran
     return;
   }
 
-  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt, indices_tgt);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt, indices_tgt);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> void
-TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
-    const pcl::PointCloud<PointSource> &cloud_src,
-    const pcl::PointCloud<PointTarget> &cloud_tgt,
-    const pcl::Correspondences &correspondences,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
+TransformationEstimationSVD<PointSource, PointTarget, Scalar>::
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                const pcl::Correspondences& correspondences,
+                                Matrix4& transformation_matrix) const
 {
-  ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src, correspondences, true);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt, correspondences, false);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
-TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
-    ConstCloudIterator<PointSource>& source_it,
-    ConstCloudIterator<PointTarget>& target_it,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
+TransformationEstimationSVD<PointSource, PointTarget, Scalar>::
+    estimateRigidTransformation(ConstCloudIterator<PointSource>& source_it,
+                                ConstCloudIterator<PointTarget>& target_it,
+                                Matrix4& transformation_matrix) const
 {
   // Convert to Eigen format
-  const int npts = static_cast <int> (source_it.size ());
-
-
+  const int npts = static_cast<int>(source_it.size());
 
-  if (use_umeyama_)
-  {
-    Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_src (3, npts);
-    Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_tgt (3, npts);
+  if (use_umeyama_) {
+    Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_src(3, npts);
+    Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_tgt(3, npts);
 
-    for (int i = 0; i < npts; ++i)
-    {
-      cloud_src (0, i) = source_it->x;
-      cloud_src (1, i) = source_it->y;
-      cloud_src (2, i) = source_it->z;
+    for (int i = 0; i < npts; ++i) {
+      cloud_src(0, i) = source_it->x;
+      cloud_src(1, i) = source_it->y;
+      cloud_src(2, i) = source_it->z;
       ++source_it;
 
-      cloud_tgt (0, i) = target_it->x;
-      cloud_tgt (1, i) = target_it->y;
-      cloud_tgt (2, i) = target_it->z;
+      cloud_tgt(0, i) = target_it->x;
+      cloud_tgt(1, i) = target_it->y;
+      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);
+    transformation_matrix = pcl::umeyama(cloud_src, cloud_tgt, false);
   }
-  else
-  {
-    source_it.reset (); target_it.reset ();
+  else {
+    source_it.reset();
+    target_it.reset();
     // <cloud_src,cloud_src> is the source dataset
-    transformation_matrix.setIdentity ();
+    transformation_matrix.setIdentity();
 
     Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
     // Estimate the centroids of source, target
-    compute3DCentroid (source_it, centroid_src);
-    compute3DCentroid (target_it, centroid_tgt);
-    source_it.reset (); target_it.reset ();
+    compute3DCentroid(source_it, centroid_src);
+    compute3DCentroid(target_it, centroid_tgt);
+    source_it.reset();
+    target_it.reset();
 
     // Subtract the centroids from source, target
-    Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean, cloud_tgt_demean;
-    demeanPointCloud (source_it, centroid_src, cloud_src_demean);
-    demeanPointCloud (target_it, centroid_tgt, cloud_tgt_demean);
-
-    getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);
+    Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean,
+        cloud_tgt_demean;
+    demeanPointCloud(source_it, centroid_src, cloud_src_demean);
+    demeanPointCloud(target_it, centroid_tgt, cloud_tgt_demean);
+
+    getTransformationFromCorrelation(cloud_src_demean,
+                                     centroid_src,
+                                     cloud_tgt_demean,
+                                     centroid_tgt,
+                                     transformation_matrix);
   }
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> void
-TransformationEstimationSVD<PointSource, PointTarget, Scalar>::getTransformationFromCorrelation (
-    const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_src_demean,
-    const Eigen::Matrix<Scalar, 4, 1> &centroid_src,
-    const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean,
-    const Eigen::Matrix<Scalar, 4, 1> &centroid_tgt,
-    Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
+TransformationEstimationSVD<PointSource, PointTarget, Scalar>::
+    getTransformationFromCorrelation(
+        const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_src_demean,
+        const Eigen::Matrix<Scalar, 4, 1>& centroid_src,
+        const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_tgt_demean,
+        const Eigen::Matrix<Scalar, 4, 1>& centroid_tgt,
+        Matrix4& transformation_matrix) const
 {
-  transformation_matrix.setIdentity ();
+  transformation_matrix.setIdentity();
 
   // Assemble the correlation matrix H = source * target'
-  Eigen::Matrix<Scalar, 3, 3> H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner (3, 3);
+  Eigen::Matrix<Scalar, 3, 3> H =
+      (cloud_src_demean * cloud_tgt_demean.transpose()).topLeftCorner(3, 3);
 
   // Compute the Singular Value Decomposition
-  Eigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3> > svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
-  Eigen::Matrix<Scalar, 3, 3> u = svd.matrixU ();
-  Eigen::Matrix<Scalar, 3, 3> v = svd.matrixV ();
+  Eigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3>> svd(
+      H, Eigen::ComputeFullU | Eigen::ComputeFullV);
+  Eigen::Matrix<Scalar, 3, 3> u = svd.matrixU();
+  Eigen::Matrix<Scalar, 3, 3> v = svd.matrixV();
 
   // Compute R = V * U'
-  if (u.determinant () * v.determinant () < 0)
-  {
+  if (u.determinant() * v.determinant() < 0) {
     for (int x = 0; x < 3; ++x)
-      v (x, 2) *= -1;
+      v(x, 2) *= -1;
   }
 
-  Eigen::Matrix<Scalar, 3, 3> R = v * u.transpose ();
+  Eigen::Matrix<Scalar, 3, 3> R = v * u.transpose();
 
   // Return the correct transformation
-  transformation_matrix.topLeftCorner (3, 3) = R;
-  const Eigen::Matrix<Scalar, 3, 1> Rc (R * centroid_src.head (3));
-  transformation_matrix.block (0, 3, 3, 1) = centroid_tgt.head (3) - Rc;
+  transformation_matrix.topLeftCorner(3, 3) = R;
+  const Eigen::Matrix<Scalar, 3, 1> Rc(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<T,U>;
+//#define PCL_INSTANTIATE_TransformationEstimationSVD(T,U) template class PCL_EXPORTS
+// pcl::registration::TransformationEstimationSVD<T,U>;
 
 #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 e6b9327d..51a5b217 100644
--- a/registration/include/pcl/registration/impl/transformation_estimation_svd_scale.hpp
+++ b/registration/include/pcl/registration/impl/transformation_estimation_svd_scale.hpp
@@ -40,71 +40,71 @@
 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_SCALE_HPP_
 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_SCALE_HPP_
 
-
-namespace pcl
-{
-
-namespace registration
-{
-
-template <typename PointSource, typename PointTarget, typename Scalar> void
-TransformationEstimationSVDScale<PointSource, PointTarget, Scalar>::getTransformationFromCorrelation (
-    const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_src_demean,
-    const Eigen::Matrix<Scalar, 4, 1> &centroid_src,
-    const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean,
-    const Eigen::Matrix<Scalar, 4, 1> &centroid_tgt,
-    Matrix4 &transformation_matrix) const
+namespace pcl {
+
+namespace registration {
+
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
+TransformationEstimationSVDScale<PointSource, PointTarget, Scalar>::
+    getTransformationFromCorrelation(
+        const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_src_demean,
+        const Eigen::Matrix<Scalar, 4, 1>& centroid_src,
+        const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_tgt_demean,
+        const Eigen::Matrix<Scalar, 4, 1>& centroid_tgt,
+        Matrix4& transformation_matrix) const
 {
-  transformation_matrix.setIdentity ();
+  transformation_matrix.setIdentity();
 
   // Assemble the correlation matrix H = source * target'
-  Eigen::Matrix<Scalar, 3, 3> H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner (3, 3);
+  Eigen::Matrix<Scalar, 3, 3> H =
+      (cloud_src_demean * cloud_tgt_demean.transpose()).topLeftCorner(3, 3);
 
   // Compute the Singular Value Decomposition
-  Eigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3> > svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
-  Eigen::Matrix<Scalar, 3, 3> u = svd.matrixU ();
-  Eigen::Matrix<Scalar, 3, 3> v = svd.matrixV ();
+  Eigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3>> svd(
+      H, Eigen::ComputeFullU | Eigen::ComputeFullV);
+  Eigen::Matrix<Scalar, 3, 3> u = svd.matrixU();
+  Eigen::Matrix<Scalar, 3, 3> v = svd.matrixV();
 
   // Compute R = V * U'
-  if (u.determinant () * v.determinant () < 0)
-  {
+  if (u.determinant() * v.determinant() < 0) {
     for (int x = 0; x < 3; ++x)
-      v (x, 2) *= -1;
+      v(x, 2) *= -1;
   }
 
-  Eigen::Matrix<Scalar, 3, 3> R = v * u.transpose ();
+  Eigen::Matrix<Scalar, 3, 3> R = v * u.transpose();
 
   // rotated cloud
   Eigen::Matrix<Scalar, 4, 4> R4;
-  R4.block (0, 0, 3, 3) = R;
-  R4 (0, 3) = 0;
-  R4 (1, 3) = 0;
-  R4 (2, 3) = 0;
-  R4 (3, 3) = 1;
+  R4.block(0, 0, 3, 3) = R;
+  R4(0, 3) = 0;
+  R4(1, 3) = 0;
+  R4(2, 3) = 0;
+  R4(3, 3) = 1;
 
   Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> src_ = R4 * cloud_src_demean;
 
   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) * src_ (0, corrIdx);
-    sum_tt += cloud_tgt_demean (1, corrIdx) * src_ (1, corrIdx);
-    sum_tt += cloud_tgt_demean (2, corrIdx) * src_ (2, corrIdx);
+  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) * src_(0, corrIdx);
+    sum_tt += cloud_tgt_demean(1, corrIdx) * src_(1, corrIdx);
+    sum_tt += cloud_tgt_demean(2, corrIdx) * src_(2, corrIdx);
   }
 
   float scale = sum_tt / sum_ss;
-  transformation_matrix.topLeftCorner (3, 3) = scale * R;
-  const Eigen::Matrix<Scalar, 3, 1> Rc (scale * R * centroid_src.head (3));
-  transformation_matrix.block (0, 3, 3, 1) = centroid_tgt. head (3) - Rc;
+  transformation_matrix.topLeftCorner(3, 3) = scale * R;
+  const Eigen::Matrix<Scalar, 3, 1> 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<T,U>;
+//#define PCL_INSTANTIATE_TransformationEstimationSVD(T,U) template class PCL_EXPORTS
+// pcl::registration::TransformationEstimationSVD<T,U>;
 
 #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 fe24d5fc..261e48fd 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,22 +39,19 @@
 
 #include <pcl/cloud_iterator.h>
 
+namespace pcl {
 
-namespace pcl
-{
-
-namespace registration
-{
+namespace registration {
 
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
 TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
-estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
-                             const pcl::PointCloud<PointTarget> &cloud_tgt,
-                             Matrix4 &transformation_matrix) const
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  const auto nr_points = cloud_src.size ();
-  if (cloud_tgt.size () != nr_points)
-  {
+  const auto nr_points = cloud_src.size();
+  if (cloud_tgt.size() != nr_points) {
     PCL_ERROR("[pcl::TransformationEstimationSymmetricPointToPlaneLLS::"
               "estimateRigidTransformation] Number or points in source (%zu) differs "
               "from target (%zu)!\n",
@@ -63,22 +60,21 @@ estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
     return;
   }
 
-  ConstCloudIterator<PointSource> source_it (cloud_src);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> void
+template <typename PointSource, typename PointTarget, typename Scalar>
+void
 TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
-estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
-                             const std::vector<int> &indices_src,
-                             const pcl::PointCloud<PointTarget> &cloud_tgt,
-                             Matrix4 &transformation_matrix) const
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::Indices& indices_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  const auto nr_points = indices_src.size ();
-  if (cloud_tgt.size () != nr_points)
-  {
+  const auto nr_points = indices_src.size();
+  if (cloud_tgt.size() != nr_points) {
     PCL_ERROR("[pcl::TransformationEstimationSymmetricPointToPlaneLLS::"
               "estimateRigidTransformation] Number or points in source (%zu) differs "
               "than target (%zu)!\n",
@@ -87,23 +83,22 @@ estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
     return;
   }
 
-  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
 TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
-estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
-                             const std::vector<int> &indices_src,
-                             const pcl::PointCloud<PointTarget> &cloud_tgt,
-                             const std::vector<int> &indices_tgt,
-                             Matrix4 &transformation_matrix) const
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::Indices& indices_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                const pcl::Indices& indices_tgt,
+                                Matrix4& transformation_matrix) const
 {
-  const auto nr_points = indices_src.size ();
-  if (indices_tgt.size () != nr_points)
-  {
+  const auto nr_points = indices_src.size();
+  if (indices_tgt.size() != nr_points) {
     PCL_ERROR("[pcl::TransformationEstimationSymmetricPointToPlaneLLS::"
               "estimateRigidTransformation] Number or points in source (%zu) differs "
               "than target (%zu)!\n",
@@ -112,114 +107,114 @@ estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
     return;
   }
 
-  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt, indices_tgt);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt, indices_tgt);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
 TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
-estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
-                             const pcl::PointCloud<PointTarget> &cloud_tgt,
-                             const pcl::Correspondences &correspondences,
-                             Matrix4 &transformation_matrix) const
+    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                                const pcl::PointCloud<PointTarget>& cloud_tgt,
+                                const pcl::Correspondences& correspondences,
+                                Matrix4& transformation_matrix) const
 {
-  ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true);
-  ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false);
-  estimateRigidTransformation (source_it, target_it, transformation_matrix);
+  ConstCloudIterator<PointSource> source_it(cloud_src, correspondences, true);
+  ConstCloudIterator<PointTarget> target_it(cloud_tgt, correspondences, false);
+  estimateRigidTransformation(source_it, target_it, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
 TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
-constructTransformationMatrix (const Vector6 &parameters,
-                               Matrix4 &transformation_matrix) const
+    constructTransformationMatrix(const Vector6& parameters,
+                                  Matrix4& transformation_matrix) const
 {
   // Construct the transformation matrix from rotation and translation
-  const Eigen::AngleAxis<Scalar> rotation_z (parameters (2), Eigen::Matrix<Scalar, 3, 1>::UnitZ ());
-  const Eigen::AngleAxis<Scalar> rotation_y (parameters (1), Eigen::Matrix<Scalar, 3, 1>::UnitY ());
-  const Eigen::AngleAxis<Scalar> rotation_x (parameters (0), Eigen::Matrix<Scalar, 3, 1>::UnitX ());
-  const Eigen::Translation<Scalar, 3> translation (parameters (3), parameters (4), parameters (5));
-  const Eigen::Transform<Scalar, 3, Eigen::Affine> transform = rotation_z * rotation_y * rotation_x *
-                                                               translation *
-                                                               rotation_z * rotation_y * rotation_x;
-  transformation_matrix = transform.matrix ();
+  const Eigen::AngleAxis<Scalar> rotation_z(parameters(2),
+                                            Eigen::Matrix<Scalar, 3, 1>::UnitZ());
+  const Eigen::AngleAxis<Scalar> rotation_y(parameters(1),
+                                            Eigen::Matrix<Scalar, 3, 1>::UnitY());
+  const Eigen::AngleAxis<Scalar> rotation_x(parameters(0),
+                                            Eigen::Matrix<Scalar, 3, 1>::UnitX());
+  const Eigen::Translation<Scalar, 3> translation(
+      parameters(3), parameters(4), parameters(5));
+  const Eigen::Transform<Scalar, 3, Eigen::Affine> transform =
+      rotation_z * rotation_y * rotation_x * translation * rotation_z * rotation_y *
+      rotation_x;
+  transformation_matrix = transform.matrix();
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
 TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
-estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it, ConstCloudIterator<PointTarget>& target_it, Matrix4 &transformation_matrix) const
+    estimateRigidTransformation(ConstCloudIterator<PointSource>& source_it,
+                                ConstCloudIterator<PointTarget>& target_it,
+                                Matrix4& transformation_matrix) const
 {
   using Matrix6 = Eigen::Matrix<Scalar, 6, 6>;
   using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
 
   Matrix6 ATA;
   Vector6 ATb;
-  ATA.setZero ();
-  ATb.setZero ();
-  auto M = ATA.template selfadjointView<Eigen::Upper> ();
+  ATA.setZero();
+  ATb.setZero();
+  auto M = ATA.template selfadjointView<Eigen::Upper>();
 
   // Approximate as a linear least squares problem
-  source_it.reset ();
-  target_it.reset ();
-  for (; source_it.isValid () && target_it.isValid (); ++source_it, ++target_it)
-  {
-    const Vector3 p (source_it->x, source_it->y, source_it->z);
-    const Vector3 q (target_it->x, target_it->y, target_it->z);
-    const Vector3 n1 (source_it->getNormalVector3fMap().template cast<Scalar>());
-    const Vector3 n2 (target_it->getNormalVector3fMap().template cast<Scalar>());
+  source_it.reset();
+  target_it.reset();
+  for (; source_it.isValid() && target_it.isValid(); ++source_it, ++target_it) {
+    const Vector3 p(source_it->x, source_it->y, source_it->z);
+    const Vector3 q(target_it->x, target_it->y, target_it->z);
+    const Vector3 n1(source_it->getNormalVector3fMap().template cast<Scalar>());
+    const Vector3 n2(target_it->getNormalVector3fMap().template cast<Scalar>());
     Vector3 n;
-    if (enforce_same_direction_normals_)
-    {
-        if (n1.dot (n2) >= 0.)
-            n = n1 + n2;
-        else
-            n = n1 - n2;
-    }
-    else
-    {
+    if (enforce_same_direction_normals_) {
+      if (n1.dot(n2) >= 0.)
         n = n1 + n2;
+      else
+        n = n1 - n2;
+    }
+    else {
+      n = n1 + n2;
     }
 
-    if (!p.array().isFinite().all() ||
-        !q.array().isFinite().all() ||
-        !n.array().isFinite().all())
-    {
+    if (!p.array().isFinite().all() || !q.array().isFinite().all() ||
+        !n.array().isFinite().all()) {
       continue;
     }
 
     Vector6 v;
-    v << (p + q).cross (n), n;
-    M.rankUpdate (v);
+    v << (p + q).cross(n), n;
+    M.rankUpdate(v);
 
-    ATb += v * (q - p).dot (n);
+    ATb += v * (q - p).dot(n);
   }
 
   // Solve A*x = b
-  const Vector6 x = M.ldlt ().solve (ATb);
+  const Vector6 x = M.ldlt().solve(ATb);
 
   // Construct the transformation matrix from x
-  constructTransformationMatrix (x, transformation_matrix);
+  constructTransformationMatrix(x, transformation_matrix);
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline void
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline void
 TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
-setEnforceSameDirectionNormals (bool enforce_same_direction_normals)
+    setEnforceSameDirectionNormals(bool enforce_same_direction_normals)
 {
-    enforce_same_direction_normals_ = enforce_same_direction_normals;
+  enforce_same_direction_normals_ = enforce_same_direction_normals;
 }
 
-
-template <typename PointSource, typename PointTarget, typename Scalar> inline bool
+template <typename PointSource, typename PointTarget, typename Scalar>
+inline bool
 TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
-getEnforceSameDirectionNormals ()
+    getEnforceSameDirectionNormals()
 {
-    return enforce_same_direction_normals_;
+  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 10799a59..67b2c07f 100644
--- a/registration/include/pcl/registration/impl/transformation_validation_euclidean.hpp
+++ b/registration/include/pcl/registration/impl/transformation_validation_euclidean.hpp
@@ -41,50 +41,51 @@
 #ifndef PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_IMPL_H_
 #define PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_IMPL_H_
 
+namespace pcl {
 
-namespace pcl
-{
-
-namespace registration
-{
+namespace registration {
 
-template <typename PointSource, typename PointTarget, typename Scalar> double
-TransformationValidationEuclidean<PointSource, PointTarget, Scalar>::validateTransformation (
-  const PointCloudSourceConstPtr &cloud_src,
-  const PointCloudTargetConstPtr &cloud_tgt,
-  const Matrix4 &transformation_matrix) const
+template <typename PointSource, typename PointTarget, typename Scalar>
+double
+TransformationValidationEuclidean<PointSource, PointTarget, Scalar>::
+    validateTransformation(const PointCloudSourceConstPtr& cloud_src,
+                           const PointCloudTargetConstPtr& cloud_tgt,
+                           const Matrix4& transformation_matrix) const
 {
   double fitness_score = 0.0;
 
   // Transform the input dataset using the final transformation
   pcl::PointCloud<PointSource> input_transformed;
-  //transformPointCloud (*cloud_src, input_transformed, transformation_matrix);
-  input_transformed.resize (cloud_src->size ());
-  for (std::size_t i = 0; i < cloud_src->size (); ++i)
-  {
-    const PointSource &src = (*cloud_src)[i];
-    PointTarget &tgt = input_transformed[i];
-    tgt.x = static_cast<float> (transformation_matrix (0, 0) * src.x + transformation_matrix (0, 1) * src.y + transformation_matrix (0, 2) * src.z + transformation_matrix (0, 3));
-    tgt.y = static_cast<float> (transformation_matrix (1, 0) * src.x + transformation_matrix (1, 1) * src.y + transformation_matrix (1, 2) * src.z + transformation_matrix (1, 3));
-    tgt.z = static_cast<float> (transformation_matrix (2, 0) * src.x + transformation_matrix (2, 1) * src.y + transformation_matrix (2, 2) * src.z + transformation_matrix (2, 3));
-   }
+  // transformPointCloud (*cloud_src, input_transformed, transformation_matrix);
+  input_transformed.resize(cloud_src->size());
+  for (std::size_t i = 0; i < cloud_src->size(); ++i) {
+    const PointSource& src = (*cloud_src)[i];
+    PointTarget& tgt = input_transformed[i];
+    tgt.x = static_cast<float>(
+        transformation_matrix(0, 0) * src.x + transformation_matrix(0, 1) * src.y +
+        transformation_matrix(0, 2) * src.z + transformation_matrix(0, 3));
+    tgt.y = static_cast<float>(
+        transformation_matrix(1, 0) * src.x + transformation_matrix(1, 1) * src.y +
+        transformation_matrix(1, 2) * src.z + transformation_matrix(1, 3));
+    tgt.z = static_cast<float>(
+        transformation_matrix(2, 0) * src.x + transformation_matrix(2, 1) * src.y +
+        transformation_matrix(2, 2) * src.z + transformation_matrix(2, 3));
+  }
 
-  typename MyPointRepresentation::ConstPtr point_rep (new MyPointRepresentation);
-  if (!force_no_recompute_)
-  {
-    tree_->setPointRepresentation (point_rep);
-    tree_->setInputCloud (cloud_tgt);
+  typename MyPointRepresentation::ConstPtr point_rep(new MyPointRepresentation);
+  if (!force_no_recompute_) {
+    tree_->setPointRepresentation(point_rep);
+    tree_->setInputCloud(cloud_tgt);
   }
 
-  std::vector<int> nn_indices (1);
-  std::vector<float> nn_dists (1);
+  pcl::Indices nn_indices(1);
+  std::vector<float> nn_dists(1);
 
   // For each point in the source dataset
   int nr = 0;
-  for (const auto& point: input_transformed)
-  {
+  for (const auto& point : input_transformed) {
     // Find its nearest neighbor in the target
-    tree_->nearestKSearch (point, 1, nn_indices, nn_dists);
+    tree_->nearestKSearch(point, 1, nn_indices, nn_dists);
 
     // Deal with occlusions (incomplete targets)
     if (nn_dists[0] > max_range_)
@@ -97,11 +98,10 @@ TransformationValidationEuclidean<PointSource, PointTarget, Scalar>::validateTra
 
   if (nr > 0)
     return (fitness_score / nr);
-  return (std::numeric_limits<double>::max ());
+  return (std::numeric_limits<double>::max());
 }
 
 } // namespace registration
 } // namespace pcl
 
-#endif    // PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_IMPL_H_
-
+#endif // PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_IMPL_H_
diff --git a/registration/include/pcl/registration/incremental_registration.h b/registration/include/pcl/registration/incremental_registration.h
index 515746d1..ca5b1a64 100644
--- a/registration/include/pcl/registration/incremental_registration.h
+++ b/registration/include/pcl/registration/incremental_registration.h
@@ -37,91 +37,93 @@
 
 #pragma once
 
-#include <pcl/point_cloud.h>
 #include <pcl/registration/registration.h>
+#include <pcl/point_cloud.h>
 
 namespace pcl {
-  namespace registration {
-
-    /** \brief Incremental @ref IterativeClosestPoint class
-      *
-      * This class provides a way to register a stream of clouds where each cloud will be aligned to the previous cloud.
-      *
-      * \code
-      * IterativeClosestPoint<PointXYZ,PointXYZ>::Ptr icp (new IterativeClosestPoint<PointXYZ,PointXYZ>);
-      * icp->setMaxCorrespondenceDistance (0.05);
-      * icp->setMaximumIterations (50);
-      *
-      * IncrementalRegistration<PointXYZ> iicp;
-      * iicp.setRegistration (icp);
-      *
-      * while (true){
-      *   PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
-      *   read_cloud (*cloud);
-      *   iicp.registerCloud (cloud);
-      *
-      *   PointCloud<PointXYZ>::Ptr tmp (new PointCloud<PointXYZ>);
-      *   transformPointCloud (*cloud, *tmp, iicp.getAbsoluteTransform ());
-      *   write_cloud (*tmp);
-      * }
-      * \endcode
-      *
-      * \author Michael 'v4hn' Goerner
-      * \ingroup registration
-      */
-    template <typename PointT, typename Scalar = float>
-    class IncrementalRegistration {
-      public:
-        using PointCloudPtr = typename pcl::PointCloud<PointT>::Ptr;
-        using PointCloudConstPtr = typename pcl::PointCloud<PointT>::ConstPtr;
-
-        using RegistrationPtr = typename pcl::Registration<PointT,PointT,Scalar>::Ptr;
-        using Matrix4 = typename pcl::Registration<PointT,PointT,Scalar>::Matrix4;
-
-        IncrementalRegistration ();
-
-        /** \brief Empty destructor */
-        virtual ~IncrementalRegistration () {}
-
-        /** \brief Register new point cloud incrementally
-          * \note You have to set a valid registration object with @ref setRegistration before using this
-          * \note The class doesn't copy cloud. If you afterwards change cloud, that will affect this class.
-          * \param[in] cloud point cloud to register
-          * \param[in] delta_estimate estimated transform between last registered cloud and this one
-          * \return true if registration converged
-          */
-        bool
-        registerCloud (const PointCloudConstPtr& cloud, const Matrix4& delta_estimate = Matrix4::Identity ());
-
-        /** \brief Get estimated transform between the last two registered clouds */
-        inline Matrix4
-        getDeltaTransform () const;
-
-        /** \brief Get estimated overall transform */
-        inline Matrix4
-        getAbsoluteTransform () const;
-
-        /** \brief Reset incremental Registration without resetting registration_ */
-        inline void
-        reset ();
-
-        /** \brief Set registration instance used to align clouds */
-        inline void
-        setRegistration (RegistrationPtr);
-      protected:
-
-        /** \brief last registered point cloud */
-        PointCloudConstPtr last_cloud_;
-
-        /** \brief registration instance to align clouds */
-        RegistrationPtr registration_;
-
-        /** \brief estimated transforms */
-        Matrix4 delta_transform_;
-        Matrix4 abs_transform_;
-    };
-
-  }
-}
+namespace registration {
+
+/** \brief Incremental @ref IterativeClosestPoint class
+ *
+ * This class provides a way to register a stream of clouds where each cloud will be
+ * aligned to the previous cloud.
+ *
+ * \code
+ * IterativeClosestPoint<PointXYZ,PointXYZ>::Ptr icp
+ *   (new IterativeClosestPoint<PointXYZ,PointXYZ>);
+ * icp->setMaxCorrespondenceDistance (0.05);
+ * icp->setMaximumIterations (50);
+ *
+ * IncrementalRegistration<PointXYZ> iicp;
+ * iicp.setRegistration (icp);
+ *
+ * while (true){
+ *   PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
+ *   read_cloud (*cloud);
+ *   iicp.registerCloud (cloud);
+ *
+ *   PointCloud<PointXYZ>::Ptr tmp (new PointCloud<PointXYZ>);
+ *   transformPointCloud (*cloud, *tmp, iicp.getAbsoluteTransform ());
+ *   write_cloud (*tmp);
+ * }
+ * \endcode
+ *
+ * \author Michael 'v4hn' Goerner
+ * \ingroup registration
+ */
+template <typename PointT, typename Scalar = float>
+class IncrementalRegistration {
+public:
+  using PointCloudPtr = typename pcl::PointCloud<PointT>::Ptr;
+  using PointCloudConstPtr = typename pcl::PointCloud<PointT>::ConstPtr;
+
+  using RegistrationPtr = typename pcl::Registration<PointT, PointT, Scalar>::Ptr;
+  using Matrix4 = typename pcl::Registration<PointT, PointT, Scalar>::Matrix4;
+
+  IncrementalRegistration();
+
+  /** \brief Empty destructor */
+  virtual ~IncrementalRegistration() {}
+
+  /** \brief Register new point cloud incrementally
+   * \note You have to set a valid registration object with @ref setRegistration before
+   * using this \note The class doesn't copy cloud. If you afterwards change cloud, that
+   * will affect this class. \param[in] cloud point cloud to register \param[in]
+   * delta_estimate estimated transform between last registered cloud and this one
+   * \return true if registration converged
+   */
+  bool
+  registerCloud(const PointCloudConstPtr& cloud,
+                const Matrix4& delta_estimate = Matrix4::Identity());
+
+  /** \brief Get estimated transform between the last two registered clouds */
+  inline Matrix4
+  getDeltaTransform() const;
+
+  /** \brief Get estimated overall transform */
+  inline Matrix4
+  getAbsoluteTransform() const;
+
+  /** \brief Reset incremental Registration without resetting registration_ */
+  inline void
+  reset();
+
+  /** \brief Set registration instance used to align clouds */
+  inline void setRegistration(RegistrationPtr);
+
+protected:
+  /** \brief last registered point cloud */
+  PointCloudConstPtr last_cloud_;
+
+  /** \brief registration instance to align clouds */
+  RegistrationPtr registration_;
+
+  /** \brief estimated transforms */
+  Matrix4 delta_transform_;
+  Matrix4 abs_transform_;
+};
+
+} // namespace registration
+} // namespace pcl
 
 #include <pcl/registration/impl/incremental_registration.hpp>
diff --git a/registration/include/pcl/registration/joint_icp.h b/registration/include/pcl/registration/joint_icp.h
index d4b1ca84..0475a8af 100644
--- a/registration/include/pcl/registration/joint_icp.h
+++ b/registration/include/pcl/registration/joint_icp.h
@@ -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
@@ -40,189 +40,209 @@
 
 // PCL includes
 #include <pcl/registration/icp.h>
-namespace pcl
-{
-  /** \brief @b JointIterativeClosestPoint extends ICP to multiple frames which
-   *  share the same transform. This is particularly useful when solving for 
-   *  camera extrinsics using multiple observations. When given a single pair of 
-   *  clouds, this reduces to vanilla ICP.
-    *
-    * \author Stephen Miller
-    * \ingroup registration
-    */
-  template <typename PointSource, typename PointTarget, typename Scalar = float>
-  class JointIterativeClosestPoint : public IterativeClosestPoint<PointSource, PointTarget, Scalar>
+namespace pcl {
+/** \brief @b JointIterativeClosestPoint extends ICP to multiple frames which
+ *  share the same transform. This is particularly useful when solving for
+ *  camera extrinsics using multiple observations. When given a single pair of
+ *  clouds, this reduces to vanilla ICP.
+ *
+ * \author Stephen Miller
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename Scalar = float>
+class JointIterativeClosestPoint
+: public IterativeClosestPoint<PointSource, PointTarget, Scalar> {
+public:
+  using PointCloudSource = typename IterativeClosestPoint<PointSource,
+                                                          PointTarget,
+                                                          Scalar>::PointCloudSource;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+
+  using PointCloudTarget = typename IterativeClosestPoint<PointSource,
+                                                          PointTarget,
+                                                          Scalar>::PointCloudTarget;
+  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
+  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
+
+  using KdTree = pcl::search::KdTree<PointTarget>;
+  using KdTreePtr = typename KdTree::Ptr;
+
+  using KdTreeReciprocal = pcl::search::KdTree<PointSource>;
+  using KdTreeReciprocalPtr = typename KdTree::Ptr;
+
+  using PointIndicesPtr = PointIndices::Ptr;
+  using PointIndicesConstPtr = PointIndices::ConstPtr;
+
+  using Ptr = shared_ptr<JointIterativeClosestPoint<PointSource, PointTarget, Scalar>>;
+  using ConstPtr =
+      shared_ptr<const JointIterativeClosestPoint<PointSource, PointTarget, Scalar>>;
+
+  using CorrespondenceEstimation =
+      pcl::registration::CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>;
+  using CorrespondenceEstimationPtr = typename CorrespondenceEstimation::Ptr;
+  using CorrespondenceEstimationConstPtr = typename CorrespondenceEstimation::ConstPtr;
+
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::reg_name_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::getClassName;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::setInputSource;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::input_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::indices_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::target_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::nr_iterations_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::max_iterations_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
+      previous_transformation_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::final_transformation_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::transformation_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
+      transformation_epsilon_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::converged_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::inlier_threshold_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
+      min_number_correspondences_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::update_visualizer_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
+      euclidean_fitness_epsilon_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::correspondences_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
+      transformation_estimation_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
+      correspondence_estimation_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
+      correspondence_rejectors_;
+
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
+      use_reciprocal_correspondence_;
+
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::convergence_criteria_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::source_has_normals_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::target_has_normals_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::need_source_blob_;
+  using IterativeClosestPoint<PointSource, PointTarget, Scalar>::need_target_blob_;
+
+  using Matrix4 =
+      typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::Matrix4;
+
+  /** \brief Empty constructor. */
+  JointIterativeClosestPoint()
   {
-    public:
-      using PointCloudSource = typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::PointCloudSource;
-      using PointCloudSourcePtr = typename PointCloudSource::Ptr;
-      using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
-
-      using PointCloudTarget = typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::PointCloudTarget;
-      using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
-      using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
-
-      using KdTree = pcl::search::KdTree<PointTarget>;
-      using KdTreePtr = typename KdTree::Ptr;
-
-      using KdTreeReciprocal = pcl::search::KdTree<PointSource>;
-      using KdTreeReciprocalPtr = typename KdTree::Ptr;
-
-
-      using PointIndicesPtr = PointIndices::Ptr;
-      using PointIndicesConstPtr = PointIndices::ConstPtr;
-
-      using Ptr = shared_ptr<JointIterativeClosestPoint<PointSource, PointTarget, Scalar> >;
-      using ConstPtr = shared_ptr<const JointIterativeClosestPoint<PointSource, PointTarget, Scalar> >;
-
-      using CorrespondenceEstimation = pcl::registration::CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>;
-      using CorrespondenceEstimationPtr = typename CorrespondenceEstimation::Ptr;
-      using CorrespondenceEstimationConstPtr = typename CorrespondenceEstimation::ConstPtr;
-
-
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::reg_name_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::getClassName;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::setInputSource;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::input_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::indices_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::target_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::nr_iterations_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::max_iterations_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::previous_transformation_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::final_transformation_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::transformation_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::transformation_epsilon_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::converged_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::inlier_threshold_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::min_number_correspondences_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::update_visualizer_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::euclidean_fitness_epsilon_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::correspondences_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::transformation_estimation_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::correspondence_estimation_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::correspondence_rejectors_;
-      
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::use_reciprocal_correspondence_;
-      
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::convergence_criteria_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::source_has_normals_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::target_has_normals_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::need_source_blob_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::need_target_blob_;
-
-
-      using Matrix4 = typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::Matrix4;
-
-      /** \brief Empty constructor. */
-      JointIterativeClosestPoint ()
-      {
-        IterativeClosestPoint<PointSource, PointTarget, Scalar> ();
-        reg_name_ = "JointIterativeClosestPoint";
-      };
-
-      /** \brief Empty destructor */
-      ~JointIterativeClosestPoint () {}
-
-
-      /** \brief Provide a pointer to the input source 
-        * (e.g., the point cloud that we want to align to the target)
-        */
-      void
-      setInputSource (const PointCloudSourceConstPtr& /*cloud*/) override
-      {
-        PCL_WARN ("[pcl::%s::setInputSource] Warning; JointIterativeClosestPoint expects multiple clouds. Please use addInputSource.", 
-            getClassName ().c_str ());
-        return;
-      }
-
-      /** \brief Add a source cloud to the joint solver
-        *
-        * \param[in] cloud source cloud
-        */
-      inline void
-      addInputSource (const PointCloudSourceConstPtr &cloud)
-      {
-        // Set the parent InputSource, just to get all cached values (e.g. the existence of normals).
-        if (sources_.empty ())
-          IterativeClosestPoint<PointSource, PointTarget, Scalar>::setInputSource (cloud);
-        sources_.push_back (cloud);
-      }
-      
-      /** \brief Provide a pointer to the input target 
-        * (e.g., the point cloud that we want to align to the target)
-        */
-      void
-      setInputTarget (const PointCloudTargetConstPtr& /*cloud*/) override
-      {
-        PCL_WARN ("[pcl::%s::setInputTarget] Warning; JointIterativeClosestPoint expects multiple clouds. Please use addInputTarget.", 
-            getClassName ().c_str ());
-        return;
-      }
-
-      /** \brief Add a target cloud to the joint solver
-        *
-        * \param[in] cloud target cloud
-        */
-      inline void
-      addInputTarget (const PointCloudTargetConstPtr &cloud)
-      {
-        // Set the parent InputTarget, just to get all cached values (e.g. the existence of normals).
-        if (targets_.empty ())
-          IterativeClosestPoint<PointSource, PointTarget, Scalar>::setInputTarget (cloud);
-        targets_.push_back (cloud);
-      }
-
-      /** \brief Add a manual correspondence estimator
-        * If you choose to do this, you must add one for each 
-        * input source / target pair. They do not need to have trees 
-        * or input clouds set ahead of time.
-        *
-        * \param[in] ce Correspondence estimation
-        */
-      inline void
-      addCorrespondenceEstimation (CorrespondenceEstimationPtr ce)
-      {
-        correspondence_estimations_.push_back (ce);
-      }
-
-      /** \brief Reset my list of input sources
-        */
-      inline void
-      clearInputSources ()
-      { sources_.clear (); }
-
-      /** \brief Reset my list of input targets
-        */
-      inline void
-      clearInputTargets ()
-      { targets_.clear (); }
-
-      /** \brief Reset my list of correspondence estimation methods.
-        */
-      inline void
-      clearCorrespondenceEstimations ()
-      { correspondence_estimations_.clear (); }
-
-
-    protected:
-
-      /** \brief Rigid transformation computation method  with initial guess.
-        * \param output the transformed input point cloud dataset using the rigid transformation found
-        * \param guess the initial guess of the transformation to compute
-        */
-      void 
-      computeTransformation (PointCloudSource &output, const Matrix4 &guess) override;
-      
-      /** \brief Looks at the Estimators and Rejectors and determines whether their blob-setter methods need to be called */
-      void
-      determineRequiredBlobData () override;
-
-      std::vector<PointCloudSourceConstPtr> sources_;
-      std::vector<PointCloudTargetConstPtr> targets_;
-      std::vector<CorrespondenceEstimationPtr> correspondence_estimations_;
+    IterativeClosestPoint<PointSource, PointTarget, Scalar>();
+    reg_name_ = "JointIterativeClosestPoint";
   };
 
-}
+  /** \brief Empty destructor */
+  ~JointIterativeClosestPoint() {}
+
+  /** \brief Provide a pointer to the input source
+   * (e.g., the point cloud that we want to align to the target)
+   */
+  void
+  setInputSource(const PointCloudSourceConstPtr& /*cloud*/) override
+  {
+    PCL_WARN("[pcl::%s::setInputSource] Warning; JointIterativeClosestPoint expects "
+             "multiple clouds. Please use addInputSource.\n",
+             getClassName().c_str());
+    return;
+  }
+
+  /** \brief Add a source cloud to the joint solver
+   *
+   * \param[in] cloud source cloud
+   */
+  inline void
+  addInputSource(const PointCloudSourceConstPtr& cloud)
+  {
+    // Set the parent InputSource, just to get all cached values (e.g. the existence of
+    // normals).
+    if (sources_.empty())
+      IterativeClosestPoint<PointSource, PointTarget, Scalar>::setInputSource(cloud);
+    sources_.push_back(cloud);
+  }
+
+  /** \brief Provide a pointer to the input target
+   * (e.g., the point cloud that we want to align to the target)
+   */
+  void
+  setInputTarget(const PointCloudTargetConstPtr& /*cloud*/) override
+  {
+    PCL_WARN("[pcl::%s::setInputTarget] Warning; JointIterativeClosestPoint expects "
+             "multiple clouds. Please use addInputTarget.\n",
+             getClassName().c_str());
+    return;
+  }
+
+  /** \brief Add a target cloud to the joint solver
+   *
+   * \param[in] cloud target cloud
+   */
+  inline void
+  addInputTarget(const PointCloudTargetConstPtr& cloud)
+  {
+    // Set the parent InputTarget, just to get all cached values (e.g. the existence of
+    // normals).
+    if (targets_.empty())
+      IterativeClosestPoint<PointSource, PointTarget, Scalar>::setInputTarget(cloud);
+    targets_.push_back(cloud);
+  }
+
+  /** \brief Add a manual correspondence estimator
+   * If you choose to do this, you must add one for each
+   * input source / target pair. They do not need to have trees
+   * or input clouds set ahead of time.
+   *
+   * \param[in] ce Correspondence estimation
+   */
+  inline void
+  addCorrespondenceEstimation(CorrespondenceEstimationPtr ce)
+  {
+    correspondence_estimations_.push_back(ce);
+  }
+
+  /** \brief Reset my list of input sources
+   */
+  inline void
+  clearInputSources()
+  {
+    sources_.clear();
+  }
+
+  /** \brief Reset my list of input targets
+   */
+  inline void
+  clearInputTargets()
+  {
+    targets_.clear();
+  }
+
+  /** \brief Reset my list of correspondence estimation methods.
+   */
+  inline void
+  clearCorrespondenceEstimations()
+  {
+    correspondence_estimations_.clear();
+  }
+
+protected:
+  /** \brief Rigid transformation computation method  with initial guess.
+   * \param output the transformed input point cloud dataset using the rigid
+   * transformation found \param guess the initial guess of the transformation to
+   * compute
+   */
+  void
+  computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
+
+  /** \brief Looks at the Estimators and Rejectors and determines whether their
+   * blob-setter methods need to be called */
+  void
+  determineRequiredBlobData() override;
+
+  std::vector<PointCloudSourceConstPtr> sources_;
+  std::vector<PointCloudTargetConstPtr> targets_;
+  std::vector<CorrespondenceEstimationPtr> correspondence_estimations_;
+};
+
+} // namespace pcl
 
 #include <pcl/registration/impl/joint_icp.hpp>
diff --git a/registration/include/pcl/registration/lum.h b/registration/include/pcl/registration/lum.h
index f5ed9726..97d2df64 100644
--- a/registration/include/pcl/registration/lum.h
+++ b/registration/include/pcl/registration/lum.h
@@ -40,305 +40,316 @@
 
 #pragma once
 
-#include <pcl/memory.h>
-#include <pcl/pcl_macros.h>
-#include <pcl/pcl_base.h>
-#include <pcl/registration/eigen.h>
-#include <pcl/registration/boost.h>
 #include <pcl/common/transforms.h>
-#include <pcl/correspondence.h>
 #include <pcl/registration/boost_graph.h>
+#include <pcl/correspondence.h>
+#include <pcl/memory.h>
+#include <pcl/pcl_base.h>
+#include <pcl/pcl_macros.h>
 
-namespace Eigen
-{
-  using Vector6f = Eigen::Matrix<float, 6, 1>;
-  using Matrix6f = Eigen::Matrix<float, 6, 6>;
-}
-
-namespace pcl
-{
-  namespace registration
-  {
-    /** \brief Globally Consistent Scan Matching based on an algorithm by Lu and Milios.
-      * \details A GraphSLAM algorithm where registration data is managed in a graph:
-      * <ul>
-      *  <li>Vertices represent poses and hold the point cloud data and relative transformations.</li>
-      *  <li>Edges represent pose constraints and hold the correspondence data between two point clouds.</li>
-      * </ul>
-      * Computation uses the first point cloud in the SLAM graph as a reference pose and attempts to align all other point clouds to it simultaneously.
-      * For more information:
-      * <ul><li>
-      * F. Lu, E. Milios,
-      * Globally Consistent Range Scan Alignment for Environment Mapping,
-      * Autonomous Robots 4, April 1997
-      * </li><li>
-      * Dorit Borrmann, Jan Elseberg, Kai Lingemann, Andreas Nüchter, and Joachim Hertzberg,
-      * The Efficient Extension of Globally Consistent Scan Matching to 6 DoF,
-      * In Proceedings of the 4th International Symposium on 3D Data Processing, Visualization and Transmission (3DPVT '08), June 2008
-      * </li></ul>
-      * Usage example:
-      * \code
-      * pcl::registration::LUM<pcl::PointXYZ> lum;
-      * // Add point clouds as vertices to the SLAM graph
-      * lum.addPointCloud (cloud_0);
-      * lum.addPointCloud (cloud_1);
-      * lum.addPointCloud (cloud_2);
-      * // Use your favorite pairwise correspondence estimation algorithm(s)
-      * corrs_0_to_1 = someAlgo (cloud_0, cloud_1);
-      * corrs_1_to_2 = someAlgo (cloud_1, cloud_2);
-      * corrs_2_to_0 = someAlgo (lum.getPointCloud (2), lum.getPointCloud (0));
-      * // Add the correspondence results as edges to the SLAM graph
-      * lum.setCorrespondences (0, 1, corrs_0_to_1);
-      * lum.setCorrespondences (1, 2, corrs_1_to_2);
-      * lum.setCorrespondences (2, 0, corrs_2_to_0);
-      * // Change the computation parameters
-      * lum.setMaxIterations (5);
-      * lum.setConvergenceThreshold (0.0);
-      * // Perform the actual LUM computation
-      * lum.compute ();
-      * // Return the concatenated point cloud result
-      * cloud_out = lum.getConcatenatedCloud ();
-      * // Return the separate point cloud transformations
-      * for(int i = 0; i < lum.getNumVertices (); i++)
-      * {
-      *   transforms_out[i] = lum.getTransformation (i);
-      * }
-      * \endcode
-      * \author Frits Florentinus, Jochen Sprickerhof
-      * \ingroup registration
-      */
-    template<typename PointT>
-    class LUM
-    {
-      public:
-        using Ptr = shared_ptr<LUM<PointT> >;
-        using ConstPtr = shared_ptr<const LUM<PointT> >;
-
-        using PointCloud = pcl::PointCloud<PointT>;
-        using PointCloudPtr = typename PointCloud::Ptr;
-        using PointCloudConstPtr = typename PointCloud::ConstPtr;
-
-        struct VertexProperties
-        {
-          PointCloudPtr cloud_;
-          Eigen::Vector6f pose_;
-          PCL_MAKE_ALIGNED_OPERATOR_NEW
-        };
-        struct EdgeProperties
-        {
-          pcl::CorrespondencesPtr corrs_;
-          Eigen::Matrix6f cinv_;
-          Eigen::Vector6f cinvd_;
-          PCL_MAKE_ALIGNED_OPERATOR_NEW
-        };
-
-        using SLAMGraph = boost::adjacency_list<boost::eigen_vecS, boost::eigen_vecS, boost::bidirectionalS, VertexProperties, EdgeProperties, boost::no_property, boost::eigen_listS>;
-        using SLAMGraphPtr = shared_ptr<SLAMGraph>;
-        using Vertex = typename SLAMGraph::vertex_descriptor;
-        using Edge = typename SLAMGraph::edge_descriptor;
-
-        /** \brief Empty constructor.
-          */
-        LUM () 
-          : slam_graph_ (new SLAMGraph)
-          , max_iterations_ (5)
-          , convergence_threshold_ (0.0)
-        {
-        }
-
-        /** \brief Set the internal SLAM graph structure.
-          * \details All data used and produced by LUM is stored in this boost::adjacency_list.
-          * It is recommended to use the LUM class itself to build the graph.
-          * This method could otherwise be useful for managing several SLAM graphs in one instance of LUM.
-          * \param[in] slam_graph The new SLAM graph.
-          */
-        inline void
-        setLoopGraph (const SLAMGraphPtr &slam_graph);
-
-        /** \brief Get the internal SLAM graph structure.
-          * \details All data used and produced by LUM is stored in this boost::adjacency_list.
-          * It is recommended to use the LUM class itself to build the graph.
-          * This method could otherwise be useful for managing several SLAM graphs in one instance of LUM.
-          * \return The current SLAM graph.
-          */
-        inline SLAMGraphPtr
-        getLoopGraph () const;
-
-        /** \brief Get the number of vertices in the SLAM graph.
-          * \return The current number of vertices in the SLAM graph.
-          */
-        typename SLAMGraph::vertices_size_type
-        getNumVertices () const;
-
-        /** \brief Set the maximum number of iterations for the compute() method.
-          * \details The compute() method finishes when max_iterations are met or when the convergence criteria is met.
-          * \param[in] max_iterations The new maximum number of iterations (default = 5).
-          */
-        void
-        setMaxIterations (int max_iterations);
-
-        /** \brief Get the maximum number of iterations for the compute() method.
-          * \details The compute() method finishes when max_iterations are met or when the convergence criteria is met.
-          * \return The current maximum number of iterations (default = 5).
-          */
-        inline int
-        getMaxIterations () const;
-
-        /** \brief Set the convergence threshold for the compute() method.
-          * \details When the compute() method computes the new poses relative to the old poses, it will determine the length of the difference vector.
-          * When the average length of all difference vectors becomes less than the convergence_threshold the convergence is assumed to be met.
-          * \param[in] convergence_threshold The new convergence threshold (default = 0.0).
-          */
-        void
-        setConvergenceThreshold (float convergence_threshold);
-
-        /** \brief Get the convergence threshold for the compute() method.
-          * \details When the compute() method computes the new poses relative to the old poses, it will determine the length of the difference vector.
-          * When the average length of all difference vectors becomes less than the convergence_threshold the convergence is assumed to be met.
-          * \return The current convergence threshold (default = 0.0).
-          */
-        inline float
-        getConvergenceThreshold () const;
-
-        /** \brief Add a new point cloud to the SLAM graph.
-          * \details This method will add a new vertex to the SLAM graph and attach a point cloud to that vertex.
-          * Optionally you can specify a pose estimate for this point cloud.
-          * A vertex' pose is always relative to the first vertex in the SLAM graph, i.e. the first point cloud that was added.
-          * Because this first vertex is the reference, you can not set a pose estimate for this vertex.
-          * Providing pose estimates to the vertices in the SLAM graph will reduce overall computation time of LUM.
-          * \note Vertex descriptors are typecastable to int.
-          * \param[in] cloud The new point cloud.
-          * \param[in] pose (optional) The pose estimate relative to the reference pose (first point cloud that was added).
-          * \return The vertex descriptor of the newly created vertex.
-          */
-        Vertex
-        addPointCloud (const PointCloudPtr &cloud, const Eigen::Vector6f &pose = Eigen::Vector6f::Zero ());
-
-        /** \brief Change a point cloud on one of the SLAM graph's vertices.
-          * \details This method will change the point cloud attached to an existing vertex and will not alter the SLAM graph structure.
-          * Note that the correspondences attached to this vertex will not change and may need to be updated manually.
-          * \note Vertex descriptors are typecastable to int.
-          * \param[in] vertex The vertex descriptor of which to change the point cloud.
-          * \param[in] cloud The new point cloud for that vertex.
-          */
-        inline void
-        setPointCloud (const Vertex &vertex, const PointCloudPtr &cloud);
-
-        /** \brief Return a point cloud from one of the SLAM graph's vertices.
-          * \note Vertex descriptors are typecastable to int.
-          * \param[in] vertex The vertex descriptor of which to return the point cloud.
-          * \return The current point cloud for that vertex.
-          */
-        inline PointCloudPtr
-        getPointCloud (const Vertex &vertex) const;
-
-        /** \brief Change a pose estimate on one of the SLAM graph's vertices.
-          * \details A vertex' pose is always relative to the first vertex in the SLAM graph, i.e. the first point cloud that was added.
-          * Because this first vertex is the reference, you can not set a pose estimate for this vertex.
-          * Providing pose estimates to the vertices in the SLAM graph will reduce overall computation time of LUM.
-          * \note Vertex descriptors are typecastable to int.
-          * \param[in] vertex The vertex descriptor of which to set the pose estimate.
-          * \param[in] pose The new pose estimate for that vertex.
-          */
-        inline void
-        setPose (const Vertex &vertex, const Eigen::Vector6f &pose);
-
-        /** \brief Return a pose estimate from one of the SLAM graph's vertices.
-          * \note Vertex descriptors are typecastable to int.
-          * \param[in] vertex The vertex descriptor of which to return the pose estimate.
-          * \return The current pose estimate of that vertex.
-          */
-        inline Eigen::Vector6f
-        getPose (const Vertex &vertex) const;
-
-        /** \brief Return a pose estimate from one of the SLAM graph's vertices as an affine transformation matrix.
-          * \note Vertex descriptors are typecastable to int.
-          * \param[in] vertex The vertex descriptor of which to return the transformation matrix.
-          * \return The current transformation matrix of that vertex.
-          */
-        inline Eigen::Affine3f
-        getTransformation (const Vertex &vertex) const;
-
-        /** \brief Add/change a set of correspondences for one of the SLAM graph's edges.
-          * \details The edges in the SLAM graph are directional and point from source vertex to target vertex.
-          * The query indices of the correspondences, index the points at the source vertex' point cloud.
-          * The matching indices of the correspondences, index the points at the target vertex' point cloud.
-          * If no edge was present at the specified location, this method will add a new edge to the SLAM graph and attach the correspondences to that edge.
-          * If the edge was already present, this method will overwrite the correspondence information of that edge and will not alter the SLAM graph structure.
-          * \note Vertex descriptors are typecastable to int.
-          * \param[in] source_vertex The vertex descriptor of the correspondences' source point cloud.
-          * \param[in] target_vertex The vertex descriptor of the correspondences' target point cloud.
-          * \param[in] corrs The new set of correspondences for that edge.
-          */
-        void
-        setCorrespondences (const Vertex &source_vertex, 
-                            const Vertex &target_vertex, 
-                            const pcl::CorrespondencesPtr &corrs);
-
-        /** \brief Return a set of correspondences from one of the SLAM graph's edges.
-          * \note Vertex descriptors are typecastable to int.
-          * \param[in] source_vertex The vertex descriptor of the correspondences' source point cloud.
-          * \param[in] target_vertex The vertex descriptor of the correspondences' target point cloud.
-          * \return The current set of correspondences of that edge.
-          */
-        inline pcl::CorrespondencesPtr
-        getCorrespondences (const Vertex &source_vertex, const Vertex &target_vertex) const;
-
-        /** \brief Perform LUM's globally consistent scan matching.
-          * \details Computation uses the first point cloud in the SLAM graph as a reference pose and attempts to align all other point clouds to it simultaneously.
-          * <br>
-          * Things to keep in mind:
-          * <ul>
-          *  <li>Only those parts of the graph connected to the reference pose will properly align to it.</li>
-          *  <li>All sets of correspondences should span the same space and need to be sufficient to determine a rigid transformation.</li>
-          *  <li>The algorithm draws it strength from loops in the graph because it will distribute errors evenly amongst those loops.</li>
-          * </ul>
-          * Computation ends when either of the following conditions hold:
-          * <ul>
-          *  <li>The number of iterations reaches max_iterations. Use setMaxIterations() to change.</li>
-          *  <li>The convergence criteria is met. Use setConvergenceThreshold() to change.</li>
-          * </ul>
-          * Computation will change the pose estimates for the vertices of the SLAM graph, not the point clouds attached to them.
-          * The results can be retrieved with getPose(), getTransformation(), getTransformedCloud() or getConcatenatedCloud().
-          */
-        void
-        compute ();
-
-        /** \brief Return a point cloud from one of the SLAM graph's vertices compounded onto its current pose estimate.
-          * \note Vertex descriptors are typecastable to int.
-          * \param[in] vertex The vertex descriptor of which to return the transformed point cloud.
-          * \return The transformed point cloud of that vertex.
-          */
-        PointCloudPtr
-        getTransformedCloud (const Vertex &vertex) const;
-
-        /** \brief Return a concatenated point cloud of all the SLAM graph's point clouds compounded onto their current pose estimates.
-          * \return The concatenated transformed point clouds of the entire SLAM graph.
-          */
-        PointCloudPtr
-        getConcatenatedCloud () const;
-
-      protected:
-        /** \brief Linearized computation of C^-1 and C^-1*D (results stored in slam_graph_). */
-        void
-        computeEdge (const Edge &e);
-
-        /** \brief Returns a pose corrected 6DoF incidence matrix. */
-        inline Eigen::Matrix6f
-        incidenceCorrection (const Eigen::Vector6f &pose);
-
-      private:
-        /** \brief The internal SLAM graph structure. */
-        SLAMGraphPtr slam_graph_;
-
-        /** \brief The maximum number of iterations for the compute() method. */
-        int max_iterations_;
-
-        /** \brief The convergence threshold for the summed vector lengths of all poses. */
-        float convergence_threshold_;
-    };
-  }
-}
+namespace Eigen {
+using Vector6f = Eigen::Matrix<float, 6, 1>;
+using Matrix6f = Eigen::Matrix<float, 6, 6>;
+} // namespace Eigen
+
+namespace pcl {
+namespace registration {
+/** \brief Globally Consistent Scan Matching based on an algorithm by Lu and Milios.
+ * \details A GraphSLAM algorithm where registration data is managed in a graph:
+ * <ul>
+ *  <li>Vertices represent poses and hold the point cloud data and relative
+ * transformations.</li> <li>Edges represent pose constraints and hold the
+ * correspondence data between two point clouds.</li>
+ * </ul>
+ * Computation uses the first point cloud in the SLAM graph as a reference pose and
+ * attempts to align all other point clouds to it simultaneously. For more information:
+ * <ul><li>
+ * F. Lu, E. Milios,
+ * Globally Consistent Range Scan Alignment for Environment Mapping,
+ * Autonomous Robots 4, April 1997
+ * </li><li>
+ * Dorit Borrmann, Jan Elseberg, Kai Lingemann, Andreas Nüchter, and Joachim Hertzberg,
+ * The Efficient Extension of Globally Consistent Scan Matching to 6 DoF,
+ * In Proceedings of the 4th International Symposium on 3D Data Processing,
+ * Visualization and Transmission (3DPVT '08), June 2008
+ * </li></ul>
+ * Usage example:
+ * \code
+ * pcl::registration::LUM<pcl::PointXYZ> lum;
+ * // Add point clouds as vertices to the SLAM graph
+ * lum.addPointCloud (cloud_0);
+ * lum.addPointCloud (cloud_1);
+ * lum.addPointCloud (cloud_2);
+ * // Use your favorite pairwise correspondence estimation algorithm(s)
+ * corrs_0_to_1 = someAlgo (cloud_0, cloud_1);
+ * corrs_1_to_2 = someAlgo (cloud_1, cloud_2);
+ * corrs_2_to_0 = someAlgo (lum.getPointCloud (2), lum.getPointCloud (0));
+ * // Add the correspondence results as edges to the SLAM graph
+ * lum.setCorrespondences (0, 1, corrs_0_to_1);
+ * lum.setCorrespondences (1, 2, corrs_1_to_2);
+ * lum.setCorrespondences (2, 0, corrs_2_to_0);
+ * // Change the computation parameters
+ * lum.setMaxIterations (5);
+ * lum.setConvergenceThreshold (0.0);
+ * // Perform the actual LUM computation
+ * lum.compute ();
+ * // Return the concatenated point cloud result
+ * cloud_out = lum.getConcatenatedCloud ();
+ * // Return the separate point cloud transformations
+ * for(int i = 0; i < lum.getNumVertices (); i++)
+ * {
+ *   transforms_out[i] = lum.getTransformation (i);
+ * }
+ * \endcode
+ * \author Frits Florentinus, Jochen Sprickerhof
+ * \ingroup registration
+ */
+template <typename PointT>
+class LUM {
+public:
+  using Ptr = shared_ptr<LUM<PointT>>;
+  using ConstPtr = shared_ptr<const LUM<PointT>>;
+
+  using PointCloud = pcl::PointCloud<PointT>;
+  using PointCloudPtr = typename PointCloud::Ptr;
+  using PointCloudConstPtr = typename PointCloud::ConstPtr;
+
+  struct VertexProperties {
+    PointCloudPtr cloud_;
+    Eigen::Vector6f pose_;
+    PCL_MAKE_ALIGNED_OPERATOR_NEW
+  };
+  struct EdgeProperties {
+    pcl::CorrespondencesPtr corrs_;
+    Eigen::Matrix6f cinv_;
+    Eigen::Vector6f cinvd_;
+    PCL_MAKE_ALIGNED_OPERATOR_NEW
+  };
+
+  using SLAMGraph = boost::adjacency_list<boost::eigen_vecS,
+                                          boost::eigen_vecS,
+                                          boost::bidirectionalS,
+                                          VertexProperties,
+                                          EdgeProperties,
+                                          boost::no_property,
+                                          boost::eigen_listS>;
+  using SLAMGraphPtr = shared_ptr<SLAMGraph>;
+  using Vertex = typename SLAMGraph::vertex_descriptor;
+  using Edge = typename SLAMGraph::edge_descriptor;
+
+  /** \brief Empty constructor.
+   */
+  LUM() : slam_graph_(new SLAMGraph), max_iterations_(5), convergence_threshold_(0.0) {}
+
+  /** \brief Set the internal SLAM graph structure.
+   * \details All data used and produced by LUM is stored in this boost::adjacency_list.
+   * It is recommended to use the LUM class itself to build the graph.
+   * This method could otherwise be useful for managing several SLAM graphs in one
+   * instance of LUM. \param[in] slam_graph The new SLAM graph.
+   */
+  inline void
+  setLoopGraph(const SLAMGraphPtr& slam_graph);
+
+  /** \brief Get the internal SLAM graph structure.
+   * \details All data used and produced by LUM is stored in this boost::adjacency_list.
+   * It is recommended to use the LUM class itself to build the graph.
+   * This method could otherwise be useful for managing several SLAM graphs in one
+   * instance of LUM. \return The current SLAM graph.
+   */
+  inline SLAMGraphPtr
+  getLoopGraph() const;
+
+  /** \brief Get the number of vertices in the SLAM graph.
+   * \return The current number of vertices in the SLAM graph.
+   */
+  typename SLAMGraph::vertices_size_type
+  getNumVertices() const;
+
+  /** \brief Set the maximum number of iterations for the compute() method.
+   * \details The compute() method finishes when max_iterations are met or when the
+   * convergence criteria is met. \param[in] max_iterations The new maximum number of
+   * iterations (default = 5).
+   */
+  void
+  setMaxIterations(int max_iterations);
+
+  /** \brief Get the maximum number of iterations for the compute() method.
+   * \details The compute() method finishes when max_iterations are met or when the
+   * convergence criteria is met. \return The current maximum number of iterations
+   * (default = 5).
+   */
+  inline int
+  getMaxIterations() const;
+
+  /** \brief Set the convergence threshold for the compute() method.
+   * \details When the compute() method computes the new poses relative to the old
+   * poses, it will determine the length of the difference vector. When the average
+   * length of all difference vectors becomes less than the convergence_threshold the
+   * convergence is assumed to be met. \param[in] convergence_threshold The new
+   * convergence threshold (default = 0.0).
+   */
+  void
+  setConvergenceThreshold(float convergence_threshold);
+
+  /** \brief Get the convergence threshold for the compute() method.
+   * \details When the compute() method computes the new poses relative to the old
+   * poses, it will determine the length of the difference vector. When the average
+   * length of all difference vectors becomes less than the convergence_threshold the
+   * convergence is assumed to be met. \return The current convergence threshold
+   * (default = 0.0).
+   */
+  inline float
+  getConvergenceThreshold() const;
+
+  /** \brief Add a new point cloud to the SLAM graph.
+   * \details This method will add a new vertex to the SLAM graph and attach a point
+   * cloud to that vertex. Optionally you can specify a pose estimate for this point
+   * cloud. A vertex' pose is always relative to the first vertex in the SLAM graph,
+   * i.e. the first point cloud that was added. Because this first vertex is the
+   * reference, you can not set a pose estimate for this vertex. Providing pose
+   * estimates to the vertices in the SLAM graph will reduce overall computation time of
+   * LUM. \note Vertex descriptors are typecastable to int. \param[in] cloud The new
+   * point cloud. \param[in] pose (optional) The pose estimate relative to the reference
+   * pose (first point cloud that was added). \return The vertex descriptor of the newly
+   * created vertex.
+   */
+  Vertex
+  addPointCloud(const PointCloudPtr& cloud,
+                const Eigen::Vector6f& pose = Eigen::Vector6f::Zero());
+
+  /** \brief Change a point cloud on one of the SLAM graph's vertices.
+   * \details This method will change the point cloud attached to an existing vertex and
+   * will not alter the SLAM graph structure. Note that the correspondences attached to
+   * this vertex will not change and may need to be updated manually. \note Vertex
+   * descriptors are typecastable to int. \param[in] vertex The vertex descriptor of
+   * which to change the point cloud. \param[in] cloud The new point cloud for that
+   * vertex.
+   */
+  inline void
+  setPointCloud(const Vertex& vertex, const PointCloudPtr& cloud);
+
+  /** \brief Return a point cloud from one of the SLAM graph's vertices.
+   * \note Vertex descriptors are typecastable to int.
+   * \param[in] vertex The vertex descriptor of which to return the point cloud.
+   * \return The current point cloud for that vertex.
+   */
+  inline PointCloudPtr
+  getPointCloud(const Vertex& vertex) const;
+
+  /** \brief Change a pose estimate on one of the SLAM graph's vertices.
+   * \details A vertex' pose is always relative to the first vertex in the SLAM graph,
+   * i.e. the first point cloud that was added. Because this first vertex is the
+   * reference, you can not set a pose estimate for this vertex. Providing pose
+   * estimates to the vertices in the SLAM graph will reduce overall computation time of
+   * LUM. \note Vertex descriptors are typecastable to int. \param[in] vertex The vertex
+   * descriptor of which to set the pose estimate. \param[in] pose The new pose estimate
+   * for that vertex.
+   */
+  inline void
+  setPose(const Vertex& vertex, const Eigen::Vector6f& pose);
+
+  /** \brief Return a pose estimate from one of the SLAM graph's vertices.
+   * \note Vertex descriptors are typecastable to int.
+   * \param[in] vertex The vertex descriptor of which to return the pose estimate.
+   * \return The current pose estimate of that vertex.
+   */
+  inline Eigen::Vector6f
+  getPose(const Vertex& vertex) const;
+
+  /** \brief Return a pose estimate from one of the SLAM graph's vertices as an affine
+   * transformation matrix. \note Vertex descriptors are typecastable to int. \param[in]
+   * vertex The vertex descriptor of which to return the transformation matrix. \return
+   * The current transformation matrix of that vertex.
+   */
+  inline Eigen::Affine3f
+  getTransformation(const Vertex& vertex) const;
+
+  /** \brief Add/change a set of correspondences for one of the SLAM graph's edges.
+   * \details The edges in the SLAM graph are directional and point from source vertex
+   * to target vertex. The query indices of the correspondences, index the points at the
+   * source vertex' point cloud. The matching indices of the correspondences, index the
+   * points at the target vertex' point cloud. If no edge was present at the specified
+   * location, this method will add a new edge to the SLAM graph and attach the
+   * correspondences to that edge. If the edge was already present, this method will
+   * overwrite the correspondence information of that edge and will not alter the SLAM
+   * graph structure. \note Vertex descriptors are typecastable to int. \param[in]
+   * source_vertex The vertex descriptor of the correspondences' source point cloud.
+   * \param[in] target_vertex The vertex descriptor of the correspondences' target point
+   * cloud. \param[in] corrs The new set of correspondences for that edge.
+   */
+  void
+  setCorrespondences(const Vertex& source_vertex,
+                     const Vertex& target_vertex,
+                     const pcl::CorrespondencesPtr& corrs);
+
+  /** \brief Return a set of correspondences from one of the SLAM graph's edges.
+   * \note Vertex descriptors are typecastable to int.
+   * \param[in] source_vertex The vertex descriptor of the correspondences' source point
+   * cloud. \param[in] target_vertex The vertex descriptor of the correspondences'
+   * target point cloud. \return The current set of correspondences of that edge.
+   */
+  inline pcl::CorrespondencesPtr
+  getCorrespondences(const Vertex& source_vertex, const Vertex& target_vertex) const;
+
+  /** \brief Perform LUM's globally consistent scan matching.
+   * \details Computation uses the first point cloud in the SLAM graph as a reference
+   * pose and attempts to align all other point clouds to it simultaneously. <br> Things
+   * to keep in mind: <ul> <li>Only those parts of the graph connected to the reference
+   * pose will properly align to it.</li> <li>All sets of correspondences should span
+   * the same space and need to be sufficient to determine a rigid transformation.</li>
+   *  <li>The algorithm draws it strength from loops in the graph because it will
+   * distribute errors evenly amongst those loops.</li>
+   * </ul>
+   * Computation ends when either of the following conditions hold:
+   * <ul>
+   *  <li>The number of iterations reaches max_iterations. Use setMaxIterations() to
+   * change.</li> <li>The convergence criteria is met. Use setConvergenceThreshold() to
+   * change.</li>
+   * </ul>
+   * Computation will change the pose estimates for the vertices of the SLAM graph, not
+   * the point clouds attached to them. The results can be retrieved with getPose(),
+   * getTransformation(), getTransformedCloud() or getConcatenatedCloud().
+   */
+  void
+  compute();
+
+  /** \brief Return a point cloud from one of the SLAM graph's vertices compounded onto
+   * its current pose estimate. \note Vertex descriptors are typecastable to int.
+   * \param[in] vertex The vertex descriptor of which to return the transformed point
+   * cloud. \return The transformed point cloud of that vertex.
+   */
+  PointCloudPtr
+  getTransformedCloud(const Vertex& vertex) const;
+
+  /** \brief Return a concatenated point cloud of all the SLAM graph's point clouds
+   * compounded onto their current pose estimates. \return The concatenated transformed
+   * point clouds of the entire SLAM graph.
+   */
+  PointCloudPtr
+  getConcatenatedCloud() const;
+
+protected:
+  /** \brief Linearized computation of C^-1 and C^-1*D (results stored in slam_graph_).
+   */
+  void
+  computeEdge(const Edge& e);
+
+  /** \brief Returns a pose corrected 6DoF incidence matrix. */
+  inline Eigen::Matrix6f
+  incidenceCorrection(const Eigen::Vector6f& pose);
+
+private:
+  /** \brief The internal SLAM graph structure. */
+  SLAMGraphPtr slam_graph_;
+
+  /** \brief The maximum number of iterations for the compute() method. */
+  int max_iterations_;
+
+  /** \brief The convergence threshold for the summed vector lengths of all poses. */
+  float convergence_threshold_;
+};
+} // namespace registration
+} // namespace pcl
 
 #ifdef PCL_NO_PRECOMPILE
 #include <pcl/registration/impl/lum.hpp>
diff --git a/registration/include/pcl/registration/matching_candidate.h b/registration/include/pcl/registration/matching_candidate.h
index 78c3223f..bb1bf21d 100644
--- a/registration/include/pcl/registration/matching_candidate.h
+++ b/registration/include/pcl/registration/matching_candidate.h
@@ -20,7 +20,7 @@
  *     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
+ *  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
@@ -37,65 +37,56 @@
 
 #pragma once
 
+#include <pcl/common/common.h>
+#include <pcl/registration/registration.h>
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
-#include <pcl/registration/registration.h>
-#include <pcl/common/common.h>
-
-namespace pcl
-{
-  namespace registration
-  {
-    /** \brief Container for matching candidate consisting of
-    *
-    * * fitness score value as a result of the matching algorithm
-    * * correspondences between source and target data set
-    * * transformation matrix calculated based on the correspondences
-    *
-    */
-    struct MatchingCandidate
-    {
-      /** \brief Constructor. */
-      MatchingCandidate () :
-        fitness_score (FLT_MAX),
-        transformation (Eigen::Matrix4f::Identity ())
-      {};
 
-      /** \brief Value constructor. */
-      MatchingCandidate (float s, const pcl::Correspondences &c, const Eigen::Matrix4f &m) :
-        fitness_score (s),
-        correspondences (c),
-        transformation (m)
-      {};
+namespace pcl {
+namespace registration {
+/** \brief Container for matching candidate consisting of
+ *
+ * * fitness score value as a result of the matching algorithm
+ * * correspondences between source and target data set
+ * * transformation matrix calculated based on the correspondences
+ *
+ */
+struct MatchingCandidate {
+  /** \brief Constructor. */
+  MatchingCandidate()
+  : fitness_score(FLT_MAX), transformation(Eigen::Matrix4f::Identity()){};
 
-      /** \brief Destructor. */
-      ~MatchingCandidate ()
-      {};
+  /** \brief Value constructor. */
+  MatchingCandidate(float s, const pcl::Correspondences& c, const Eigen::Matrix4f& m)
+  : fitness_score(s), correspondences(c), transformation(m){};
 
+  /** \brief Destructor. */
+  ~MatchingCandidate(){};
 
-      /** \brief Fitness score of current candidate resulting from matching algorithm. */
-      float fitness_score;
+  /** \brief Fitness score of current candidate resulting from matching algorithm. */
+  float fitness_score;
 
-      /** \brief Correspondences between source <-> target. */
-      pcl::Correspondences correspondences;
+  /** \brief Correspondences between source <-> target. */
+  pcl::Correspondences correspondences;
 
-      /** \brief Corresponding transformation matrix retrieved using \a corrs. */
-      Eigen::Matrix4f transformation;
+  /** \brief Corresponding transformation matrix retrieved using \a corrs. */
+  Eigen::Matrix4f transformation;
 
-      PCL_MAKE_ALIGNED_OPERATOR_NEW
-    };
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+};
 
-    using MatchingCandidates = std::vector<MatchingCandidate, Eigen::aligned_allocator<MatchingCandidate> >;
+using MatchingCandidates =
+    std::vector<MatchingCandidate, Eigen::aligned_allocator<MatchingCandidate>>;
 
-    /** \brief Sorting of candidates based on fitness score value. */
-    struct by_score
-    {
-      /** \brief Operator used to sort candidates based on fitness score. */
-      bool operator () (MatchingCandidate const &left, MatchingCandidate const &right)
-      {
-        return (left.fitness_score < right.fitness_score);
-      }
-    };
+/** \brief Sorting of candidates based on fitness score value. */
+struct by_score {
+  /** \brief Operator used to sort candidates based on fitness score. */
+  bool
+  operator()(MatchingCandidate const& left, MatchingCandidate const& right)
+  {
+    return (left.fitness_score < right.fitness_score);
+  }
+};
 
-  };  // namespace registration
+}; // namespace registration
 }; // namespace pcl
diff --git a/registration/include/pcl/registration/meta_registration.h b/registration/include/pcl/registration/meta_registration.h
index 3ddb5d72..cab4e4c5 100644
--- a/registration/include/pcl/registration/meta_registration.h
+++ b/registration/include/pcl/registration/meta_registration.h
@@ -37,92 +37,94 @@
 
 #pragma once
 
-#include <pcl/point_cloud.h>
 #include <pcl/registration/registration.h>
+#include <pcl/point_cloud.h>
 
 namespace pcl {
-  namespace registration {
-
-    /** \brief Meta @ref Registration class
-      * \note This method accumulates all registered points and becomes more costly with each registered point cloud.
-      *
-      * This class provides a way to register a stream of clouds where each cloud
-      * will be aligned to the conglomerate of all previous clouds.
-      *
-      * \code
-      * IterativeClosestPoint<PointXYZ,PointXYZ>::Ptr icp (new IterativeClosestPoint<PointXYZ,PointXYZ>);
-      * icp->setMaxCorrespondenceDistance (0.05);
-      * icp->setMaximumIterations (50);
-      *
-      * MetaRegistration<PointXYZ> mreg;
-      * mreg.setRegistration (icp);
-      *
-      * while (true)
-      * {
-      *   PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
-      *   read_cloud (*cloud);
-      *   mreg.registerCloud (cloud);
-      *
-      *   PointCloud<PointXYZ>::Ptr tmp (new PointCloud<PointXYZ>);
-      *   transformPointCloud (*cloud, *tmp, mreg.getAbsoluteTransform ());
-      *   write_cloud (*tmp);
-      * }
-      * \endcode
-      *
-      * \author Michael 'v4hn' Goerner
-      * \ingroup registration
-      */
-    template <typename PointT, typename Scalar = float>
-    class MetaRegistration {
-      public:
-        using PointCloudPtr = typename pcl::PointCloud<PointT>::Ptr;
-        using PointCloudConstPtr = typename pcl::PointCloud<PointT>::ConstPtr;
-
-        using RegistrationPtr = typename pcl::Registration<PointT,PointT,Scalar>::Ptr;
-        using Matrix4 = typename pcl::Registration<PointT,PointT,Scalar>::Matrix4;
-
-        MetaRegistration ();
-
-        /** \brief Empty destructor */
-        virtual ~MetaRegistration () {};
-
-        /** \brief Register new point cloud
-          * \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 registration converged
-          */
-        bool
-        registerCloud (const PointCloudConstPtr& cloud, const Matrix4& delta_estimate = Matrix4::Identity ());
-
-        /** \brief Get estimated transform of the last registered cloud */
-        inline Matrix4
-        getAbsoluteTransform () const;
-
-        /** \brief Reset MetaRegistration without resetting registration_ */
-        inline void
-        reset ();
-
-        /** \brief Set registration instance used to align clouds */
-        inline void
-        setRegistration (RegistrationPtr);
-
-        /** \brief get accumulated meta point cloud */
-        inline PointCloudConstPtr
-        getMetaCloud () const;
-      protected:
-
-        /** \brief registered accumulated point cloud */
-        PointCloudPtr full_cloud_;
-
-        /** \brief registration instance to align clouds */
-        RegistrationPtr registration_;
-
-        /** \brief estimated transform */
-        Matrix4 abs_transform_;
-    };
-
-  }
-}
+namespace registration {
+
+/** \brief Meta @ref Registration class
+ * \note This method accumulates all registered points and becomes more costly with each
+ * registered point cloud.
+ *
+ * This class provides a way to register a stream of clouds where each cloud
+ * will be aligned to the conglomerate of all previous clouds.
+ *
+ * \code
+ * IterativeClosestPoint<PointXYZ,PointXYZ>::Ptr icp
+ *   (new IterativeClosestPoint<PointXYZ,PointXYZ>);
+ * icp->setMaxCorrespondenceDistance (0.05);
+ * icp->setMaximumIterations (50);
+ *
+ * MetaRegistration<PointXYZ> mreg;
+ * mreg.setRegistration (icp);
+ *
+ * while (true)
+ * {
+ *   PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
+ *   read_cloud (*cloud);
+ *   mreg.registerCloud (cloud);
+ *
+ *   PointCloud<PointXYZ>::Ptr tmp (new PointCloud<PointXYZ>);
+ *   transformPointCloud (*cloud, *tmp, mreg.getAbsoluteTransform ());
+ *   write_cloud (*tmp);
+ * }
+ * \endcode
+ *
+ * \author Michael 'v4hn' Goerner
+ * \ingroup registration
+ */
+template <typename PointT, typename Scalar = float>
+class MetaRegistration {
+public:
+  using PointCloudPtr = typename pcl::PointCloud<PointT>::Ptr;
+  using PointCloudConstPtr = typename pcl::PointCloud<PointT>::ConstPtr;
+
+  using RegistrationPtr = typename pcl::Registration<PointT, PointT, Scalar>::Ptr;
+  using Matrix4 = typename pcl::Registration<PointT, PointT, Scalar>::Matrix4;
+
+  MetaRegistration();
+
+  /** \brief Empty destructor */
+  virtual ~MetaRegistration(){};
+
+  /** \brief Register new point cloud
+   * \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
+   * registration converged
+   */
+  bool
+  registerCloud(const PointCloudConstPtr& cloud,
+                const Matrix4& delta_estimate = Matrix4::Identity());
+
+  /** \brief Get estimated transform of the last registered cloud */
+  inline Matrix4
+  getAbsoluteTransform() const;
+
+  /** \brief Reset MetaRegistration without resetting registration_ */
+  inline void
+  reset();
+
+  /** \brief Set registration instance used to align clouds */
+  inline void setRegistration(RegistrationPtr);
+
+  /** \brief get accumulated meta point cloud */
+  inline PointCloudConstPtr
+  getMetaCloud() const;
+
+protected:
+  /** \brief registered accumulated point cloud */
+  PointCloudPtr full_cloud_;
+
+  /** \brief registration instance to align clouds */
+  RegistrationPtr registration_;
+
+  /** \brief estimated transform */
+  Matrix4 abs_transform_;
+};
+
+} // namespace registration
+} // namespace pcl
 
 #include <pcl/registration/impl/meta_registration.hpp>
diff --git a/registration/include/pcl/registration/ndt.h b/registration/include/pcl/registration/ndt.h
index 36f10f7e..ef00d528 100644
--- a/registration/include/pcl/registration/ndt.h
+++ b/registration/include/pcl/registration/ndt.h
@@ -40,428 +40,478 @@
 
 #pragma once
 
+#include <pcl/common/utils.h>
+#include <pcl/filters/voxel_grid_covariance.h>
+#include <pcl/registration/registration.h>
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
-#include <pcl/registration/registration.h>
-#include <pcl/filters/voxel_grid_covariance.h>
 
 #include <unsupported/Eigen/NonLinearOptimization>
 
-namespace pcl
-{
-  /** \brief A 3D Normal Distribution Transform registration implementation for point cloud data.
-    * \note For more information please see
-    * <b>Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform —
-    * an Efficient Representation for Registration, Surface Analysis, and Loop Detection.
-    * PhD thesis, Orebro University. Orebro Studies in Technology 36.</b>,
-    * <b>More, J., and Thuente, D. (1994). Line Search Algorithm with Guaranteed Sufficient Decrease
-    * In ACM Transactions on Mathematical Software.</b> and
-    * Sun, W. and Yuan, Y, (2006) Optimization Theory and Methods: Nonlinear Programming. 89-100
-    * \note Math refactored by Todor Stoyanov.
-    * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific)
-    */
-  template<typename PointSource, typename PointTarget>
-  class NormalDistributionsTransform : public Registration<PointSource, PointTarget>
+namespace pcl {
+/** \brief A 3D Normal Distribution Transform registration implementation for point
+ * cloud data. \note For more information please see <b>Magnusson, M. (2009). The
+ * Three-Dimensional Normal-Distributions Transform — an Efficient Representation for
+ * Registration, Surface Analysis, and Loop Detection. PhD thesis, Orebro University.
+ * Orebro Studies in Technology 36.</b>, <b>More, J., and Thuente, D. (1994). Line
+ * Search Algorithm with Guaranteed Sufficient Decrease In ACM Transactions on
+ * Mathematical Software.</b> and Sun, W. and Yuan, Y, (2006) Optimization Theory and
+ * Methods: Nonlinear Programming. 89-100 \note Math refactored by Todor Stoyanov.
+ * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific)
+ */
+template <typename PointSource, typename PointTarget>
+class NormalDistributionsTransform : public Registration<PointSource, PointTarget> {
+protected:
+  using PointCloudSource =
+      typename Registration<PointSource, PointTarget>::PointCloudSource;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+
+  using PointCloudTarget =
+      typename Registration<PointSource, PointTarget>::PointCloudTarget;
+  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
+  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
+
+  using PointIndicesPtr = PointIndices::Ptr;
+  using PointIndicesConstPtr = PointIndices::ConstPtr;
+
+  /** \brief Typename of searchable voxel grid containing mean and covariance. */
+  using TargetGrid = VoxelGridCovariance<PointTarget>;
+  /** \brief Typename of pointer to searchable voxel grid. */
+  using TargetGridPtr = TargetGrid*;
+  /** \brief Typename of const pointer to searchable voxel grid. */
+  using TargetGridConstPtr = const TargetGrid*;
+  /** \brief Typename of const pointer to searchable voxel grid leaf. */
+  using TargetGridLeafConstPtr = typename TargetGrid::LeafConstPtr;
+
+public:
+  using Ptr = shared_ptr<NormalDistributionsTransform<PointSource, PointTarget>>;
+  using ConstPtr =
+      shared_ptr<const NormalDistributionsTransform<PointSource, PointTarget>>;
+
+  /** \brief Constructor.
+   * Sets \ref outlier_ratio_ to 0.35, \ref step_size_ to 0.05 and \ref resolution_
+   * to 1.0
+   */
+  NormalDistributionsTransform();
+
+  /** \brief Empty destructor */
+  ~NormalDistributionsTransform() {}
+
+  /** \brief Provide a pointer to the input target (e.g., the point cloud that we want
+   * to align the input source to). \param[in] cloud the input point cloud target
+   */
+  inline void
+  setInputTarget(const PointCloudTargetConstPtr& cloud) override
   {
-    protected:
-
-      using PointCloudSource = typename Registration<PointSource, PointTarget>::PointCloudSource;
-      using PointCloudSourcePtr = typename PointCloudSource::Ptr;
-      using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
-
-      using PointCloudTarget = typename Registration<PointSource, PointTarget>::PointCloudTarget;
-      using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
-      using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
-
-      using PointIndicesPtr = PointIndices::Ptr;
-      using PointIndicesConstPtr = PointIndices::ConstPtr;
-
-      /** \brief Typename of searchable voxel grid containing mean and covariance. */
-      using TargetGrid = VoxelGridCovariance<PointTarget>;
-      /** \brief Typename of pointer to searchable voxel grid. */
-      using TargetGridPtr = TargetGrid *;
-      /** \brief Typename of const pointer to searchable voxel grid. */
-      using TargetGridConstPtr = const TargetGrid *;
-      /** \brief Typename of const pointer to searchable voxel grid leaf. */
-      using TargetGridLeafConstPtr = typename TargetGrid::LeafConstPtr;
-
-
-    public:
-
-      using Ptr = shared_ptr< NormalDistributionsTransform<PointSource, PointTarget> >;
-      using ConstPtr = shared_ptr< const NormalDistributionsTransform<PointSource, PointTarget> >;
-
-
-      /** \brief Constructor.
-        * Sets \ref outlier_ratio_ to 0.35, \ref step_size_ to 0.05 and \ref resolution_ to 1.0
-        */
-      NormalDistributionsTransform ();
-      
-      /** \brief Empty destructor */
-      ~NormalDistributionsTransform () {}
-
-      /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to).
-        * \param[in] cloud the input point cloud target
-        */
-      inline void
-      setInputTarget (const PointCloudTargetConstPtr &cloud) override
-      {
-        Registration<PointSource, PointTarget>::setInputTarget (cloud);
-        init ();
-      }
-
-      /** \brief Set/change the voxel grid resolution.
-        * \param[in] resolution side length of voxels
-        */
-      inline void
-      setResolution (float resolution)
-      {
-        // Prevents unnessary voxel initiations
-        if (resolution_ != resolution)
-        {
-          resolution_ = resolution;
-          if (input_)
-            init ();
-        }
-      }
-
-      /** \brief Get voxel grid resolution.
-        * \return side length of voxels
-        */
-      inline float
-      getResolution () const
-      {
-        return (resolution_);
-      }
-
-      /** \brief Get the newton line search maximum step length.
-        * \return maximum step length
-        */
-      inline double
-      getStepSize () const
-      {
-        return (step_size_);
-      }
-
-      /** \brief Set/change the newton line search maximum step length.
-        * \param[in] step_size maximum step length
-        */
-      inline void
-      setStepSize (double step_size)
-      {
-        step_size_ = step_size;
-      }
-
-      /** \brief Get the point cloud outlier ratio.
-        * \return outlier ratio
-        */
-      inline double
-      getOulierRatio () const
-      {
-        return (outlier_ratio_);
-      }
-
-      /** \brief Set/change the point cloud outlier ratio.
-        * \param[in] outlier_ratio outlier ratio
-        */
-      inline void
-      setOulierRatio (double outlier_ratio)
-      {
-        outlier_ratio_ = outlier_ratio;
-      }
-
-      /** \brief Get the registration alignment probability.
-        * \return transformation probability
-        */
-      inline double
-      getTransformationProbability () const
-      {
-        return (trans_probability_);
-      }
-
-      /** \brief Get the number of iterations required to calculate alignment.
-        * \return final number of iterations
-        */
-      inline int
-      getFinalNumIteration () const
-      {
-        return (nr_iterations_);
-      }
-
-      /** \brief Convert 6 element transformation vector to affine transformation.
-        * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw]
-        * \param[out] trans affine transform corresponding to given transfomation vector
-        */
-      static void
-      convertTransform (const Eigen::Matrix<double, 6, 1> &x, Eigen::Affine3f &trans)
-      {
-        trans = Eigen::Translation<float, 3>(float (x (0)), float (x (1)), float (x (2))) *
-                Eigen::AngleAxis<float>(float (x (3)), Eigen::Vector3f::UnitX ()) *
-                Eigen::AngleAxis<float>(float (x (4)), Eigen::Vector3f::UnitY ()) *
-                Eigen::AngleAxis<float>(float (x (5)), Eigen::Vector3f::UnitZ ());
-      }
-
-      /** \brief Convert 6 element transformation vector to transformation matrix.
-        * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw]
-        * \param[out] trans 4x4 transformation matrix corresponding to given transfomation vector
-        */
-      static void
-      convertTransform (const Eigen::Matrix<double, 6, 1> &x, Eigen::Matrix4f &trans)
-      {
-        Eigen::Affine3f _affine;
-        convertTransform (x, _affine);
-        trans = _affine.matrix ();
-      }
-
-    protected:
-
-      using Registration<PointSource, PointTarget>::reg_name_;
-      using Registration<PointSource, PointTarget>::getClassName;
-      using Registration<PointSource, PointTarget>::input_;
-      using Registration<PointSource, PointTarget>::indices_;
-      using Registration<PointSource, PointTarget>::target_;
-      using Registration<PointSource, PointTarget>::nr_iterations_;
-      using Registration<PointSource, PointTarget>::max_iterations_;
-      using Registration<PointSource, PointTarget>::previous_transformation_;
-      using Registration<PointSource, PointTarget>::final_transformation_;
-      using Registration<PointSource, PointTarget>::transformation_;
-      using Registration<PointSource, PointTarget>::transformation_epsilon_;
-      using Registration<PointSource, PointTarget>::transformation_rotation_epsilon_;
-      using Registration<PointSource, PointTarget>::converged_;
-      using Registration<PointSource, PointTarget>::corr_dist_threshold_;
-      using Registration<PointSource, PointTarget>::inlier_threshold_;
-
-      using Registration<PointSource, PointTarget>::update_visualizer_;
-
-      /** \brief Estimate the transformation and returns the transformed source (input) as output.
-        * \param[out] output the resultant input transformed point cloud dataset
-        */
-      virtual void
-      computeTransformation (PointCloudSource &output)
-      {
-        computeTransformation (output, Eigen::Matrix4f::Identity ());
-      }
-
-      /** \brief Estimate the transformation and returns the transformed source (input) as output.
-        * \param[out] output the resultant input transformed point cloud dataset
-        * \param[in] guess the initial gross estimation of the transformation
-        */
-      void
-      computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) override;
-
-      /** \brief Initiate covariance voxel structure. */
-      void inline
-      init ()
-      {
-        target_cells_.setLeafSize (resolution_, resolution_, resolution_);
-        target_cells_.setInputCloud ( target_ );
-        // Initiate voxel structure.
-        target_cells_.filter (true);
+    Registration<PointSource, PointTarget>::setInputTarget(cloud);
+    init();
+  }
+
+  /** \brief Set/change the voxel grid resolution.
+   * \param[in] resolution side length of voxels
+   */
+  inline void
+  setResolution(float resolution)
+  {
+    // Prevents unnessary voxel initiations
+    if (resolution_ != resolution) {
+      resolution_ = resolution;
+      if (input_) {
+        init();
       }
-
-      /** \brief Compute derivatives of probability function w.r.t. the transformation vector.
-        * \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009].
-        * \param[out] score_gradient the gradient vector of the probability function w.r.t. the transformation vector
-        * \param[out] hessian the hessian matrix of the probability function w.r.t. the transformation vector
-        * \param[in] trans_cloud transformed point cloud
-        * \param[in] p the current transform vector
-        * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation.
-        */
-      double
-      computeDerivatives (Eigen::Matrix<double, 6, 1> &score_gradient,
-                          Eigen::Matrix<double, 6, 6> &hessian,
-                          PointCloudSource &trans_cloud,
-                          Eigen::Matrix<double, 6, 1> &p,
+    }
+  }
+
+  /** \brief Get voxel grid resolution.
+   * \return side length of voxels
+   */
+  inline float
+  getResolution() const
+  {
+    return resolution_;
+  }
+
+  /** \brief Get the newton line search maximum step length.
+   * \return maximum step length
+   */
+  inline double
+  getStepSize() const
+  {
+    return step_size_;
+  }
+
+  /** \brief Set/change the newton line search maximum step length.
+   * \param[in] step_size maximum step length
+   */
+  inline void
+  setStepSize(double step_size)
+  {
+    step_size_ = step_size;
+  }
+
+  /** \brief Get the point cloud outlier ratio.
+   * \return outlier ratio
+   */
+  inline double
+  getOulierRatio() const
+  {
+    return outlier_ratio_;
+  }
+
+  /** \brief Set/change the point cloud outlier ratio.
+   * \param[in] outlier_ratio outlier ratio
+   */
+  inline void
+  setOulierRatio(double outlier_ratio)
+  {
+    outlier_ratio_ = outlier_ratio;
+  }
+
+  /** \brief Get the registration alignment probability.
+   * \return transformation probability
+   */
+  inline double
+  getTransformationProbability() const
+  {
+    return trans_probability_;
+  }
+
+  /** \brief Get the number of iterations required to calculate alignment.
+   * \return final number of iterations
+   */
+  inline int
+  getFinalNumIteration() const
+  {
+    return nr_iterations_;
+  }
+
+  /** \brief Convert 6 element transformation vector to affine transformation.
+   * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw]
+   * \param[out] trans affine transform corresponding to given transfomation vector
+   */
+  static void
+  convertTransform(const Eigen::Matrix<double, 6, 1>& x, Eigen::Affine3f& trans)
+  {
+    trans = Eigen::Translation<float, 3>(x.head<3>().cast<float>()) *
+            Eigen::AngleAxis<float>(float(x(3)), Eigen::Vector3f::UnitX()) *
+            Eigen::AngleAxis<float>(float(x(4)), Eigen::Vector3f::UnitY()) *
+            Eigen::AngleAxis<float>(float(x(5)), Eigen::Vector3f::UnitZ());
+  }
+
+  /** \brief Convert 6 element transformation vector to transformation matrix.
+   * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw]
+   * \param[out] trans 4x4 transformation matrix corresponding to given transfomation
+   * vector
+   */
+  static void
+  convertTransform(const Eigen::Matrix<double, 6, 1>& x, Eigen::Matrix4f& trans)
+  {
+    Eigen::Affine3f _affine;
+    convertTransform(x, _affine);
+    trans = _affine.matrix();
+  }
+
+protected:
+  using Registration<PointSource, PointTarget>::reg_name_;
+  using Registration<PointSource, PointTarget>::getClassName;
+  using Registration<PointSource, PointTarget>::input_;
+  using Registration<PointSource, PointTarget>::indices_;
+  using Registration<PointSource, PointTarget>::target_;
+  using Registration<PointSource, PointTarget>::nr_iterations_;
+  using Registration<PointSource, PointTarget>::max_iterations_;
+  using Registration<PointSource, PointTarget>::previous_transformation_;
+  using Registration<PointSource, PointTarget>::final_transformation_;
+  using Registration<PointSource, PointTarget>::transformation_;
+  using Registration<PointSource, PointTarget>::transformation_epsilon_;
+  using Registration<PointSource, PointTarget>::transformation_rotation_epsilon_;
+  using Registration<PointSource, PointTarget>::converged_;
+  using Registration<PointSource, PointTarget>::corr_dist_threshold_;
+  using Registration<PointSource, PointTarget>::inlier_threshold_;
+
+  using Registration<PointSource, PointTarget>::update_visualizer_;
+
+  /** \brief Estimate the transformation and returns the transformed source (input) as
+   * output. \param[out] output the resultant input transformed point cloud dataset
+   */
+  virtual void
+  computeTransformation(PointCloudSource& output)
+  {
+    computeTransformation(output, Eigen::Matrix4f::Identity());
+  }
+
+  /** \brief Estimate the transformation and returns the transformed source (input) as
+   * output. \param[out] output the resultant input transformed point cloud dataset
+   * \param[in] guess the initial gross estimation of the transformation
+   */
+  void
+  computeTransformation(PointCloudSource& output,
+                        const Eigen::Matrix4f& guess) override;
+
+  /** \brief Initiate covariance voxel structure. */
+  void inline init()
+  {
+    target_cells_.setLeafSize(resolution_, resolution_, resolution_);
+    target_cells_.setInputCloud(target_);
+    // Initiate voxel structure.
+    target_cells_.filter(true);
+  }
+
+  /** \brief Compute derivatives of probability function w.r.t. the transformation
+   * vector. \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009]. \param[out]
+   * score_gradient the gradient vector of the probability function w.r.t. the
+   * transformation vector \param[out] hessian the hessian matrix of the probability
+   * function w.r.t. the transformation vector \param[in] trans_cloud transformed point
+   * cloud \param[in] transform the current transform vector \param[in] compute_hessian
+   * flag to calculate hessian, unnessissary for step calculation.
+   */
+  double
+  computeDerivatives(Eigen::Matrix<double, 6, 1>& score_gradient,
+                     Eigen::Matrix<double, 6, 6>& hessian,
+                     const PointCloudSource& trans_cloud,
+                     const Eigen::Matrix<double, 6, 1>& transform,
+                     bool compute_hessian = true);
+
+  /** \brief Compute individual point contirbutions to derivatives of probability
+   * function w.r.t. the transformation vector. \note Equation 6.10, 6.12 and 6.13
+   * [Magnusson 2009]. \param[in,out] score_gradient the gradient vector of the
+   * probability function w.r.t. the transformation vector \param[in,out] hessian the
+   * hessian matrix of the probability function w.r.t. the transformation vector
+   * \param[in] x_trans transformed point minus mean of occupied covariance voxel
+   * \param[in] c_inv covariance of occupied covariance voxel
+   * \param[in] compute_hessian flag to calculate hessian, unnessissary for step
+   * calculation.
+   */
+  double
+  updateDerivatives(Eigen::Matrix<double, 6, 1>& score_gradient,
+                    Eigen::Matrix<double, 6, 6>& hessian,
+                    const Eigen::Vector3d& x_trans,
+                    const Eigen::Matrix3d& c_inv,
+                    bool compute_hessian = true) const;
+
+  /** \brief Precompute anglular components of derivatives.
+   * \note Equation 6.19 and 6.21 [Magnusson 2009].
+   * \param[in] transform the current transform vector
+   * \param[in] compute_hessian flag to calculate hessian, unnessissary for step
+   * calculation.
+   */
+  void
+  computeAngleDerivatives(const Eigen::Matrix<double, 6, 1>& transform,
                           bool compute_hessian = true);
 
-      /** \brief Compute individual point contirbutions to derivatives of probability function w.r.t. the transformation vector.
-        * \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009].
-        * \param[in,out] score_gradient the gradient vector of the probability function w.r.t. the transformation vector
-        * \param[in,out] hessian the hessian matrix of the probability function w.r.t. the transformation vector
-        * \param[in] x_trans transformed point minus mean of occupied covariance voxel
-        * \param[in] c_inv covariance of occupied covariance voxel
-        * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation.
-        */
-      double
-      updateDerivatives (Eigen::Matrix<double, 6, 1> &score_gradient,
-                         Eigen::Matrix<double, 6, 6> &hessian,
-                         Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv,
-                         bool compute_hessian = true);
-
-      /** \brief Precompute anglular components of derivatives.
-        * \note Equation 6.19 and 6.21 [Magnusson 2009].
-        * \param[in] p the current transform vector
-        * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation.
-        */
-      void
-      computeAngleDerivatives (Eigen::Matrix<double, 6, 1> &p, bool compute_hessian = true);
-
-      /** \brief Compute point derivatives.
-        * \note Equation 6.18-21 [Magnusson 2009].
-        * \param[in] x point from the input cloud
-        * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation.
-        */
-      void
-      computePointDerivatives (Eigen::Vector3d &x, bool compute_hessian = true);
-
-      /** \brief Compute hessian of probability function w.r.t. the transformation vector.
-        * \note Equation 6.13 [Magnusson 2009].
-        * \param[out] hessian the hessian matrix of the probability function w.r.t. the transformation vector
-        * \param[in] trans_cloud transformed point cloud
-        * \param[in] p the current transform vector
-        */
-      void
-      computeHessian (Eigen::Matrix<double, 6, 6> &hessian,
-                      PointCloudSource &trans_cloud,
-                      Eigen::Matrix<double, 6, 1> &p);
-
-      /** \brief Compute individual point contirbutions to hessian of probability function w.r.t. the transformation vector.
-        * \note Equation 6.13 [Magnusson 2009].
-        * \param[in,out] hessian the hessian matrix of the probability function w.r.t. the transformation vector
-        * \param[in] x_trans transformed point minus mean of occupied covariance voxel
-        * \param[in] c_inv covariance of occupied covariance voxel
-        */
-      void
-      updateHessian (Eigen::Matrix<double, 6, 6> &hessian,
-                     Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv);
-
-      /** \brief Compute line search step length and update transform and probability derivatives using More-Thuente method.
-        * \note Search Algorithm [More, Thuente 1994]
-        * \param[in] x initial transformation vector, \f$ x \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ \vec{p} \f$ in Algorithm 2 [Magnusson 2009]
-        * \param[in] step_dir descent direction, \f$ p \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ \delta \vec{p} \f$ normalized in Algorithm 2 [Magnusson 2009]
-        * \param[in] step_init initial step length estimate, \f$ \alpha_0 \f$ in Moore-Thuente (1994) and the noramal of \f$ \delta \vec{p} \f$ in Algorithm 2 [Magnusson 2009]
-        * \param[in] step_max maximum step length, \f$ \alpha_max \f$ in Moore-Thuente (1994)
-        * \param[in] step_min minimum step length, \f$ \alpha_min \f$ in Moore-Thuente (1994)
-        * \param[out] score final score function value, \f$ f(x + \alpha p) \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ score \f$ in Algorithm 2 [Magnusson 2009]
-        * \param[in,out] score_gradient gradient of score function w.r.t. transformation vector, \f$ f'(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$ \vec{g} \f$ in Algorithm 2 [Magnusson 2009]
-        * \param[out] hessian hessian of score function w.r.t. transformation vector, \f$ f''(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$ H \f$ in Algorithm 2 [Magnusson 2009]
-        * \param[in,out] trans_cloud transformed point cloud, \f$ X \f$ transformed by \f$ T(\vec{p},\vec{x}) \f$ in Algorithm 2 [Magnusson 2009]
-        * \return final step length
-        */
-      double
-      computeStepLengthMT (const Eigen::Matrix<double, 6, 1> &x,
-                           Eigen::Matrix<double, 6, 1> &step_dir,
-                           double step_init,
-                           double step_max, double step_min,
-                           double &score,
-                           Eigen::Matrix<double, 6, 1> &score_gradient,
-                           Eigen::Matrix<double, 6, 6> &hessian,
-                           PointCloudSource &trans_cloud);
-
-      /** \brief Update interval of possible step lengths for More-Thuente method, \f$ I \f$ in More-Thuente (1994)
-        * \note Updating Algorithm until some value satisfies \f$ \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$
-        * and Modified Updating Algorithm from then on [More, Thuente 1994].
-        * \param[in,out] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$ in Moore-Thuente (1994)
-        * \param[in,out] f_l value at first endpoint, \f$ f_l \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_l) \f$ for Update Algorithm and \f$ \phi(\alpha_l) \f$ for Modified Update Algorithm
-        * \param[in,out] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente (1994), \f$ \psi'(\alpha_l) \f$ for Update Algorithm and \f$ \phi'(\alpha_l) \f$ for Modified Update Algorithm
-        * \param[in,out] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$ in Moore-Thuente (1994)
-        * \param[in,out] f_u value at second endpoint, \f$ f_u \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_u) \f$ for Update Algorithm and \f$ \phi(\alpha_u) \f$ for Modified Update Algorithm
-        * \param[in,out] g_u derivative at second endpoint, \f$ g_u \f$ in Moore-Thuente (1994), \f$ \psi'(\alpha_u) \f$ for Update Algorithm and \f$ \phi'(\alpha_u) \f$ for Modified Update Algorithm
-        * \param[in] a_t trial value, \f$ \alpha_t \f$ in Moore-Thuente (1994)
-        * \param[in] f_t value at trial value, \f$ f_t \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_t) \f$ for Update Algorithm and \f$ \phi(\alpha_t) \f$ for Modified Update Algorithm
-        * \param[in] g_t derivative at trial value, \f$ g_t \f$ in Moore-Thuente (1994), \f$ \psi'(\alpha_t) \f$ for Update Algorithm and \f$ \phi'(\alpha_t) \f$ for Modified Update Algorithm
-        * \return if interval converges
-        */
-      bool
-      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);
-
-      /** \brief Select new trial value for More-Thuente method.
-        * \note Trial Value Selection [More, Thuente 1994], \f$ \psi(\alpha_k) \f$ is used for \f$ f_k \f$ and \f$ g_k \f$
-        * until some value satisfies the test \f$ \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$
-        * then \f$ \phi(\alpha_k) \f$ is used from then on.
-        * \note Interpolation Minimizer equations from Optimization Theory and Methods: Nonlinear Programming By Wenyu Sun, Ya-xiang Yuan (89-100).
-        * \param[in] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$ in Moore-Thuente (1994)
-        * \param[in] f_l value at first endpoint, \f$ f_l \f$ in Moore-Thuente (1994)
-        * \param[in] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente (1994)
-        * \param[in] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$ in Moore-Thuente (1994)
-        * \param[in] f_u value at second endpoint, \f$ f_u \f$ in Moore-Thuente (1994)
-        * \param[in] g_u derivative at second endpoint, \f$ g_u \f$ in Moore-Thuente (1994)
-        * \param[in] a_t previous trial value, \f$ \alpha_t \f$ in Moore-Thuente (1994)
-        * \param[in] f_t value at previous trial value, \f$ f_t \f$ in Moore-Thuente (1994)
-        * \param[in] g_t derivative at previous trial value, \f$ g_t \f$ in Moore-Thuente (1994)
-        * \return new trial value
-        */
-      double
-      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);
-
-      /** \brief Auxiliary function used to determine endpoints of More-Thuente interval.
-        * \note \f$ \psi(\alpha) \f$ in Equation 1.6 (Moore, Thuente 1994)
-        * \param[in] a the step length, \f$ \alpha \f$ in More-Thuente (1994)
-        * \param[in] f_a function value at step length a, \f$ \phi(\alpha) \f$ in More-Thuente (1994)
-        * \param[in] f_0 initial function value, \f$ \phi(0) \f$ in Moore-Thuente (1994)
-        * \param[in] g_0 initial function gradiant, \f$ \phi'(0) \f$ in More-Thuente (1994)
-        * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More, Thuente 1994]
-        * \return sufficient decrease value
-        */
-      inline double
-      auxilaryFunction_PsiMT (double a, double f_a, double f_0, double g_0, double mu = 1.e-4)
-      {
-        return (f_a - f_0 - mu * g_0 * a);
-      }
-
-      /** \brief Auxiliary function derivative used to determine endpoints of More-Thuente interval.
-        * \note \f$ \psi'(\alpha) \f$, derivative of Equation 1.6 (Moore, Thuente 1994)
-        * \param[in] g_a function gradient at step length a, \f$ \phi'(\alpha) \f$ in More-Thuente (1994)
-        * \param[in] g_0 initial function gradiant, \f$ \phi'(0) \f$ in More-Thuente (1994)
-        * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More, Thuente 1994]
-        * \return sufficient decrease derivative
-        */
-      inline double
-      auxilaryFunction_dPsiMT (double g_a, double g_0, double mu = 1.e-4)
-      {
-        return (g_a - mu * g_0);
-      }
-
-      /** \brief The voxel grid generated from target cloud containing point means and covariances. */
-      TargetGrid target_cells_;
-
-      //double fitness_epsilon_;
-
-      /** \brief The side length of voxels. */
-      float resolution_;
-
-      /** \brief The maximum step length. */
-      double step_size_;
-
-      /** \brief The ratio of outliers of points w.r.t. a normal distribution, Equation 6.7 [Magnusson 2009]. */
-      double outlier_ratio_;
-
-      /** \brief The normalization constants used fit the point distribution to a normal distribution, Equation 6.8 [Magnusson 2009]. */
-      double gauss_d1_, gauss_d2_;
-
-      /** \brief The probability score of the transform applied to the input cloud, Equation 6.9 and 6.10 [Magnusson 2009]. */
-      double trans_probability_;
-
-      /** \brief Precomputed Angular Gradient
-        *
-        * The precomputed angular derivatives for the jacobian of a transformation vector, Equation 6.19 [Magnusson 2009]. 
-        */
-      Eigen::Vector3d j_ang_a_, j_ang_b_, j_ang_c_, j_ang_d_, j_ang_e_, j_ang_f_, j_ang_g_, j_ang_h_;
-
-      /** \brief Precomputed Angular Hessian
-        *
-        * The precomputed angular derivatives for the hessian of a transformation vector, Equation 6.19 [Magnusson 2009].
-        */
-      Eigen::Vector3d h_ang_a2_, h_ang_a3_,
-                      h_ang_b2_, h_ang_b3_,
-                      h_ang_c2_, h_ang_c3_,
-                      h_ang_d1_, h_ang_d2_, h_ang_d3_,
-                      h_ang_e1_, h_ang_e2_, h_ang_e3_,
-                      h_ang_f1_, h_ang_f2_, h_ang_f3_;
-
-      /** \brief The first order derivative of the transformation of a point w.r.t. the transform vector, \f$ J_E \f$ in Equation 6.18 [Magnusson 2009]. */
-      Eigen::Matrix<double, 3, 6> point_gradient_;
-
-      /** \brief The second order derivative of the transformation of a point w.r.t. the transform vector, \f$ H_E \f$ in Equation 6.20 [Magnusson 2009]. */
-      Eigen::Matrix<double, 18, 6> point_hessian_;
-
-    public:
-      PCL_MAKE_ALIGNED_OPERATOR_NEW
-  };
-}
+  /** \brief Compute point derivatives.
+   * \note Equation 6.18-21 [Magnusson 2009].
+   * \param[in] x point from the input cloud
+   * \param[in] compute_hessian flag to calculate hessian, unnessissary for step
+   * calculation.
+   */
+  void
+  computePointDerivatives(const Eigen::Vector3d& x, bool compute_hessian = true);
+
+  /** \brief Compute hessian of probability function w.r.t. the transformation vector.
+   * \note Equation 6.13 [Magnusson 2009].
+   * \param[out] hessian the hessian matrix of the probability function w.r.t. the
+   * transformation vector \param[in] trans_cloud transformed point cloud
+   */
+  void
+  computeHessian(Eigen::Matrix<double, 6, 6>& hessian,
+                 const PointCloudSource& trans_cloud);
+
+  /** \brief Compute hessian of probability function w.r.t. the transformation vector.
+   * \note Equation 6.13 [Magnusson 2009].
+   * \param[out] hessian the hessian matrix of the probability function w.r.t. the
+   * transformation vector \param[in] trans_cloud transformed point cloud \param[in]
+   * transform the current transform vector
+   */
+  PCL_DEPRECATED(1, 15, "Parameter `transform` is not required")
+  void
+  computeHessian(Eigen::Matrix<double, 6, 6>& hessian,
+                 const PointCloudSource& trans_cloud,
+                 const Eigen::Matrix<double, 6, 1>& transform)
+  {
+    pcl::utils::ignore(transform);
+    computeHessian(hessian, trans_cloud);
+  }
+
+  /** \brief Compute individual point contirbutions to hessian of probability function
+   * w.r.t. the transformation vector. \note Equation 6.13 [Magnusson 2009].
+   * \param[in,out] hessian the hessian matrix of the probability function w.r.t. the
+   * transformation vector \param[in] x_trans transformed point minus mean of occupied
+   * covariance voxel \param[in] c_inv covariance of occupied covariance voxel
+   */
+  void
+  updateHessian(Eigen::Matrix<double, 6, 6>& hessian,
+                const Eigen::Vector3d& x_trans,
+                const Eigen::Matrix3d& c_inv) const;
+
+  /** \brief Compute line search step length and update transform and probability
+   * derivatives using More-Thuente method. \note Search Algorithm [More, Thuente 1994]
+   * \param[in] transform initial transformation vector, \f$ x \f$ in Equation 1.3
+   * (Moore, Thuente 1994) and \f$ \vec{p} \f$ in Algorithm 2 [Magnusson 2009]
+   * \param[in] step_dir descent direction, \f$ p \f$ in Equation 1.3 (Moore, Thuente
+   * 1994) and \f$ \delta \vec{p} \f$ normalized in Algorithm 2 [Magnusson 2009]
+   * \param[in] step_init initial step length estimate, \f$ \alpha_0 \f$ in
+   * Moore-Thuente (1994) and the noramal of \f$ \delta \vec{p} \f$ in Algorithm 2
+   * [Magnusson 2009] \param[in] step_max maximum step length, \f$ \alpha_max \f$ in
+   * Moore-Thuente (1994) \param[in] step_min minimum step length, \f$ \alpha_min \f$ in
+   * Moore-Thuente (1994) \param[out] score final score function value, \f$ f(x + \alpha
+   * p) \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ score \f$ in Algorithm 2
+   * [Magnusson 2009] \param[in,out] score_gradient gradient of score function w.r.t.
+   * transformation vector, \f$ f'(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$
+   * \vec{g} \f$ in Algorithm 2 [Magnusson 2009] \param[out] hessian hessian of score
+   * function w.r.t. transformation vector, \f$ f''(x + \alpha p) \f$ in Moore-Thuente
+   * (1994) and \f$ H \f$ in Algorithm 2 [Magnusson 2009] \param[in,out] trans_cloud
+   * transformed point cloud, \f$ X \f$ transformed by \f$ T(\vec{p},\vec{x}) \f$ in
+   * Algorithm 2 [Magnusson 2009] \return final step length
+   */
+  double
+  computeStepLengthMT(const Eigen::Matrix<double, 6, 1>& transform,
+                      Eigen::Matrix<double, 6, 1>& step_dir,
+                      double step_init,
+                      double step_max,
+                      double step_min,
+                      double& score,
+                      Eigen::Matrix<double, 6, 1>& score_gradient,
+                      Eigen::Matrix<double, 6, 6>& hessian,
+                      PointCloudSource& trans_cloud);
+
+  /** \brief Update interval of possible step lengths for More-Thuente method, \f$ I \f$
+   * in More-Thuente (1994) \note Updating Algorithm until some value satisfies \f$
+   * \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$ and Modified Updating
+   * Algorithm from then on [More, Thuente 1994]. \param[in,out] a_l first endpoint of
+   * interval \f$ I \f$, \f$ \alpha_l \f$ in Moore-Thuente (1994) \param[in,out] f_l
+   * value at first endpoint, \f$ f_l \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_l)
+   * \f$ for Update Algorithm and \f$ \phi(\alpha_l) \f$ for Modified Update Algorithm
+   * \param[in,out] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente
+   * (1994), \f$ \psi'(\alpha_l) \f$ for Update Algorithm and \f$ \phi'(\alpha_l) \f$
+   * for Modified Update Algorithm \param[in,out] a_u second endpoint of interval \f$ I
+   * \f$, \f$ \alpha_u \f$ in Moore-Thuente (1994) \param[in,out] f_u value at second
+   * endpoint, \f$ f_u \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_u) \f$ for Update
+   * Algorithm and \f$ \phi(\alpha_u) \f$ for Modified Update Algorithm \param[in,out]
+   * g_u derivative at second endpoint, \f$ g_u \f$ in Moore-Thuente (1994), \f$
+   * \psi'(\alpha_u) \f$ for Update Algorithm and \f$ \phi'(\alpha_u) \f$ for Modified
+   * Update Algorithm \param[in] a_t trial value, \f$ \alpha_t \f$ in Moore-Thuente
+   * (1994) \param[in] f_t value at trial value, \f$ f_t \f$ in Moore-Thuente (1994),
+   * \f$ \psi(\alpha_t) \f$ for Update Algorithm and \f$ \phi(\alpha_t) \f$ for Modified
+   * Update Algorithm \param[in] g_t derivative at trial value, \f$ g_t \f$ in
+   * Moore-Thuente (1994), \f$ \psi'(\alpha_t) \f$ for Update Algorithm and \f$
+   * \phi'(\alpha_t) \f$ for Modified Update Algorithm \return if interval converges
+   */
+  bool
+  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) const;
+
+  /** \brief Select new trial value for More-Thuente method.
+   * \note Trial Value Selection [More, Thuente 1994], \f$ \psi(\alpha_k) \f$ is used
+   * for \f$ f_k \f$ and \f$ g_k \f$ until some value satisfies the test \f$
+   * \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$ then \f$
+   * \phi(\alpha_k) \f$ is used from then on. \note Interpolation Minimizer equations
+   * from Optimization Theory and Methods: Nonlinear Programming By Wenyu Sun, Ya-xiang
+   * Yuan (89-100). \param[in] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l
+   * \f$ in Moore-Thuente (1994) \param[in] f_l value at first endpoint, \f$ f_l \f$ in
+   * Moore-Thuente (1994) \param[in] g_l derivative at first endpoint, \f$ g_l \f$ in
+   * Moore-Thuente (1994) \param[in] a_u second endpoint of interval \f$ I \f$, \f$
+   * \alpha_u \f$ in Moore-Thuente (1994) \param[in] f_u value at second endpoint, \f$
+   * f_u \f$ in Moore-Thuente (1994) \param[in] g_u derivative at second endpoint, \f$
+   * g_u \f$ in Moore-Thuente (1994) \param[in] a_t previous trial value, \f$ \alpha_t
+   * \f$ in Moore-Thuente (1994) \param[in] f_t value at previous trial value, \f$ f_t
+   * \f$ in Moore-Thuente (1994) \param[in] g_t derivative at previous trial value, \f$
+   * g_t \f$ in Moore-Thuente (1994) \return new trial value
+   */
+  double
+  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) const;
+
+  /** \brief Auxiliary function used to determine endpoints of More-Thuente interval.
+   * \note \f$ \psi(\alpha) \f$ in Equation 1.6 (Moore, Thuente 1994)
+   * \param[in] a the step length, \f$ \alpha \f$ in More-Thuente (1994)
+   * \param[in] f_a function value at step length a, \f$ \phi(\alpha) \f$ in
+   * More-Thuente (1994) \param[in] f_0 initial function value, \f$ \phi(0) \f$ in
+   * Moore-Thuente (1994) \param[in] g_0 initial function gradient, \f$ \phi'(0) \f$ in
+   * More-Thuente (1994) \param[in] mu the step length, constant \f$ \mu \f$ in
+   * Equation 1.1 [More, Thuente 1994] \return sufficient decrease value
+   */
+  inline double
+  auxilaryFunction_PsiMT(
+      double a, double f_a, double f_0, double g_0, double mu = 1.e-4) const
+  {
+    return f_a - f_0 - mu * g_0 * a;
+  }
+
+  /** \brief Auxiliary function derivative used to determine endpoints of More-Thuente
+   * interval. \note \f$ \psi'(\alpha) \f$, derivative of Equation 1.6 (Moore, Thuente
+   * 1994) \param[in] g_a function gradient at step length a, \f$ \phi'(\alpha) \f$ in
+   * More-Thuente (1994) \param[in] g_0 initial function gradient, \f$ \phi'(0) \f$ in
+   * More-Thuente (1994) \param[in] mu the step length, constant \f$ \mu \f$ in
+   * Equation 1.1 [More, Thuente 1994] \return sufficient decrease derivative
+   */
+  inline double
+  auxilaryFunction_dPsiMT(double g_a, double g_0, double mu = 1.e-4) const
+  {
+    return g_a - mu * g_0;
+  }
+
+  /** \brief The voxel grid generated from target cloud containing point means and
+   * covariances. */
+  TargetGrid target_cells_;
+
+  /** \brief The side length of voxels. */
+  float resolution_;
+
+  /** \brief The maximum step length. */
+  double step_size_;
+
+  /** \brief The ratio of outliers of points w.r.t. a normal distribution, Equation 6.7
+   * [Magnusson 2009]. */
+  double outlier_ratio_;
+
+  /** \brief The normalization constants used fit the point distribution to a normal
+   * distribution, Equation 6.8 [Magnusson 2009]. */
+  double gauss_d1_, gauss_d2_;
+
+  /** \brief The probability score of the transform applied to the input cloud,
+   * Equation 6.9 and 6.10 [Magnusson 2009]. */
+  double trans_probability_;
+
+  /** \brief Precomputed Angular Gradient
+   *
+   * The precomputed angular derivatives for the jacobian of a transformation vector,
+   * Equation 6.19 [Magnusson 2009].
+   */
+  Eigen::Matrix<double, 8, 4> angular_jacobian_;
+
+  /** \brief Precomputed Angular Hessian
+   *
+   * The precomputed angular derivatives for the hessian of a transformation vector,
+   * Equation 6.19 [Magnusson 2009].
+   */
+  Eigen::Matrix<double, 15, 4> angular_hessian_;
+
+  /** \brief The first order derivative of the transformation of a point w.r.t. the
+   * transform vector, \f$ J_E \f$ in Equation 6.18 [Magnusson 2009]. */
+  Eigen::Matrix<double, 3, 6> point_jacobian_;
+
+  /** \brief The second order derivative of the transformation of a point w.r.t. the
+   * transform vector, \f$ H_E \f$ in Equation 6.20 [Magnusson 2009]. */
+  Eigen::Matrix<double, 18, 6> point_hessian_;
+
+public:
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+};
+} // namespace pcl
 
 #include <pcl/registration/impl/ndt.hpp>
diff --git a/registration/include/pcl/registration/ndt_2d.h b/registration/include/pcl/registration/ndt_2d.h
index 8551db4f..0409da5a 100644
--- a/registration/include/pcl/registration/ndt_2d.h
+++ b/registration/include/pcl/registration/ndt_2d.h
@@ -40,116 +40,137 @@
 
 #pragma once
 
-#include <pcl/memory.h>
-#include <pcl/pcl_macros.h>
 #include <pcl/registration/registration.h>
+#include <pcl/memory.h>
+
+namespace pcl {
+/** \brief @b NormalDistributionsTransform2D provides an implementation of the
+ * Normal Distributions Transform algorithm for scan matching.
+ *
+ * This implementation is intended to match the definition:
+ * Peter Biber and Wolfgang Straßer. The normal distributions transform: A
+ * new approach to laser scan matching. In Proceedings of the IEEE In-
+ * ternational Conference on Intelligent Robots and Systems (IROS), pages
+ * 2743–2748, Las Vegas, USA, October 2003.
+ *
+ * \author James Crosby
+ */
+template <typename PointSource, typename PointTarget>
+class NormalDistributionsTransform2D : public Registration<PointSource, PointTarget> {
+  using PointCloudSource =
+      typename Registration<PointSource, PointTarget>::PointCloudSource;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+
+  using PointCloudTarget =
+      typename Registration<PointSource, PointTarget>::PointCloudTarget;
+
+  using PointIndicesPtr = PointIndices::Ptr;
+  using PointIndicesConstPtr = PointIndices::ConstPtr;
+
+public:
+  using Ptr = shared_ptr<NormalDistributionsTransform2D<PointSource, PointTarget>>;
+  using ConstPtr =
+      shared_ptr<const NormalDistributionsTransform2D<PointSource, PointTarget>>;
+
+  /** \brief Empty constructor. */
+  NormalDistributionsTransform2D()
+  : Registration<PointSource, PointTarget>()
+  , grid_centre_(0, 0)
+  , grid_step_(1, 1)
+  , grid_extent_(20, 20)
+  , newton_lambda_(1, 1, 1)
+  {
+    reg_name_ = "NormalDistributionsTransform2D";
+  }
+
+  /** \brief Empty destructor */
+  ~NormalDistributionsTransform2D() {}
 
-namespace pcl
-{
-  /** \brief @b NormalDistributionsTransform2D provides an implementation of the
-    * Normal Distributions Transform algorithm for scan matching.
-    *
-    * This implementation is intended to match the definition:
-    * Peter Biber and Wolfgang Straßer. The normal distributions transform: A
-    * new approach to laser scan matching. In Proceedings of the IEEE In-
-    * ternational Conference on Intelligent Robots and Systems (IROS), pages
-    * 2743–2748, Las Vegas, USA, October 2003.
-    *
-    * \author James Crosby
-    */
-  template <typename PointSource, typename PointTarget>
-  class NormalDistributionsTransform2D : public Registration<PointSource, PointTarget>
+  /** \brief centre of the ndt grid (target coordinate system)
+   * \param centre value to set
+   */
+  virtual void
+  setGridCentre(const Eigen::Vector2f& centre)
+  {
+    grid_centre_ = centre;
+  }
+
+  /** \brief Grid spacing (step) of the NDT grid
+   * \param[in] step value to set
+   */
+  virtual void
+  setGridStep(const Eigen::Vector2f& step)
+  {
+    grid_step_ = step;
+  }
+
+  /** \brief NDT Grid extent (in either direction from the grid centre)
+   * \param[in] extent value to set
+   */
+  virtual void
+  setGridExtent(const Eigen::Vector2f& extent)
+  {
+    grid_extent_ = extent;
+  }
+
+  /** \brief NDT Newton optimisation step size parameter
+   * \param[in] lambda step size: 1 is simple newton optimisation, smaller values may
+   * improve convergence
+   */
+  virtual void
+  setOptimizationStepSize(const double& lambda)
+  {
+    newton_lambda_ = Eigen::Vector3d(lambda, lambda, lambda);
+  }
+
+  /** \brief NDT Newton optimisation step size parameter
+   * \param[in] lambda step size: (1,1,1) is simple newton optimisation,
+   * smaller values may improve convergence, or elements may be set to
+   * zero to prevent optimisation over some parameters
+   *
+   * This overload allows control of updates to the individual (x, y,
+   * theta) free parameters in the optimisation. If, for example, theta is
+   * believed to be close to the correct value a small value of lambda[2]
+   * should be used.
+   */
+  virtual void
+  setOptimizationStepSize(const Eigen::Vector3d& lambda)
   {
-    using PointCloudSource = typename Registration<PointSource, PointTarget>::PointCloudSource;
-    using PointCloudSourcePtr = typename PointCloudSource::Ptr;
-    using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
-
-    using PointCloudTarget = typename Registration<PointSource, PointTarget>::PointCloudTarget;
-
-    using PointIndicesPtr = PointIndices::Ptr;
-    using PointIndicesConstPtr = PointIndices::ConstPtr;
-
-    public:
-
-        using Ptr = shared_ptr< NormalDistributionsTransform2D<PointSource, PointTarget> >;
-        using ConstPtr = shared_ptr< const NormalDistributionsTransform2D<PointSource, PointTarget> >;
-
-      /** \brief Empty constructor. */
-      NormalDistributionsTransform2D ()
-        : Registration<PointSource,PointTarget> (),
-          grid_centre_ (0,0), grid_step_ (1,1), grid_extent_ (20,20), newton_lambda_ (1,1,1)
-      {
-        reg_name_ = "NormalDistributionsTransform2D";
-      }
-      
-      /** \brief Empty destructor */
-      ~NormalDistributionsTransform2D () {}
- 
-      /** \brief centre of the ndt grid (target coordinate system)
-        * \param centre value to set
-        */
-      virtual void
-      setGridCentre (const Eigen::Vector2f& centre) { grid_centre_ = centre; }
-
-      /** \brief Grid spacing (step) of the NDT grid
-        * \param[in] step value to set
-        */
-      virtual void
-      setGridStep (const Eigen::Vector2f& step) { grid_step_ = step; }
-
-      /** \brief NDT Grid extent (in either direction from the grid centre)
-        * \param[in] extent value to set
-        */
-      virtual void
-      setGridExtent (const Eigen::Vector2f& extent) { grid_extent_ = extent; }
-
-      /** \brief NDT Newton optimisation step size parameter
-        * \param[in] lambda step size: 1 is simple newton optimisation, smaller values may improve convergence
-        */
-       virtual void
-       setOptimizationStepSize (const double& lambda) { newton_lambda_ = Eigen::Vector3d (lambda, lambda, lambda); }
-
-      /** \brief NDT Newton optimisation step size parameter
-        * \param[in] lambda step size: (1,1,1) is simple newton optimisation,
-        * smaller values may improve convergence, or elements may be set to
-        * zero to prevent optimisation over some parameters
-        *
-        * This overload allows control of updates to the individual (x, y,
-        * theta) free parameters in the optimisation. If, for example, theta is
-        * believed to be close to the correct value a small value of lambda[2]
-        * should be used.
-        */
-       virtual void
-       setOptimizationStepSize (const Eigen::Vector3d& lambda) { newton_lambda_ = lambda; }
-
-    protected:
-      /** \brief Rigid transformation computation method with initial guess.
-        * \param[out] output the transformed input point cloud dataset using the rigid transformation found
-        * \param[in] guess the initial guess of the transformation to compute
-        */
-      void 
-      computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) override;
-
-      using Registration<PointSource, PointTarget>::reg_name_;
-      using Registration<PointSource, PointTarget>::target_;
-      using Registration<PointSource, PointTarget>::converged_;
-      using Registration<PointSource, PointTarget>::nr_iterations_;
-      using Registration<PointSource, PointTarget>::max_iterations_;
-      using Registration<PointSource, PointTarget>::transformation_epsilon_;
-      using Registration<PointSource, PointTarget>::transformation_rotation_epsilon_;
-      using Registration<PointSource, PointTarget>::transformation_;
-      using Registration<PointSource, PointTarget>::previous_transformation_;
-      using Registration<PointSource, PointTarget>::final_transformation_;
-      using Registration<PointSource, PointTarget>::update_visualizer_;
-      using Registration<PointSource, PointTarget>::indices_;
-
-      Eigen::Vector2f grid_centre_;
-      Eigen::Vector2f grid_step_;
-      Eigen::Vector2f grid_extent_;
-      Eigen::Vector3d newton_lambda_;
-    public:
-      PCL_MAKE_ALIGNED_OPERATOR_NEW
-  };
+    newton_lambda_ = lambda;
+  }
+
+protected:
+  /** \brief Rigid transformation computation method with initial guess.
+   * \param[out] output the transformed input point cloud dataset using the rigid
+   * transformation found \param[in] guess the initial guess of the transformation to
+   * compute
+   */
+  void
+  computeTransformation(PointCloudSource& output,
+                        const Eigen::Matrix4f& guess) override;
+
+  using Registration<PointSource, PointTarget>::reg_name_;
+  using Registration<PointSource, PointTarget>::target_;
+  using Registration<PointSource, PointTarget>::converged_;
+  using Registration<PointSource, PointTarget>::nr_iterations_;
+  using Registration<PointSource, PointTarget>::max_iterations_;
+  using Registration<PointSource, PointTarget>::transformation_epsilon_;
+  using Registration<PointSource, PointTarget>::transformation_rotation_epsilon_;
+  using Registration<PointSource, PointTarget>::transformation_;
+  using Registration<PointSource, PointTarget>::previous_transformation_;
+  using Registration<PointSource, PointTarget>::final_transformation_;
+  using Registration<PointSource, PointTarget>::update_visualizer_;
+  using Registration<PointSource, PointTarget>::indices_;
+
+  Eigen::Vector2f grid_centre_;
+  Eigen::Vector2f grid_step_;
+  Eigen::Vector2f grid_extent_;
+  Eigen::Vector3d newton_lambda_;
+
+public:
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+};
 
 } // namespace pcl
 
diff --git a/registration/include/pcl/registration/pairwise_graph_registration.h b/registration/include/pcl/registration/pairwise_graph_registration.h
index 1e22ad7b..5d18c34d 100644
--- a/registration/include/pcl/registration/pairwise_graph_registration.h
+++ b/registration/include/pcl/registration/pairwise_graph_registration.h
@@ -43,73 +43,70 @@
 #include <pcl/registration/graph_registration.h>
 #include <pcl/registration/registration.h>
 
-namespace pcl
-{
-  /** \brief @b PairwiseGraphRegistration class aligns the clouds two by two
-    * \author Nicola Fioraio
-    * \ingroup registration
-    */
-  template <typename GraphT, typename PointT>
-  class PairwiseGraphRegistration : public GraphRegistration<GraphT>
-  {
-    public:
-      using GraphRegistration<GraphT>::graph_handler_;
-      using GraphRegistration<GraphT>::last_aligned_vertex_;
-      using GraphRegistration<GraphT>::last_vertices_;
-
-      using RegistrationPtr = typename Registration<PointT, PointT>::Ptr;
-      using GraphHandlerVertex = typename pcl::registration::GraphHandler<GraphT>::Vertex;
+namespace pcl {
+/** \brief @b PairwiseGraphRegistration class aligns the clouds two by two
+ * \author Nicola Fioraio
+ * \ingroup registration
+ */
+template <typename GraphT, typename PointT>
+class PairwiseGraphRegistration : public GraphRegistration<GraphT> {
+public:
+  using GraphRegistration<GraphT>::graph_handler_;
+  using GraphRegistration<GraphT>::last_aligned_vertex_;
+  using GraphRegistration<GraphT>::last_vertices_;
 
-      /** \brief Empty destructor */
-      virtual ~PairwiseGraphRegistration () {}
+  using RegistrationPtr = typename Registration<PointT, PointT>::Ptr;
+  using GraphHandlerVertex = typename pcl::registration::GraphHandler<GraphT>::Vertex;
 
+  /** \brief Empty destructor */
+  virtual ~PairwiseGraphRegistration() {}
 
-      /** \brief Empty constructor */
-      PairwiseGraphRegistration () : registration_method_ (), incremental_ (true)
-      {}
-      /** \brief Constructor */
-      PairwiseGraphRegistration (const RegistrationPtr& reg, bool incremental) : registration_method_ (reg), incremental_ (incremental)
-      {}
+  /** \brief Empty constructor */
+  PairwiseGraphRegistration() : registration_method_(), incremental_(true) {}
+  /** \brief Constructor */
+  PairwiseGraphRegistration(const RegistrationPtr& reg, bool incremental)
+  : registration_method_(reg), incremental_(incremental)
+  {}
 
-      /** \brief Set the registration object */
-      inline void
-      setRegistrationMethod (const RegistrationPtr& reg)
-      {
-        registration_method_ = reg;
-      }
+  /** \brief Set the registration object */
+  inline void
+  setRegistrationMethod(const RegistrationPtr& reg)
+  {
+    registration_method_ = reg;
+  }
 
-      /** \brief Get the registration object */
-      inline RegistrationPtr
-      getRegistrationMethod ()
-      {
-        return registration_method_;
-      }
+  /** \brief Get the registration object */
+  inline RegistrationPtr
+  getRegistrationMethod()
+  {
+    return registration_method_;
+  }
 
-      /** \brief If True the initial transformation is always set to the Identity */
-      inline void
-      setIncremental (bool incremental)
-      {
-        incremental_ = incremental;
-      }
+  /** \brief If True the initial transformation is always set to the Identity */
+  inline void
+  setIncremental(bool incremental)
+  {
+    incremental_ = incremental;
+  }
 
-      /** \brief Is incremental ? */
-      inline bool
-      isIncremental () const
-      {
-        return incremental_;
-      }
+  /** \brief Is incremental ? */
+  inline bool
+  isIncremental() const
+  {
+    return incremental_;
+  }
 
-    protected:
-      /** \brief The registration object */
-      RegistrationPtr registration_method_;
-      /** \brief If True the initial transformation is always set to the Identity */
-      bool incremental_;
+protected:
+  /** \brief The registration object */
+  RegistrationPtr registration_method_;
+  /** \brief If True the initial transformation is always set to the Identity */
+  bool incremental_;
 
-    private:
-      /** \brief The registration method */
-      virtual void
-      computeRegistration ();
-  };
-}
+private:
+  /** \brief The registration method */
+  virtual void
+  computeRegistration();
+};
+} // namespace pcl
 
 #include <pcl/registration/impl/pairwise_graph_registration.hpp>
diff --git a/registration/include/pcl/registration/ppf_registration.h b/registration/include/pcl/registration/ppf_registration.h
index e05bfcf4..602cce85 100644
--- a/registration/include/pcl/registration/ppf_registration.h
+++ b/registration/include/pcl/registration/ppf_registration.h
@@ -40,259 +40,309 @@
 
 #pragma once
 
-#include <pcl/registration/registration.h>
 #include <pcl/features/ppf.h>
+#include <pcl/registration/registration.h>
 
 #include <unordered_map>
 
-namespace pcl
-{
-  class PCL_EXPORTS PPFHashMapSearch
+namespace pcl {
+class PCL_EXPORTS PPFHashMapSearch {
+public:
+  /** \brief Data structure to hold the information for the key in the feature hash map
+   * of the PPFHashMapSearch class \note It uses multiple pair levels in order to enable
+   * the usage of the boost::hash function which has the std::pair implementation (i.e.,
+   * does not require a custom hash function)
+   */
+  struct HashKeyStruct : public std::pair<int, std::pair<int, std::pair<int, int>>> {
+    HashKeyStruct() = default;
+
+    HashKeyStruct(int a, int b, int c, int d)
+    {
+      this->first = a;
+      this->second.first = b;
+      this->second.second.first = c;
+      this->second.second.second = d;
+    }
+
+    std::size_t
+    operator()(const HashKeyStruct& s) const noexcept
+    {
+      const std::size_t h1 = std::hash<int>{}(s.first);
+      const std::size_t h2 = std::hash<int>{}(s.second.first);
+      const std::size_t h3 = std::hash<int>{}(s.second.second.first);
+      const std::size_t h4 = std::hash<int>{}(s.second.second.second);
+      return h1 ^ (h2 << 1) ^ (h3 << 2) ^ (h4 << 3);
+    }
+  };
+  using FeatureHashMapType =
+      std::unordered_multimap<HashKeyStruct,
+                              std::pair<std::size_t, std::size_t>,
+                              HashKeyStruct>;
+  using FeatureHashMapTypePtr = shared_ptr<FeatureHashMapType>;
+  using Ptr = shared_ptr<PPFHashMapSearch>;
+  using ConstPtr = shared_ptr<const PPFHashMapSearch>;
+
+  /** \brief Constructor for the PPFHashMapSearch class which sets the two step
+   * parameters for the enclosed data structure \param angle_discretization_step the
+   * step value between each bin of the hash map for the angular values \param
+   * distance_discretization_step the step value between each bin of the hash map for
+   * the distance values
+   */
+  PPFHashMapSearch(float angle_discretization_step = 12.0f / 180.0f *
+                                                     static_cast<float>(M_PI),
+                   float distance_discretization_step = 0.01f)
+  : feature_hash_map_(new FeatureHashMapType)
+  , internals_initialized_(false)
+  , angle_discretization_step_(angle_discretization_step)
+  , distance_discretization_step_(distance_discretization_step)
+  , max_dist_(-1.0f)
+  {}
+
+  /** \brief Method that sets the feature cloud to be inserted in the hash map
+   * \param feature_cloud a const smart pointer to the PPFSignature feature cloud
+   */
+  void
+  setInputFeatureCloud(PointCloud<PPFSignature>::ConstPtr feature_cloud);
+
+  /** \brief Function for finding the nearest neighbors for the given feature inside the
+   * discretized hash map \param f1 The 1st value describing the query PPFSignature
+   * feature \param f2 The 2nd value describing the query PPFSignature feature \param f3
+   * The 3rd value describing the query PPFSignature feature \param f4 The 4th value
+   * describing the query PPFSignature feature \param indices a vector of pair indices
+   * representing the feature pairs that have been found in the bin corresponding to the
+   * query feature
+   */
+  void
+  nearestNeighborSearch(float& f1,
+                        float& f2,
+                        float& f3,
+                        float& f4,
+                        std::vector<std::pair<std::size_t, std::size_t>>& indices);
+
+  /** \brief Convenience method for returning a copy of the class instance as a
+   * shared_ptr */
+  Ptr
+  makeShared()
+  {
+    return Ptr(new PPFHashMapSearch(*this));
+  }
+
+  /** \brief Returns the angle discretization step parameter (the step value between
+   * each bin of the hash map for the angular values) */
+  inline float
+  getAngleDiscretizationStep() const
   {
-    public:
-      /** \brief Data structure to hold the information for the key in the feature hash map of the
-        * PPFHashMapSearch class
-        * \note It uses multiple pair levels in order to enable the usage of the boost::hash function
-        * which has the std::pair implementation (i.e., does not require a custom hash function)
-        */
-      struct HashKeyStruct : public std::pair <int, std::pair <int, std::pair <int, int> > >
-      {
-        HashKeyStruct () = default;
-
-        HashKeyStruct(int a, int b, int c, int d)
-        {
-          this->first = a;
-          this->second.first = b;
-          this->second.second.first = c;
-          this->second.second.second = d;
-        }
-
-        std::size_t operator()(const HashKeyStruct& s) const noexcept
-        {
-            const std::size_t h1 = std::hash<int>{} (s.first);
-            const std::size_t h2 = std::hash<int>{} (s.second.first);
-            const std::size_t h3 = std::hash<int>{} (s.second.second.first);
-            const std::size_t h4 = std::hash<int>{} (s.second.second.second);
-            return h1 ^ (h2 << 1) ^ (h3 << 2) ^ (h4 << 3);
-        }
-      };
-      using FeatureHashMapType = std::unordered_multimap<HashKeyStruct, std::pair<std::size_t, std::size_t>, HashKeyStruct>;
-      using FeatureHashMapTypePtr = shared_ptr<FeatureHashMapType>;
-      using Ptr = shared_ptr<PPFHashMapSearch>;
-      using ConstPtr = shared_ptr<const PPFHashMapSearch>;
-
-
-      /** \brief Constructor for the PPFHashMapSearch class which sets the two step parameters for the enclosed data structure
-       * \param angle_discretization_step the step value between each bin of the hash map for the angular values
-       * \param distance_discretization_step the step value between each bin of the hash map for the distance values
-       */
-      PPFHashMapSearch (float angle_discretization_step = 12.0f / 180.0f * static_cast<float> (M_PI),
-                        float distance_discretization_step = 0.01f)
-        : feature_hash_map_ (new FeatureHashMapType)
-        , internals_initialized_ (false)
-        , angle_discretization_step_ (angle_discretization_step)
-        , distance_discretization_step_ (distance_discretization_step)
-        , max_dist_ (-1.0f)
-      {
-      }
-
-      /** \brief Method that sets the feature cloud to be inserted in the hash map
-       * \param feature_cloud a const smart pointer to the PPFSignature feature cloud
-       */
-      void
-      setInputFeatureCloud (PointCloud<PPFSignature>::ConstPtr feature_cloud);
-
-      /** \brief Function for finding the nearest neighbors for the given feature inside the discretized hash map
-       * \param f1 The 1st value describing the query PPFSignature feature
-       * \param f2 The 2nd value describing the query PPFSignature feature
-       * \param f3 The 3rd value describing the query PPFSignature feature
-       * \param f4 The 4th value describing the query PPFSignature feature
-       * \param indices a vector of pair indices representing the feature pairs that have been found in the bin
-       * corresponding to the query feature
-       */
-      void
-      nearestNeighborSearch (float &f1, float &f2, float &f3, float &f4,
-                             std::vector<std::pair<std::size_t, std::size_t> > &indices);
-
-      /** \brief Convenience method for returning a copy of the class instance as a shared_ptr */
-      Ptr
-      makeShared() { return Ptr (new PPFHashMapSearch (*this)); }
-
-      /** \brief Returns the angle discretization step parameter (the step value between each bin of the hash map for the angular values) */
-      inline float
-      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 () const { return distance_discretization_step_; }
-
-      /** \brief Returns the maximum distance found between any feature pair in the given input feature cloud */
-      inline float
-      getModelDiameter () const { return max_dist_; }
-
-      std::vector <std::vector <float> > alpha_m_;
-    private:
-      FeatureHashMapTypePtr feature_hash_map_;
-      bool internals_initialized_;
-
-      float angle_discretization_step_, distance_discretization_step_;
-      float max_dist_;
+    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() const
+  {
+    return distance_discretization_step_;
+  }
+
+  /** \brief Returns the maximum distance found between any feature pair in the given
+   * input feature cloud */
+  inline float
+  getModelDiameter() const
+  {
+    return max_dist_;
+  }
+
+  std::vector<std::vector<float>> alpha_m_;
+
+private:
+  FeatureHashMapTypePtr feature_hash_map_;
+  bool internals_initialized_;
+
+  float angle_discretization_step_, distance_discretization_step_;
+  float max_dist_;
+};
+
+/** \brief Class that registers two point clouds based on their sets of PPFSignatures.
+ * Please refer to the following publication for more details:
+ *    B. Drost, M. Ulrich, N. Navab, S. Ilic
+ *    Model Globally, Match Locally: Efficient and Robust 3D Object Recognition
+ *    2010 IEEE Conference on Computer Vision and Pattern Recognition (CVPR)
+ *    13-18 June 2010, San Francisco, CA
+ *
+ * \note This class works in tandem with the PPFEstimation class
+ *
+ * \author Alexandru-Eugen Ichim
+ */
+template <typename PointSource, typename PointTarget>
+class PPFRegistration : public Registration<PointSource, PointTarget> {
+public:
+  /** \brief Structure for storing a pose (represented as an Eigen::Affine3f) and an
+   * integer for counting votes \note initially used std::pair<Eigen::Affine3f, unsigned
+   * int>, but it proved problematic because of the Eigen structures alignment problems
+   * - std::pair does not have a custom allocator
+   */
+  struct PoseWithVotes {
+    PoseWithVotes(Eigen::Affine3f& a_pose, unsigned int& a_votes)
+    : pose(a_pose), votes(a_votes)
+    {}
+
+    Eigen::Affine3f pose;
+    unsigned int votes;
   };
+  using PoseWithVotesList =
+      std::vector<PoseWithVotes, Eigen::aligned_allocator<PoseWithVotes>>;
+
+  /// input_ is the model cloud
+  using Registration<PointSource, PointTarget>::input_;
+  /// target_ is the scene cloud
+  using Registration<PointSource, PointTarget>::target_;
+  using Registration<PointSource, PointTarget>::converged_;
+  using Registration<PointSource, PointTarget>::final_transformation_;
+  using Registration<PointSource, PointTarget>::transformation_;
+
+  using PointCloudSource = pcl::PointCloud<PointSource>;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+
+  using PointCloudTarget = pcl::PointCloud<PointTarget>;
+  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
+  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
+
+  /** \brief Empty constructor that initializes all the parameters of the algorithm with
+   * default values */
+  PPFRegistration()
+  : Registration<PointSource, PointTarget>()
+  , scene_reference_point_sampling_rate_(5)
+  , clustering_position_diff_threshold_(0.01f)
+  , clustering_rotation_diff_threshold_(20.0f / 180.0f * static_cast<float>(M_PI))
+  {}
+
+  /** \brief Method for setting the position difference clustering parameter
+   * \param clustering_position_diff_threshold distance threshold below which two poses
+   * are considered close enough to be in the same cluster (for the clustering phase of
+   * the algorithm)
+   */
+  inline void
+  setPositionClusteringThreshold(float clustering_position_diff_threshold)
+  {
+    clustering_position_diff_threshold_ = clustering_position_diff_threshold;
+  }
 
-  /** \brief Class that registers two point clouds based on their sets of PPFSignatures.
-   * Please refer to the following publication for more details:
-   *    B. Drost, M. Ulrich, N. Navab, S. Ilic
-   *    Model Globally, Match Locally: Efficient and Robust 3D Object Recognition
-   *    2010 IEEE Conference on Computer Vision and Pattern Recognition (CVPR)
-   *    13-18 June 2010, San Francisco, CA
-   *
-   * \note This class works in tandem with the PPFEstimation class
-   *
-   * \author Alexandru-Eugen Ichim
+  /** \brief Returns the parameter defining the position difference clustering parameter
+   * - distance threshold below which two poses are considered close enough to be in the
+   * same cluster (for the clustering phase of the algorithm)
    */
-  template <typename PointSource, typename PointTarget>
-  class PPFRegistration : public Registration<PointSource, PointTarget>
+  inline float
+  getPositionClusteringThreshold()
   {
-    public:
-      /** \brief Structure for storing a pose (represented as an Eigen::Affine3f) and an integer for counting votes
-        * \note initially used std::pair<Eigen::Affine3f, unsigned int>, but it proved problematic
-        * because of the Eigen structures alignment problems - std::pair does not have a custom allocator
-        */
-      struct PoseWithVotes
-      {
-        PoseWithVotes(Eigen::Affine3f &a_pose, unsigned int &a_votes)
-        : pose (a_pose),
-          votes (a_votes)
-        {}
-
-        Eigen::Affine3f pose;
-        unsigned int votes;
-      };
-      using PoseWithVotesList = std::vector<PoseWithVotes, Eigen::aligned_allocator<PoseWithVotes> >;
-
-      /// input_ is the model cloud
-      using Registration<PointSource, PointTarget>::input_;
-      /// target_ is the scene cloud
-      using Registration<PointSource, PointTarget>::target_;
-      using Registration<PointSource, PointTarget>::converged_;
-      using Registration<PointSource, PointTarget>::final_transformation_;
-      using Registration<PointSource, PointTarget>::transformation_;
-
-      using PointCloudSource = pcl::PointCloud<PointSource>;
-      using PointCloudSourcePtr = typename PointCloudSource::Ptr;
-      using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
-
-      using PointCloudTarget = pcl::PointCloud<PointTarget>;
-      using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
-      using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
-
-
-      /** \brief Empty constructor that initializes all the parameters of the algorithm with default values */
-      PPFRegistration ()
-      :  Registration<PointSource, PointTarget> (),
-         scene_reference_point_sampling_rate_ (5),
-         clustering_position_diff_threshold_ (0.01f),
-         clustering_rotation_diff_threshold_ (20.0f / 180.0f * static_cast<float> (M_PI))
-      {}
-
-      /** \brief Method for setting the position difference clustering parameter
-       * \param clustering_position_diff_threshold distance threshold below which two poses are
-       * considered close enough to be in the same cluster (for the clustering phase of the algorithm)
-       */
-      inline void
-      setPositionClusteringThreshold (float clustering_position_diff_threshold) { clustering_position_diff_threshold_ = clustering_position_diff_threshold; }
-
-      /** \brief Returns the parameter defining the position difference clustering parameter -
-       * distance threshold below which two poses are considered close enough to be in the same cluster
-       * (for the clustering phase of the algorithm)
-       */
-      inline float
-      getPositionClusteringThreshold () { return clustering_position_diff_threshold_; }
-
-      /** \brief Method for setting the rotation clustering parameter
-       * \param clustering_rotation_diff_threshold rotation difference threshold below which two
-       * poses are considered to be in the same cluster (for the clustering phase of the algorithm)
-       */
-      inline void
-      setRotationClusteringThreshold (float clustering_rotation_diff_threshold) { clustering_rotation_diff_threshold_ = clustering_rotation_diff_threshold; }
-
-      /** \brief Returns the parameter defining the rotation clustering threshold
-       */
-      inline float
-      getRotationClusteringThreshold () { return clustering_rotation_diff_threshold_; }
-
-      /** \brief Method for setting the scene reference point sampling rate
-       * \param scene_reference_point_sampling_rate sampling rate for the scene reference point
-       */
-      inline void
-      setSceneReferencePointSamplingRate (unsigned int scene_reference_point_sampling_rate) { scene_reference_point_sampling_rate_ = scene_reference_point_sampling_rate; }
-
-      /** \brief Returns the parameter for the scene reference point sampling rate of the algorithm */
-      inline unsigned int
-      getSceneReferencePointSamplingRate () { return scene_reference_point_sampling_rate_; }
-
-      /** \brief Function that sets the search method for the algorithm
-       * \note Right now, the only available method is the one initially proposed by
-       * the authors - by using a hash map with discretized feature vectors
-       * \param search_method smart pointer to the search method to be set
-       */
-      inline void
-      setSearchMethod (PPFHashMapSearch::Ptr search_method) { search_method_ = search_method; }
-
-      /** \brief Getter function for the search method of the class */
-      inline PPFHashMapSearch::Ptr
-      getSearchMethod () { return search_method_; }
-
-      /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to)
-       * \param cloud the input point cloud target
-       */
-      void
-      setInputTarget (const PointCloudTargetConstPtr &cloud) override;
-
-
-    private:
-      /** \brief Method that calculates the transformation between the input_ and target_ point clouds, based on the PPF features */
-      void
-      computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) override;
-
-
-      /** \brief the search method that is going to be used to find matching feature pairs */
-      PPFHashMapSearch::Ptr search_method_;
-
-      /** \brief parameter for the sampling rate of the scene reference points */
-      unsigned int scene_reference_point_sampling_rate_;
-
-      /** \brief position and rotation difference thresholds below which two
-        * poses are considered to be in the same cluster (for the clustering phase of the algorithm) */
-      float clustering_position_diff_threshold_, clustering_rotation_diff_threshold_;
-
-      /** \brief use a kd-tree with range searches of range max_dist to skip an O(N) pass through the point cloud */
-      typename pcl::KdTreeFLANN<PointTarget>::Ptr scene_search_tree_;
-
-      /** \brief static method used for the std::sort function to order two PoseWithVotes
-       * instances by their number of votes*/
-      static bool
-      poseWithVotesCompareFunction (const PoseWithVotes &a,
-                                    const PoseWithVotes &b);
-
-      /** \brief static method used for the std::sort function to order two pairs <index, votes>
-       * by the number of votes (unsigned integer value) */
-      static bool
-      clusterVotesCompareFunction (const std::pair<std::size_t, unsigned int> &a,
-                                   const std::pair<std::size_t, unsigned int> &b);
-
-      /** \brief Method that clusters a set of given poses by using the clustering thresholds
-       * and their corresponding number of votes (see publication for more details) */
-      void
-      clusterPoses (PoseWithVotesList &poses,
-                    PoseWithVotesList &result);
-
-      /** \brief Method that checks whether two poses are close together - based on the clustering threshold parameters
-       * of the class */
-      bool
-      posesWithinErrorBounds (Eigen::Affine3f &pose1,
-                              Eigen::Affine3f &pose2);
-  };
-}
+    return clustering_position_diff_threshold_;
+  }
+
+  /** \brief Method for setting the rotation clustering parameter
+   * \param clustering_rotation_diff_threshold rotation difference threshold below which
+   * two poses are considered to be in the same cluster (for the clustering phase of the
+   * algorithm)
+   */
+  inline void
+  setRotationClusteringThreshold(float clustering_rotation_diff_threshold)
+  {
+    clustering_rotation_diff_threshold_ = clustering_rotation_diff_threshold;
+  }
+
+  /** \brief Returns the parameter defining the rotation clustering threshold
+   */
+  inline float
+  getRotationClusteringThreshold()
+  {
+    return clustering_rotation_diff_threshold_;
+  }
+
+  /** \brief Method for setting the scene reference point sampling rate
+   * \param scene_reference_point_sampling_rate sampling rate for the scene reference
+   * point
+   */
+  inline void
+  setSceneReferencePointSamplingRate(unsigned int scene_reference_point_sampling_rate)
+  {
+    scene_reference_point_sampling_rate_ = scene_reference_point_sampling_rate;
+  }
+
+  /** \brief Returns the parameter for the scene reference point sampling rate of the
+   * algorithm */
+  inline unsigned int
+  getSceneReferencePointSamplingRate()
+  {
+    return scene_reference_point_sampling_rate_;
+  }
+
+  /** \brief Function that sets the search method for the algorithm
+   * \note Right now, the only available method is the one initially proposed by
+   * the authors - by using a hash map with discretized feature vectors
+   * \param search_method smart pointer to the search method to be set
+   */
+  inline void
+  setSearchMethod(PPFHashMapSearch::Ptr search_method)
+  {
+    search_method_ = search_method;
+  }
+
+  /** \brief Getter function for the search method of the class */
+  inline PPFHashMapSearch::Ptr
+  getSearchMethod()
+  {
+    return search_method_;
+  }
+
+  /** \brief Provide a pointer to the input target (e.g., the point cloud that we want
+   * to align the input source to) \param cloud the input point cloud target
+   */
+  void
+  setInputTarget(const PointCloudTargetConstPtr& cloud) override;
+
+private:
+  /** \brief Method that calculates the transformation between the input_ and target_
+   * point clouds, based on the PPF features */
+  void
+  computeTransformation(PointCloudSource& output,
+                        const Eigen::Matrix4f& guess) override;
+
+  /** \brief the search method that is going to be used to find matching feature pairs
+   */
+  PPFHashMapSearch::Ptr search_method_;
+
+  /** \brief parameter for the sampling rate of the scene reference points */
+  uindex_t scene_reference_point_sampling_rate_;
+
+  /** \brief position and rotation difference thresholds below which two
+   * poses are considered to be in the same cluster (for the clustering phase of the
+   * algorithm) */
+  float clustering_position_diff_threshold_, clustering_rotation_diff_threshold_;
+
+  /** \brief use a kd-tree with range searches of range max_dist to skip an O(N) pass
+   * through the point cloud */
+  typename pcl::KdTreeFLANN<PointTarget>::Ptr scene_search_tree_;
+
+  /** \brief static method used for the std::sort function to order two PoseWithVotes
+   * instances by their number of votes*/
+  static bool
+  poseWithVotesCompareFunction(const PoseWithVotes& a, const PoseWithVotes& b);
+
+  /** \brief static method used for the std::sort function to order two pairs <index,
+   * votes> by the number of votes (unsigned integer value) */
+  static bool
+  clusterVotesCompareFunction(const std::pair<std::size_t, unsigned int>& a,
+                              const std::pair<std::size_t, unsigned int>& b);
+
+  /** \brief Method that clusters a set of given poses by using the clustering
+   * thresholds and their corresponding number of votes (see publication for more
+   * details) */
+  void
+  clusterPoses(PoseWithVotesList& poses, PoseWithVotesList& result);
+
+  /** \brief Method that checks whether two poses are close together - based on the
+   * clustering threshold parameters of the class */
+  bool
+  posesWithinErrorBounds(Eigen::Affine3f& pose1, Eigen::Affine3f& pose2);
+};
+} // namespace pcl
 
 #include <pcl/registration/impl/ppf_registration.hpp>
diff --git a/registration/include/pcl/registration/pyramid_feature_matching.h b/registration/include/pcl/registration/pyramid_feature_matching.h
index 92dbc354..78e3d3a1 100644
--- a/registration/include/pcl/registration/pyramid_feature_matching.h
+++ b/registration/include/pcl/registration/pyramid_feature_matching.h
@@ -43,153 +43,174 @@
 #include <pcl/pcl_base.h>
 #include <pcl/point_representation.h>
 
-namespace pcl
-{
-  /**
-   * \brief Class that compares two sets of features by using a multiscale representation of the features inside a
-   * pyramid. Each level of the pyramid offers information about the similarity of the two feature sets.
-   * \note Works with any Point/Feature type which has a PointRepresentation implementation
-   * \note The only parameters it needs are the input dimension ranges and the output dimension ranges. The input
-   * dimension ranges represent the ranges in which each dimension of the feature vector lies. As described in the
-   * paper, a minimum inter-vector distance of sqrt(nr_dims)/2 is needed. As such, the target dimension range parameter
-   * is used in order to augment/reduce the range for each dimension in order to obtain the necessary minimal
-   * inter-vector distance and to add/subtract weight to/from certain dimensions of the feature vector.
-   *
-   * Follows the algorithm presented in the publication:
-   *    Grauman, K. & Darrell, T.
-   *    The Pyramid Match Kernel: Discriminative Classification with Sets of Image Features
-   *    Tenth IEEE International Conference on Computer Vision ICCV05 Volume 1
-   *    October 2005
-   *
-   * \author Alexandru-Eugen Ichim
+namespace pcl {
+/**
+ * \brief Class that compares two sets of features by using a multiscale representation
+ * of the features inside a pyramid. Each level of the pyramid offers information about
+ * the similarity of the two feature sets. \note Works with any Point/Feature type which
+ * has a PointRepresentation implementation \note The only parameters it needs are the
+ * input dimension ranges and the output dimension ranges. The input dimension ranges
+ * represent the ranges in which each dimension of the feature vector lies. As described
+ * in the paper, a minimum inter-vector distance of sqrt(nr_dims)/2 is needed. As such,
+ * the target dimension range parameter is used in order to augment/reduce the range for
+ * each dimension in order to obtain the necessary minimal inter-vector distance and to
+ * add/subtract weight to/from certain dimensions of the feature vector.
+ *
+ * Follows the algorithm presented in the publication:
+ *    Grauman, K. & Darrell, T.
+ *    The Pyramid Match Kernel: Discriminative Classification with Sets of Image
+ * Features Tenth IEEE International Conference on Computer Vision ICCV05 Volume 1
+ *    October 2005
+ *
+ * \author Alexandru-Eugen Ichim
+ */
+template <typename PointFeature>
+class PyramidFeatureHistogram : public PCLBase<PointFeature> {
+public:
+  using PCLBase<PointFeature>::input_;
+
+  using Ptr = shared_ptr<PyramidFeatureHistogram<PointFeature>>;
+  using ConstPtr = shared_ptr<const PyramidFeatureHistogram<PointFeature>>;
+  using PyramidFeatureHistogramPtr = Ptr;
+  using FeatureRepresentationConstPtr =
+      shared_ptr<const pcl::PointRepresentation<PointFeature>>;
+
+  /** \brief Empty constructor that instantiates the feature representation variable */
+  PyramidFeatureHistogram();
+
+  /** \brief Method for setting the input dimension range parameter.
+   * \note Please check the PyramidHistogram class description for more details about
+   * this parameter.
+   */
+  inline void
+  setInputDimensionRange(std::vector<std::pair<float, float>>& dimension_range_input)
+  {
+    dimension_range_input_ = dimension_range_input;
+  }
+
+  /** \brief Method for retrieving the input dimension range vector */
+  inline std::vector<std::pair<float, float>>
+  getInputDimensionRange()
+  {
+    return dimension_range_input_;
+  }
+
+  /** \brief Method to set the target dimension range parameter.
+   * \note Please check the PyramidHistogram class description for more details about
+   * this parameter.
+   */
+  inline void
+  setTargetDimensionRange(std::vector<std::pair<float, float>>& dimension_range_target)
+  {
+    dimension_range_target_ = dimension_range_target;
+  }
+
+  /** \brief Method for retrieving the target dimension range vector */
+  inline std::vector<std::pair<float, float>>
+  getTargetDimensionRange()
+  {
+    return dimension_range_target_;
+  }
+
+  /** \brief Provide a pointer to the feature representation to use to convert features
+   * to k-D vectors. \param feature_representation the const boost shared pointer to a
+   * PointRepresentation
    */
-  template <typename PointFeature>
-  class PyramidFeatureHistogram : public PCLBase<PointFeature>
+  inline void
+  setPointRepresentation(const FeatureRepresentationConstPtr& feature_representation)
   {
-    public:
-      using PCLBase<PointFeature>::input_;
-
-      using Ptr = shared_ptr<PyramidFeatureHistogram<PointFeature> >;
-      using ConstPtr = shared_ptr<const PyramidFeatureHistogram<PointFeature>>;
-      using PyramidFeatureHistogramPtr = Ptr;
-      using FeatureRepresentationConstPtr = shared_ptr<const pcl::PointRepresentation<PointFeature> >;
-
-
-      /** \brief Empty constructor that instantiates the feature representation variable */
-      PyramidFeatureHistogram ();
-
-      /** \brief Method for setting the input dimension range parameter.
-       * \note Please check the PyramidHistogram class description for more details about this parameter.
-       */
-      inline void
-      setInputDimensionRange (std::vector<std::pair<float, float> > &dimension_range_input)
-      { dimension_range_input_ = dimension_range_input; }
-
-      /** \brief Method for retrieving the input dimension range vector */
-      inline std::vector<std::pair<float, float> >
-      getInputDimensionRange () { return dimension_range_input_; }
-
-      /** \brief Method to set the target dimension range parameter.
-       * \note Please check the PyramidHistogram class description for more details about this parameter.
-       */
-      inline void
-      setTargetDimensionRange (std::vector<std::pair<float, float> > &dimension_range_target)
-      { dimension_range_target_ = dimension_range_target; }
-
-      /** \brief Method for retrieving the target dimension range vector */
-      inline std::vector<std::pair<float, float> >
-      getTargetDimensionRange () { return dimension_range_target_; }
-
-      /** \brief Provide a pointer to the feature representation to use to convert features to k-D vectors.
-       * \param feature_representation the const boost shared pointer to a PointRepresentation
-       */
-      inline void
-      setPointRepresentation (const FeatureRepresentationConstPtr& feature_representation) { feature_representation_ = feature_representation; }
-
-      /** \brief Get a pointer to the feature representation used when converting features into k-D vectors. */
-      inline FeatureRepresentationConstPtr const
-      getPointRepresentation () { return feature_representation_; }
-
-      /** \brief The central method for inserting the feature set inside the pyramid and obtaining the complete pyramid */
-      void
-      compute ();
-
-      /** \brief Checks whether the pyramid histogram has been computed */
-      inline bool
-      isComputed () { return is_computed_; }
-
-      /** \brief Static method for comparing two pyramid histograms that returns a floating point value between 0 and 1,
-       * representing the similarity between the feature sets on which the two pyramid histograms are based.
-       * \param pyramid_a Pointer to the first pyramid to be compared (needs to be computed already).
-       * \param pyramid_b Pointer to the second pyramid to be compared (needs to be computed already).
-       */
-      static float
-      comparePyramidFeatureHistograms (const PyramidFeatureHistogramPtr &pyramid_a,
-                                       const PyramidFeatureHistogramPtr &pyramid_b);
-
-
-    private:
-      std::size_t nr_dimensions, nr_levels, nr_features;
-      std::vector<std::pair<float, float> > dimension_range_input_, dimension_range_target_;
-      FeatureRepresentationConstPtr feature_representation_;
-      bool is_computed_;
-
-      /** \brief Checks for input inconsistencies and initializes the underlying data structures */
-      bool
-      initializeHistogram ();
-
-      /** \brief Converts a feature in templated form to an STL vector. This is the point where the conversion from the
-       * input dimension range to the target dimension range is done.
-       */
-      void
-      convertFeatureToVector (const PointFeature &feature,
-                              std::vector<float> &feature_vector);
-
-      /** \brief Adds a feature vector to its corresponding bin at each level in the pyramid */
-      void
-      addFeature (std::vector<float> &feature);
-
-      /** \brief Access the pyramid bin given the position of the bin at the given pyramid level
-       * and the pyramid level
-       * \param access index of the bin at the respective level
-       * \param level the level in the pyramid
-       */
-      inline unsigned int&
-      at (std::vector<std::size_t> &access,
-          std::size_t &level);
-
-      /** \brief Access the pyramid bin given a feature vector and the pyramid level
-       * \param feature the feature in vectorized form
-       * \param level the level in the pyramid
-       */
-      inline unsigned int&
-      at (std::vector<float> &feature,
-          std::size_t &level);
-
-      /** \brief Structure for representing a single pyramid histogram level */
-      struct PyramidFeatureHistogramLevel
-      {
-        PyramidFeatureHistogramLevel () 
-        {
-        }
-
-        PyramidFeatureHistogramLevel (std::vector<std::size_t> &a_bins_per_dimension, std::vector<float> &a_bin_step) : 
-          bins_per_dimension (a_bins_per_dimension),
-          bin_step (a_bin_step)
-        {
-          initializeHistogramLevel ();
-        }
-
-        void
-        initializeHistogramLevel ();
-
-        std::vector<unsigned int> hist;
-        std::vector<std::size_t> bins_per_dimension;
-        std::vector<float> bin_step;
-      };
-      std::vector<PyramidFeatureHistogramLevel> hist_levels;
+    feature_representation_ = feature_representation;
+  }
+
+  /** \brief Get a pointer to the feature representation used when converting features
+   * into k-D vectors. */
+  inline FeatureRepresentationConstPtr const
+  getPointRepresentation()
+  {
+    return feature_representation_;
+  }
+
+  /** \brief The central method for inserting the feature set inside the pyramid and
+   * obtaining the complete pyramid */
+  void
+  compute();
+
+  /** \brief Checks whether the pyramid histogram has been computed */
+  inline bool
+  isComputed()
+  {
+    return is_computed_;
+  }
+
+  /** \brief Static method for comparing two pyramid histograms that returns a floating
+   * point value between 0 and 1, representing the similarity between the feature sets
+   * on which the two pyramid histograms are based. \param pyramid_a Pointer to the
+   * first pyramid to be compared (needs to be computed already). \param pyramid_b
+   * Pointer to the second pyramid to be compared (needs to be computed already).
+   */
+  static float
+  comparePyramidFeatureHistograms(const PyramidFeatureHistogramPtr& pyramid_a,
+                                  const PyramidFeatureHistogramPtr& pyramid_b);
+
+private:
+  std::size_t nr_dimensions, nr_levels, nr_features;
+  std::vector<std::pair<float, float>> dimension_range_input_, dimension_range_target_;
+  FeatureRepresentationConstPtr feature_representation_;
+  bool is_computed_;
+
+  /** \brief Checks for input inconsistencies and initializes the underlying data
+   * structures */
+  bool
+  initializeHistogram();
+
+  /** \brief Converts a feature in templated form to an STL vector. This is the point
+   * where the conversion from the input dimension range to the target dimension range
+   * is done.
+   */
+  void
+  convertFeatureToVector(const PointFeature& feature,
+                         std::vector<float>& feature_vector);
+
+  /** \brief Adds a feature vector to its corresponding bin at each level in the pyramid
+   */
+  void
+  addFeature(std::vector<float>& feature);
+
+  /** \brief Access the pyramid bin given the position of the bin at the given pyramid
+   * level and the pyramid level \param access index of the bin at the respective level
+   * \param level the level in the pyramid
+   */
+  inline unsigned int&
+  at(std::vector<std::size_t>& access, std::size_t& level);
+
+  /** \brief Access the pyramid bin given a feature vector and the pyramid level
+   * \param feature the feature in vectorized form
+   * \param level the level in the pyramid
+   */
+  inline unsigned int&
+  at(std::vector<float>& feature, std::size_t& level);
+
+  /** \brief Structure for representing a single pyramid histogram level */
+  struct PyramidFeatureHistogramLevel {
+    PyramidFeatureHistogramLevel() {}
+
+    PyramidFeatureHistogramLevel(std::vector<std::size_t>& a_bins_per_dimension,
+                                 std::vector<float>& a_bin_step)
+    : bins_per_dimension(a_bins_per_dimension), bin_step(a_bin_step)
+    {
+      initializeHistogramLevel();
+    }
+
+    void
+    initializeHistogramLevel();
+
+    std::vector<unsigned int> hist;
+    std::vector<std::size_t> bins_per_dimension;
+    std::vector<float> bin_step;
   };
-}
+  std::vector<PyramidFeatureHistogramLevel> hist_levels;
+};
+} // namespace pcl
 
 #ifdef PCL_NO_PRECOMPILE
 #include <pcl/registration/impl/pyramid_feature_matching.hpp>
diff --git a/registration/include/pcl/registration/registration.h b/registration/include/pcl/registration/registration.h
index ab77ded7..dc3a1193 100644
--- a/registration/include/pcl/registration/registration.h
+++ b/registration/include/pcl/registration/registration.h
@@ -41,590 +41,670 @@
 #pragma once
 
 // PCL includes
-#include <pcl/pcl_base.h>
-#include <pcl/common/transforms.h>
-#include <pcl/memory.h>
-#include <pcl/pcl_macros.h>
-#include <pcl/search/kdtree.h>
-#include <pcl/registration/boost.h>
-#include <pcl/registration/transformation_estimation.h>
 #include <pcl/registration/correspondence_estimation.h>
 #include <pcl/registration/correspondence_rejection.h>
+#include <pcl/registration/transformation_estimation.h>
+#include <pcl/search/kdtree.h>
+#include <pcl/memory.h>
+#include <pcl/pcl_base.h>
+#include <pcl/pcl_macros.h>
+
+namespace pcl {
+/** \brief @b Registration represents the base registration class for general purpose,
+ * ICP-like methods. \author Radu B. Rusu, Michael Dixon \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename Scalar = float>
+class Registration : public PCLBase<PointSource> {
+public:
+  using Matrix4 = Eigen::Matrix<Scalar, 4, 4>;
+
+  // using PCLBase<PointSource>::initCompute;
+  using PCLBase<PointSource>::deinitCompute;
+  using PCLBase<PointSource>::input_;
+  using PCLBase<PointSource>::indices_;
+
+  using Ptr = shared_ptr<Registration<PointSource, PointTarget, Scalar>>;
+  using ConstPtr = shared_ptr<const Registration<PointSource, PointTarget, Scalar>>;
+
+  using CorrespondenceRejectorPtr = pcl::registration::CorrespondenceRejector::Ptr;
+  using KdTree = pcl::search::KdTree<PointTarget>;
+  using KdTreePtr = typename KdTree::Ptr;
+
+  using KdTreeReciprocal = pcl::search::KdTree<PointSource>;
+  using KdTreeReciprocalPtr = typename KdTreeReciprocal::Ptr;
+
+  using PointCloudSource = pcl::PointCloud<PointSource>;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+
+  using PointCloudTarget = pcl::PointCloud<PointTarget>;
+  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
+  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
+
+  using PointRepresentationConstPtr = typename KdTree::PointRepresentationConstPtr;
+
+  using TransformationEstimation = typename pcl::registration::
+      TransformationEstimation<PointSource, PointTarget, Scalar>;
+  using TransformationEstimationPtr = typename TransformationEstimation::Ptr;
+  using TransformationEstimationConstPtr = typename TransformationEstimation::ConstPtr;
+
+  using CorrespondenceEstimation =
+      pcl::registration::CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>;
+  using CorrespondenceEstimationPtr = typename CorrespondenceEstimation::Ptr;
+  using CorrespondenceEstimationConstPtr = typename CorrespondenceEstimation::ConstPtr;
+
+  /** \brief The callback signature to the function updating intermediate source point
+   * cloud position during it's registration to the target point cloud. \param[in]
+   * cloud_src - the point cloud which will be updated to match target \param[in]
+   * indices_src - a selector of points in cloud_src \param[in] cloud_tgt - the point
+   * cloud we want to register against \param[in] indices_tgt - a selector of points in
+   * cloud_tgt
+   */
+  using UpdateVisualizerCallbackSignature = void(const pcl::PointCloud<PointSource>&,
+                                                 const pcl::Indices&,
+                                                 const pcl::PointCloud<PointTarget>&,
+                                                 const pcl::Indices&);
+
+  /** \brief Empty constructor. */
+  Registration()
+  : tree_(new KdTree)
+  , tree_reciprocal_(new KdTreeReciprocal)
+  , nr_iterations_(0)
+  , max_iterations_(10)
+  , ransac_iterations_(0)
+  , target_()
+  , final_transformation_(Matrix4::Identity())
+  , transformation_(Matrix4::Identity())
+  , previous_transformation_(Matrix4::Identity())
+  , transformation_epsilon_(0.0)
+  , transformation_rotation_epsilon_(0.0)
+  , euclidean_fitness_epsilon_(-std::numeric_limits<double>::max())
+  , corr_dist_threshold_(std::sqrt(std::numeric_limits<double>::max()))
+  , inlier_threshold_(0.05)
+  , converged_(false)
+  , min_number_correspondences_(3)
+  , correspondences_(new Correspondences)
+  , transformation_estimation_()
+  , correspondence_estimation_()
+  , target_cloud_updated_(true)
+  , source_cloud_updated_(true)
+  , force_no_recompute_(false)
+  , force_no_recompute_reciprocal_(false)
+  , point_representation_()
+  {}
+
+  /** \brief destructor. */
+  ~Registration() {}
+
+  /** \brief Provide a pointer to the transformation estimation object.
+   * (e.g., SVD, point to plane etc.)
+   *
+   * \param[in] te is the pointer to the corresponding transformation estimation object
+   *
+   * Code example:
+   *
+   * \code
+   * TransformationEstimationPointToPlaneLLS<PointXYZ, PointXYZ>::Ptr trans_lls
+   *   (new TransformationEstimationPointToPlaneLLS<PointXYZ, PointXYZ>);
+   * icp.setTransformationEstimation (trans_lls);
+   * // or...
+   * TransformationEstimationSVD<PointXYZ, PointXYZ>::Ptr trans_svd
+   *   (new TransformationEstimationSVD<PointXYZ, PointXYZ>);
+   * icp.setTransformationEstimation (trans_svd);
+   * \endcode
+   */
+  void
+  setTransformationEstimation(const TransformationEstimationPtr& te)
+  {
+    transformation_estimation_ = te;
+  }
+
+  /** \brief Provide a pointer to the correspondence estimation object.
+   * (e.g., regular, reciprocal, normal shooting etc.)
+   *
+   * \param[in] ce is the pointer to the corresponding correspondence estimation object
+   *
+   * Code example:
+   *
+   * \code
+   * CorrespondenceEstimation<PointXYZ, PointXYZ>::Ptr
+   *   ce (new CorrespondenceEstimation<PointXYZ, PointXYZ>);
+   * ce->setInputSource (source);
+   * ce->setInputTarget (target);
+   * icp.setCorrespondenceEstimation (ce);
+   * // or...
+   * CorrespondenceEstimationNormalShooting<PointNormal, PointNormal, PointNormal>::Ptr
+   *   cens (new CorrespondenceEstimationNormalShooting<PointNormal, PointNormal>);
+   * ce->setInputSource (source);
+   * ce->setInputTarget (target);
+   * ce->setSourceNormals (source);
+   * ce->setTargetNormals (target);
+   * icp.setCorrespondenceEstimation (cens);
+   * \endcode
+   */
+  void
+  setCorrespondenceEstimation(const CorrespondenceEstimationPtr& ce)
+  {
+    correspondence_estimation_ = ce;
+  }
+
+  /** \brief Provide a pointer to the input source
+   * (e.g., the point cloud that we want to align to the target)
+   *
+   * \param[in] cloud the input point cloud source
+   */
+  virtual void
+  setInputSource(const PointCloudSourceConstPtr& cloud);
+
+  /** \brief Get a pointer to the input point cloud dataset target. */
+  inline PointCloudSourceConstPtr const
+  getInputSource()
+  {
+    return (input_);
+  }
+
+  /** \brief Provide a pointer to the input target (e.g., the point cloud that we want
+   * to align the input source to) \param[in] cloud the input point cloud target
+   */
+  virtual inline void
+  setInputTarget(const PointCloudTargetConstPtr& cloud);
+
+  /** \brief Get a pointer to the input point cloud dataset target. */
+  inline PointCloudTargetConstPtr const
+  getInputTarget()
+  {
+    return (target_);
+  }
+
+  /** \brief Provide a pointer to the search object used to find correspondences in
+   * the target cloud.
+   * \param[in] tree a pointer to the spatial search object.
+   * \param[in] force_no_recompute If set to true, this tree will NEVER be
+   * recomputed, regardless of calls to setInputTarget. Only use if you are
+   * confident that the tree will be set correctly.
+   */
+  inline void
+  setSearchMethodTarget(const KdTreePtr& tree, bool force_no_recompute = false)
+  {
+    tree_ = tree;
+    force_no_recompute_ = force_no_recompute;
+    // Since we just set a new tree, we need to check for updates
+    target_cloud_updated_ = true;
+  }
+
+  /** \brief Get a pointer to the search method used to find correspondences in the
+   * target cloud. */
+  inline KdTreePtr
+  getSearchMethodTarget() const
+  {
+    return (tree_);
+  }
+
+  /** \brief Provide a pointer to the search object used to find correspondences in
+   * the source cloud (usually used by reciprocal correspondence finding).
+   * \param[in] tree a pointer to the spatial search object.
+   * \param[in] force_no_recompute If set to true, this tree will NEVER be
+   * recomputed, regardless of calls to setInputSource. Only use if you are
+   * extremely confident that the tree will be set correctly.
+   */
+  inline void
+  setSearchMethodSource(const KdTreeReciprocalPtr& tree,
+                        bool force_no_recompute = false)
+  {
+    tree_reciprocal_ = tree;
+    force_no_recompute_reciprocal_ = force_no_recompute;
+    // Since we just set a new tree, we need to check for updates
+    source_cloud_updated_ = true;
+  }
+
+  /** \brief Get a pointer to the search method used to find correspondences in the
+   * source cloud. */
+  inline KdTreeReciprocalPtr
+  getSearchMethodSource() const
+  {
+    return (tree_reciprocal_);
+  }
 
-namespace pcl
-{
-  /** \brief @b Registration represents the base registration class for general purpose, ICP-like methods.
-    * \author Radu B. Rusu, Michael Dixon
-    * \ingroup registration
-    */
-  template <typename PointSource, typename PointTarget, typename Scalar = float>
-  class Registration : public PCLBase<PointSource>
-  {
-    public:
-      using Matrix4 = Eigen::Matrix<Scalar, 4, 4>;
-
-      // using PCLBase<PointSource>::initCompute;
-      using PCLBase<PointSource>::deinitCompute;
-      using PCLBase<PointSource>::input_;
-      using PCLBase<PointSource>::indices_;
-
-      using Ptr = shared_ptr< Registration<PointSource, PointTarget, Scalar> >;
-      using ConstPtr = shared_ptr< const Registration<PointSource, PointTarget, Scalar> >;
-
-      using CorrespondenceRejectorPtr = pcl::registration::CorrespondenceRejector::Ptr;
-      using KdTree = pcl::search::KdTree<PointTarget>;
-      using KdTreePtr = typename KdTree::Ptr;
-
-      using KdTreeReciprocal = pcl::search::KdTree<PointSource>;
-      using KdTreeReciprocalPtr = typename KdTreeReciprocal::Ptr;
-     
-      using PointCloudSource = pcl::PointCloud<PointSource>;
-      using PointCloudSourcePtr = typename PointCloudSource::Ptr;
-      using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
-
-      using PointCloudTarget = pcl::PointCloud<PointTarget>;
-      using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
-      using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
-
-      using PointRepresentationConstPtr = typename KdTree::PointRepresentationConstPtr;
-      
-      using TransformationEstimation = typename pcl::registration::TransformationEstimation<PointSource, PointTarget, Scalar>;
-      using TransformationEstimationPtr = typename TransformationEstimation::Ptr;
-      using TransformationEstimationConstPtr = typename TransformationEstimation::ConstPtr;
-
-      using CorrespondenceEstimation = pcl::registration::CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>;
-      using CorrespondenceEstimationPtr = typename CorrespondenceEstimation::Ptr;
-      using CorrespondenceEstimationConstPtr = typename CorrespondenceEstimation::ConstPtr;
-
-      /** \brief The callback signature to the function updating intermediate source point cloud position
-        * during it's registration to the target point cloud.
-        * \param[in] cloud_src - the point cloud which will be updated to match target
-        * \param[in] indices_src - a selector of points in cloud_src
-        * \param[in] cloud_tgt - the point cloud we want to register against
-        * \param[in] indices_tgt - a selector of points in cloud_tgt
-        */
-      using UpdateVisualizerCallbackSignature = void (const pcl::PointCloud<PointSource>&,
-                                                      const std::vector<int>&,
-                                                      const pcl::PointCloud<PointTarget>&,
-                                                      const std::vector<int>&);
-
-      /** \brief Empty constructor. */
-      Registration () 
-        : tree_ (new KdTree)
-        , tree_reciprocal_ (new KdTreeReciprocal)
-        , nr_iterations_ (0)
-        , max_iterations_ (10)
-        , ransac_iterations_ (0)
-        , target_ ()
-        , final_transformation_ (Matrix4::Identity ())
-        , transformation_ (Matrix4::Identity ())
-        , previous_transformation_ (Matrix4::Identity ())
-        , transformation_epsilon_ (0.0)
-        , transformation_rotation_epsilon_(0.0)
-        , euclidean_fitness_epsilon_ (-std::numeric_limits<double>::max ())
-        , corr_dist_threshold_ (std::sqrt (std::numeric_limits<double>::max ()))
-        , inlier_threshold_ (0.05)
-        , converged_ (false)
-        , min_number_correspondences_ (3)
-        , correspondences_ (new Correspondences)
-        , transformation_estimation_ ()
-        , correspondence_estimation_ ()
-        , target_cloud_updated_ (true)
-        , source_cloud_updated_ (true)
-        , force_no_recompute_ (false)
-        , force_no_recompute_reciprocal_ (false)
-        , point_representation_ ()
-      {
-      }
-
-      /** \brief destructor. */
-      ~Registration () {}
-
-      /** \brief Provide a pointer to the transformation estimation object.
-        * (e.g., SVD, point to plane etc.) 
-        * 
-        * \param[in] te is the pointer to the corresponding transformation estimation object
-        *
-        * Code example:
-        *
-        * \code
-        * TransformationEstimationPointToPlaneLLS<PointXYZ, PointXYZ>::Ptr trans_lls (new TransformationEstimationPointToPlaneLLS<PointXYZ, PointXYZ>);
-        * icp.setTransformationEstimation (trans_lls);
-        * // or...
-        * TransformationEstimationSVD<PointXYZ, PointXYZ>::Ptr trans_svd (new TransformationEstimationSVD<PointXYZ, PointXYZ>);
-        * icp.setTransformationEstimation (trans_svd);
-        * \endcode
-        */
-      void
-      setTransformationEstimation (const TransformationEstimationPtr &te) { transformation_estimation_ = te; }
-
-      /** \brief Provide a pointer to the correspondence estimation object.
-        * (e.g., regular, reciprocal, normal shooting etc.) 
-        * 
-        * \param[in] ce is the pointer to the corresponding correspondence estimation object
-        *
-        * Code example:
-        *
-        * \code
-        * CorrespondenceEstimation<PointXYZ, PointXYZ>::Ptr ce (new CorrespondenceEstimation<PointXYZ, PointXYZ>);
-        * ce->setInputSource (source);
-        * ce->setInputTarget (target);
-        * icp.setCorrespondenceEstimation (ce);
-        * // or...
-        * CorrespondenceEstimationNormalShooting<PointNormal, PointNormal, PointNormal>::Ptr cens (new CorrespondenceEstimationNormalShooting<PointNormal, PointNormal>);
-        * ce->setInputSource (source);
-        * ce->setInputTarget (target);
-        * ce->setSourceNormals (source);
-        * ce->setTargetNormals (target);
-        * icp.setCorrespondenceEstimation (cens);
-        * \endcode
-        */
-      void
-      setCorrespondenceEstimation (const CorrespondenceEstimationPtr &ce) { correspondence_estimation_ = ce; }
-
-      /** \brief Provide a pointer to the input source 
-        * (e.g., the point cloud that we want to align to the target)
-        *
-        * \param[in] cloud the input point cloud source
-        */
-      virtual void
-      setInputSource (const PointCloudSourceConstPtr &cloud)
-      {
-        source_cloud_updated_ = true;
-        PCLBase<PointSource>::setInputCloud (cloud);
-      }
-
-      /** \brief Get a pointer to the input point cloud dataset target. */
-      inline PointCloudSourceConstPtr const
-      getInputSource () { return (input_ ); }
-
-      /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to)
-        * \param[in] cloud the input point cloud target
-        */
-      virtual inline void 
-      setInputTarget (const PointCloudTargetConstPtr &cloud); 
-
-      /** \brief Get a pointer to the input point cloud dataset target. */
-      inline PointCloudTargetConstPtr const 
-      getInputTarget () { return (target_ ); }
-
-
-      /** \brief Provide a pointer to the search object used to find correspondences in
-        * the target cloud.
-        * \param[in] tree a pointer to the spatial search object.
-        * \param[in] force_no_recompute If set to true, this tree will NEVER be 
-        * recomputed, regardless of calls to setInputTarget. Only use if you are 
-        * confident that the tree will be set correctly.
-        */
-      inline void
-      setSearchMethodTarget (const KdTreePtr &tree, 
-                             bool force_no_recompute = false) 
-      { 
-        tree_ = tree; 
-        if (force_no_recompute)
-        {
-          force_no_recompute_ = true;
-        }
-        // Since we just set a new tree, we need to check for updates
-        target_cloud_updated_ = true;
-      }
-
-      /** \brief Get a pointer to the search method used to find correspondences in the
-        * target cloud. */
-      inline KdTreePtr
-      getSearchMethodTarget () const
-      {
-        return (tree_);
-      }
-
-      /** \brief Provide a pointer to the search object used to find correspondences in
-        * the source cloud (usually used by reciprocal correspondence finding).
-        * \param[in] tree a pointer to the spatial search object.
-        * \param[in] force_no_recompute If set to true, this tree will NEVER be 
-        * recomputed, regardless of calls to setInputSource. Only use if you are 
-        * extremely confident that the tree will be set correctly.
-        */
-      inline void
-      setSearchMethodSource (const KdTreeReciprocalPtr &tree, 
-                             bool force_no_recompute = false) 
-      { 
-        tree_reciprocal_ = tree; 
-        if ( force_no_recompute )
-        {
-          force_no_recompute_reciprocal_ = true;
-        }
-        // Since we just set a new tree, we need to check for updates
-        source_cloud_updated_ = true;
-      }
-
-      /** \brief Get a pointer to the search method used to find correspondences in the
-        * source cloud. */
-      inline KdTreeReciprocalPtr
-      getSearchMethodSource () const
-      {
-        return (tree_reciprocal_);
-      }
-
-      /** \brief Get the final transformation matrix estimated by the registration method. */
-      inline Matrix4
-      getFinalTransformation () { return (final_transformation_); }
-
-      /** \brief Get the last incremental transformation matrix estimated by the registration method. */
-      inline Matrix4
-      getLastIncrementalTransformation () { return (transformation_); }
-
-      /** \brief Set the maximum number of iterations the internal optimization should run for.
-        * \param[in] nr_iterations the maximum number of iterations the internal optimization should run for
-        */
-      inline void 
-      setMaximumIterations (int nr_iterations) { max_iterations_ = nr_iterations; }
-
-      /** \brief Get the maximum number of iterations the internal optimization should run for, as set by the user. */
-      inline int 
-      getMaximumIterations () { return (max_iterations_); }
-
-      /** \brief Set the number of iterations RANSAC should run for.
-        * \param[in] ransac_iterations is the number of iterations RANSAC should run for
-        */
-      inline void 
-      setRANSACIterations (int ransac_iterations) { ransac_iterations_ = ransac_iterations; }
-
-      /** \brief Get the number of iterations RANSAC should run for, as set by the user. */
-      inline double 
-      getRANSACIterations () { return (ransac_iterations_); }
-
-      /** \brief Set the inlier distance threshold for the internal RANSAC outlier rejection loop.
-        * 
-        * The method considers a point to be an inlier, if the distance between the target data index and the transformed 
-        * source index is smaller than the given inlier distance threshold. 
-        * The value is set by default to 0.05m.
-        * \param[in] inlier_threshold the inlier distance threshold for the internal RANSAC outlier rejection loop
-        */
-      inline void 
-      setRANSACOutlierRejectionThreshold (double inlier_threshold) { inlier_threshold_ = inlier_threshold; }
-
-      /** \brief Get the inlier distance threshold for the internal outlier rejection loop as set by the user. */
-      inline double 
-      getRANSACOutlierRejectionThreshold () { return (inlier_threshold_); }
-
-      /** \brief Set the maximum distance threshold between two correspondent points in source <-> target. If the 
-        * distance is larger than this threshold, the points will be ignored in the alignment process.
-        * \param[in] distance_threshold the maximum distance threshold between a point and its nearest neighbor 
-        * correspondent in order to be considered in the alignment process
-        */
-      inline void 
-      setMaxCorrespondenceDistance (double distance_threshold) { corr_dist_threshold_ = distance_threshold; }
-
-      /** \brief Get the maximum distance threshold between two correspondent points in source <-> target. If the 
-        * distance is larger than this threshold, the points will be ignored in the alignment process.
-        */
-      inline double 
-      getMaxCorrespondenceDistance () { return (corr_dist_threshold_); }
-
-      /** \brief Set the transformation epsilon (maximum allowable translation squared difference between two consecutive
-        * transformations) in order for an optimization to be considered as having converged to the final 
-        * solution.
-        * \param[in] epsilon the transformation epsilon in order for an optimization to be considered as having 
-        * converged to the final solution.
-        */
-      inline void 
-      setTransformationEpsilon (double epsilon) { transformation_epsilon_ = epsilon; }
-
-      /** \brief Get the transformation epsilon (maximum allowable translation squared difference between two consecutive
-        * transformations) as set by the user.
-        */
-      inline double 
-      getTransformationEpsilon () { return (transformation_epsilon_); }
-
-      /** \brief Set the transformation rotation epsilon (maximum allowable rotation difference between two consecutive
-        * transformations) in order for an optimization to be considered as having converged to the final
-        * solution.
-        * \param[in] epsilon the transformation rotation epsilon in order for an optimization to be considered as having
-        * converged to the final solution (epsilon is the cos(angle) in a axis-angle representation).
-        */
-      inline void
-      setTransformationRotationEpsilon (double epsilon) { transformation_rotation_epsilon_ = epsilon; }
-
-      /** \brief Get the transformation rotation epsilon (maximum allowable difference between two consecutive
-        * transformations) as set by the user (epsilon is the cos(angle) in a axis-angle representation).
-        */
-      inline double
-      getTransformationRotationEpsilon () { return (transformation_rotation_epsilon_); }
-
-      /** \brief Set the maximum allowed Euclidean error between two consecutive steps in the ICP loop, before 
-        * the algorithm is considered to have converged. 
-        * The error is estimated as the sum of the differences between correspondences in an Euclidean sense, 
-        * divided by the number of correspondences.
-        * \param[in] epsilon the maximum allowed distance error before the algorithm will be considered to have
-        * converged
-        */
-      inline void 
-      setEuclideanFitnessEpsilon (double epsilon) { euclidean_fitness_epsilon_ = epsilon; }
-
-      /** \brief Get the maximum allowed distance error before the algorithm will be considered to have converged,
-        * as set by the user. See \ref setEuclideanFitnessEpsilon
-        */
-      inline double 
-      getEuclideanFitnessEpsilon () { return (euclidean_fitness_epsilon_); }
-
-      /** \brief Provide a boost shared pointer to the PointRepresentation to be used when comparing points
-        * \param[in] point_representation the PointRepresentation to be used by the k-D tree
-        */
-      inline void
-      setPointRepresentation (const PointRepresentationConstPtr &point_representation)
-      {
-        point_representation_ = point_representation;
-      }
-
-      /** \brief Register the user callback function which will be called from registration thread
-       * in order to update point cloud obtained after each iteration
-       * \param[in] visualizerCallback reference of the user callback function
-       */
-      inline bool
-      registerVisualizationCallback (std::function<UpdateVisualizerCallbackSignature> &visualizerCallback)
-      {
-        if (visualizerCallback)
-        {
-          update_visualizer_ = visualizerCallback;
-          return (true);
-        }
-        return (false);
-      }
-
-      /** \brief Obtain the Euclidean fitness score (e.g., mean of squared distances from the source to the target)
-        * \param[in] max_range maximum allowable distance between a point and its correspondence in the target 
-        * (default: double::max)
-        */
-      inline double 
-      getFitnessScore (double max_range = std::numeric_limits<double>::max ());
-
-      /** \brief Obtain the Euclidean fitness score (e.g., mean of squared distances from the source to the target)
-        * from two sets of correspondence distances (distances between source and target points)
-        * \param[in] distances_a the first set of distances between correspondences
-        * \param[in] distances_b the second set of distances between correspondences
-        */
-      inline double 
-      getFitnessScore (const std::vector<float> &distances_a, const std::vector<float> &distances_b);
-
-      /** \brief Return the state of convergence after the last align run */
-      inline bool 
-      hasConverged () const { return (converged_); }
-
-      /** \brief Call the registration algorithm which estimates the transformation and returns the transformed source 
-        * (input) as \a output.
-        * \param[out] output the resultant input transformed point cloud dataset
-        */
-      inline void
-      align (PointCloudSource &output);
-
-      /** \brief Call the registration algorithm which estimates the transformation and returns the transformed source 
-        * (input) as \a output.
-        * \param[out] output the resultant input transformed point cloud dataset
-        * \param[in] guess the initial gross estimation of the transformation
-        */
-      inline void 
-      align (PointCloudSource &output, const Matrix4& guess);
-
-      /** \brief Abstract class get name method. */
-      inline const std::string&
-      getClassName () const { return (reg_name_); }
-        
-      /** \brief Internal computation initialization. */
-      bool
-      initCompute ();
-
-      /** \brief Internal computation when reciprocal lookup is needed */
-      bool
-      initComputeReciprocal ();
-
-      /** \brief Add a new correspondence rejector to the list
-        * \param[in] rejector the new correspondence rejector to concatenate
-        *
-        * Code example:
-        *
-        * \code
-        * CorrespondenceRejectorDistance rej;
-        * rej.setInputCloud<PointXYZ> (keypoints_src);
-        * rej.setInputTarget<PointXYZ> (keypoints_tgt);
-        * rej.setMaximumDistance (1);
-        * rej.setInputCorrespondences (all_correspondences);
-        * 
-        * // or...
-        *
-        * \endcode
-        */
-      inline void
-      addCorrespondenceRejector (const CorrespondenceRejectorPtr &rejector)
-      {
-        correspondence_rejectors_.push_back (rejector);
-      }
-
-      /** \brief Get the list of correspondence rejectors. */
-      inline std::vector<CorrespondenceRejectorPtr>
-      getCorrespondenceRejectors ()
-      {
-        return (correspondence_rejectors_);
-      }
-
-      /** \brief Remove the i-th correspondence rejector in the list
-        * \param[in] i the position of the correspondence rejector in the list to remove
-        */
-      inline bool
-      removeCorrespondenceRejector (unsigned int i)
-      {
-        if (i >= correspondence_rejectors_.size ())
-          return (false);
-        correspondence_rejectors_.erase (correspondence_rejectors_.begin () + i);
-        return (true);
-      }
-
-      /** \brief Clear the list of correspondence rejectors. */
-      inline void
-      clearCorrespondenceRejectors ()
-      {
-        correspondence_rejectors_.clear ();
-      }
-
-    protected:
-      /** \brief The registration method name. */
-      std::string reg_name_;
-
-      /** \brief A pointer to the spatial search object. */
-      KdTreePtr tree_;
-      
-      /** \brief A pointer to the spatial search object of the source. */
-      KdTreeReciprocalPtr tree_reciprocal_;
-
-      /** \brief The number of iterations the internal optimization ran for (used internally). */
-      int nr_iterations_;
-
-      /** \brief The maximum number of iterations the internal optimization should run for.
-        * The default value is 10.
-        */
-      int max_iterations_;
-
-      /** \brief The number of iterations RANSAC should run for. */
-      int ransac_iterations_;
-
-      /** \brief The input point cloud dataset target. */
-      PointCloudTargetConstPtr target_;
-
-      /** \brief The final transformation matrix estimated by the registration method after N iterations. */
-      Matrix4 final_transformation_;
-
-      /** \brief The transformation matrix estimated by the registration method. */
-      Matrix4 transformation_;
-
-      /** \brief The previous transformation matrix estimated by the registration method (used internally). */
-      Matrix4 previous_transformation_;
-
-      /** \brief The maximum difference between two consecutive transformations in order to consider convergence 
-        * (user defined). 
-        */
-      double transformation_epsilon_;
-
-      /** \brief The maximum rotation difference between two consecutive transformations in order to consider convergence
-        * (user defined).
-        */
-      double transformation_rotation_epsilon_;
-
-      /** \brief The maximum allowed Euclidean error between two consecutive steps in the ICP loop, before the 
-        * algorithm is considered to have converged. The error is estimated as the sum of the differences between 
-        * correspondences in an Euclidean sense, divided by the number of correspondences.
-        */
-      double euclidean_fitness_epsilon_;
-
-      /** \brief The maximum distance threshold between two correspondent points in source <-> target. If the 
-        * distance is larger than this threshold, the points will be ignored in the alignment process.
-        */
-      double corr_dist_threshold_;
-
-      /** \brief The inlier distance threshold for the internal RANSAC outlier rejection loop.
-        * The method considers a point to be an inlier, if the distance between the target data index and the transformed 
-        * source index is smaller than the given inlier distance threshold. The default value is 0.05. 
-        */
-      double inlier_threshold_;
-
-      /** \brief Holds internal convergence state, given user parameters. */
-      bool converged_;
-
-      /** \brief The minimum number of correspondences that the algorithm needs before attempting to estimate the 
-        * transformation. The default value is 3.
-        */
-      int min_number_correspondences_;
-
-      /** \brief The set of correspondences determined at this ICP step. */
-      CorrespondencesPtr correspondences_;
-
-      /** \brief A TransformationEstimation object, used to calculate the 4x4 rigid transformation. */
-      TransformationEstimationPtr transformation_estimation_;
-
-      /** \brief A CorrespondenceEstimation object, used to estimate correspondences between the source and the target cloud. */
-      CorrespondenceEstimationPtr correspondence_estimation_;
-
-      /** \brief The list of correspondence rejectors to use. */
-      std::vector<CorrespondenceRejectorPtr> correspondence_rejectors_;
-
-      /** \brief Variable that stores whether we have a new target cloud, meaning we need to pre-process it again.
-       * This way, we avoid rebuilding the kd-tree for the target cloud every time the determineCorrespondences () method
-       * is called. */
-      bool target_cloud_updated_;
-      /** \brief Variable that stores whether we have a new source cloud, meaning we need to pre-process it again.
-       * This way, we avoid rebuilding the reciprocal kd-tree for the source cloud every time the determineCorrespondences () method
-       * is called. */
-      bool source_cloud_updated_;
-      /** \brief A flag which, if set, means the tree operating on the target cloud 
-       * will never be recomputed*/
-      bool force_no_recompute_;
-      
-      /** \brief A flag which, if set, means the tree operating on the source cloud 
-       * will never be recomputed*/
-      bool force_no_recompute_reciprocal_;
-
-      /** \brief Callback function to update intermediate source point cloud position during it's registration
-        * to the target point cloud.
-        */
-      std::function<UpdateVisualizerCallbackSignature> update_visualizer_;
-
-      /** \brief Search for the closest nearest neighbor of a given point.
-        * \param cloud the point cloud dataset to use for nearest neighbor search
-        * \param index the index of the query point
-        * \param indices the resultant vector of indices representing the k-nearest neighbors
-        * \param distances the resultant distances from the query point to the k-nearest neighbors
-        */
-      inline bool
-      searchForNeighbors (const PointCloudSource &cloud, int index, 
-                          std::vector<int> &indices, std::vector<float> &distances)
-      {
-        int k = tree_->nearestKSearch (cloud, index, 1, indices, distances);
-        if (k == 0)
-          return (false);
-        return (true);
-      }
-
-      /** \brief Abstract transformation computation method with initial guess */
-      virtual void 
-      computeTransformation (PointCloudSource &output, const Matrix4& guess) = 0;
-
-    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
-   };
-}
+  /** \brief Get the final transformation matrix estimated by the registration method.
+   */
+  inline Matrix4
+  getFinalTransformation()
+  {
+    return (final_transformation_);
+  }
+
+  /** \brief Get the last incremental transformation matrix estimated by the
+   * registration method. */
+  inline Matrix4
+  getLastIncrementalTransformation()
+  {
+    return (transformation_);
+  }
+
+  /** \brief Set the maximum number of iterations the internal optimization should run
+   * for. \param[in] nr_iterations the maximum number of iterations the internal
+   * optimization should run for
+   */
+  inline void
+  setMaximumIterations(int nr_iterations)
+  {
+    max_iterations_ = nr_iterations;
+  }
+
+  /** \brief Get the maximum number of iterations the internal optimization should run
+   * for, as set by the user. */
+  inline int
+  getMaximumIterations()
+  {
+    return (max_iterations_);
+  }
+
+  /** \brief Set the number of iterations RANSAC should run for.
+   * \param[in] ransac_iterations is the number of iterations RANSAC should run for
+   */
+  inline void
+  setRANSACIterations(int ransac_iterations)
+  {
+    ransac_iterations_ = ransac_iterations;
+  }
+
+  /** \brief Get the number of iterations RANSAC should run for, as set by the user. */
+  inline double
+  getRANSACIterations()
+  {
+    return (ransac_iterations_);
+  }
+
+  /** \brief Set the inlier distance threshold for the internal RANSAC outlier rejection
+   * loop.
+   *
+   * The method considers a point to be an inlier, if the distance between the target
+   * data index and the transformed source index is smaller than the given inlier
+   * distance threshold. The value is set by default to 0.05m. \param[in]
+   * inlier_threshold the inlier distance threshold for the internal RANSAC outlier
+   * rejection loop
+   */
+  inline void
+  setRANSACOutlierRejectionThreshold(double inlier_threshold)
+  {
+    inlier_threshold_ = inlier_threshold;
+  }
+
+  /** \brief Get the inlier distance threshold for the internal outlier rejection loop
+   * as set by the user. */
+  inline double
+  getRANSACOutlierRejectionThreshold()
+  {
+    return (inlier_threshold_);
+  }
+
+  /** \brief Set the maximum distance threshold between two correspondent points in
+   * source <-> target. If the distance is larger than this threshold, the points will
+   * be ignored in the alignment process. \param[in] distance_threshold the maximum
+   * distance threshold between a point and its nearest neighbor correspondent in order
+   * to be considered in the alignment process
+   */
+  inline void
+  setMaxCorrespondenceDistance(double distance_threshold)
+  {
+    corr_dist_threshold_ = distance_threshold;
+  }
+
+  /** \brief Get the maximum distance threshold between two correspondent points in
+   * source <-> target. If the distance is larger than this threshold, the points will
+   * be ignored in the alignment process.
+   */
+  inline double
+  getMaxCorrespondenceDistance()
+  {
+    return (corr_dist_threshold_);
+  }
+
+  /** \brief Set the transformation epsilon (maximum allowable translation squared
+   * difference between two consecutive transformations) in order for an optimization to
+   * be considered as having converged to the final solution. \param[in] epsilon the
+   * transformation epsilon in order for an optimization to be considered as having
+   * converged to the final solution.
+   */
+  inline void
+  setTransformationEpsilon(double epsilon)
+  {
+    transformation_epsilon_ = epsilon;
+  }
+
+  /** \brief Get the transformation epsilon (maximum allowable translation squared
+   * difference between two consecutive transformations) as set by the user.
+   */
+  inline double
+  getTransformationEpsilon()
+  {
+    return (transformation_epsilon_);
+  }
+
+  /** \brief Set the transformation rotation epsilon (maximum allowable rotation
+   * difference between two consecutive transformations) in order for an optimization to
+   * be considered as having converged to the final solution. \param[in] epsilon the
+   * transformation rotation epsilon in order for an optimization to be considered as
+   * having converged to the final solution (epsilon is the cos(angle) in a axis-angle
+   * representation).
+   */
+  inline void
+  setTransformationRotationEpsilon(double epsilon)
+  {
+    transformation_rotation_epsilon_ = epsilon;
+  }
+
+  /** \brief Get the transformation rotation epsilon (maximum allowable difference
+   * between two consecutive transformations) as set by the user (epsilon is the
+   * cos(angle) in a axis-angle representation).
+   */
+  inline double
+  getTransformationRotationEpsilon()
+  {
+    return (transformation_rotation_epsilon_);
+  }
+
+  /** \brief Set the maximum allowed Euclidean error between two consecutive steps in
+   * the ICP loop, before the algorithm is considered to have converged. The error is
+   * estimated as the sum of the differences between correspondences in an Euclidean
+   * sense, divided by the number of correspondences. \param[in] epsilon the maximum
+   * allowed distance error before the algorithm will be considered to have converged
+   */
+  inline void
+  setEuclideanFitnessEpsilon(double epsilon)
+  {
+    euclidean_fitness_epsilon_ = epsilon;
+  }
+
+  /** \brief Get the maximum allowed distance error before the algorithm will be
+   * considered to have converged, as set by the user. See \ref
+   * setEuclideanFitnessEpsilon
+   */
+  inline double
+  getEuclideanFitnessEpsilon()
+  {
+    return (euclidean_fitness_epsilon_);
+  }
+
+  /** \brief Provide a boost shared pointer to the PointRepresentation to be used when
+   * comparing points \param[in] point_representation the PointRepresentation to be used
+   * by the k-D tree
+   */
+  inline void
+  setPointRepresentation(const PointRepresentationConstPtr& point_representation)
+  {
+    point_representation_ = point_representation;
+  }
+
+  /** \brief Register the user callback function which will be called from registration
+   * thread in order to update point cloud obtained after each iteration \param[in]
+   * visualizerCallback reference of the user callback function
+   */
+  inline bool
+  registerVisualizationCallback(
+      std::function<UpdateVisualizerCallbackSignature>& visualizerCallback)
+  {
+    if (visualizerCallback) {
+      update_visualizer_ = visualizerCallback;
+      return (true);
+    }
+    return (false);
+  }
+
+  /** \brief Obtain the Euclidean fitness score (e.g., mean of squared distances from
+   * the source to the target) \param[in] max_range maximum allowable distance between a
+   * point and its correspondence in the target (default: double::max)
+   */
+  inline double
+  getFitnessScore(double max_range = std::numeric_limits<double>::max());
+
+  /** \brief Obtain the Euclidean fitness score (e.g., mean of squared distances from
+   * the source to the target) from two sets of correspondence distances (distances
+   * between source and target points) \param[in] distances_a the first set of distances
+   * between correspondences \param[in] distances_b the second set of distances between
+   * correspondences
+   */
+  inline double
+  getFitnessScore(const std::vector<float>& distances_a,
+                  const std::vector<float>& distances_b);
+
+  /** \brief Return the state of convergence after the last align run */
+  inline bool
+  hasConverged() const
+  {
+    return (converged_);
+  }
+
+  /** \brief Call the registration algorithm which estimates the transformation and
+   * returns the transformed source (input) as \a output. \param[out] output the
+   * resultant input transformed point cloud dataset
+   */
+  inline void
+  align(PointCloudSource& output);
+
+  /** \brief Call the registration algorithm which estimates the transformation and
+   * returns the transformed source (input) as \a output. \param[out] output the
+   * resultant input transformed point cloud dataset \param[in] guess the initial gross
+   * estimation of the transformation
+   */
+  inline void
+  align(PointCloudSource& output, const Matrix4& guess);
+
+  /** \brief Abstract class get name method. */
+  inline const std::string&
+  getClassName() const
+  {
+    return (reg_name_);
+  }
+
+  /** \brief Internal computation initialization. */
+  bool
+  initCompute();
+
+  /** \brief Internal computation when reciprocal lookup is needed */
+  bool
+  initComputeReciprocal();
+
+  /** \brief Add a new correspondence rejector to the list
+   * \param[in] rejector the new correspondence rejector to concatenate
+   *
+   * Code example:
+   *
+   * \code
+   * CorrespondenceRejectorDistance rej;
+   * rej.setInputCloud<PointXYZ> (keypoints_src);
+   * rej.setInputTarget<PointXYZ> (keypoints_tgt);
+   * rej.setMaximumDistance (1);
+   * rej.setInputCorrespondences (all_correspondences);
+   *
+   * // or...
+   *
+   * \endcode
+   */
+  inline void
+  addCorrespondenceRejector(const CorrespondenceRejectorPtr& rejector)
+  {
+    correspondence_rejectors_.push_back(rejector);
+  }
+
+  /** \brief Get the list of correspondence rejectors. */
+  inline std::vector<CorrespondenceRejectorPtr>
+  getCorrespondenceRejectors()
+  {
+    return (correspondence_rejectors_);
+  }
+
+  /** \brief Remove the i-th correspondence rejector in the list
+   * \param[in] i the position of the correspondence rejector in the list to remove
+   */
+  inline bool
+  removeCorrespondenceRejector(unsigned int i)
+  {
+    if (i >= correspondence_rejectors_.size())
+      return (false);
+    correspondence_rejectors_.erase(correspondence_rejectors_.begin() + i);
+    return (true);
+  }
+
+  /** \brief Clear the list of correspondence rejectors. */
+  inline void
+  clearCorrespondenceRejectors()
+  {
+    correspondence_rejectors_.clear();
+  }
+
+protected:
+  /** \brief The registration method name. */
+  std::string reg_name_;
+
+  /** \brief A pointer to the spatial search object. */
+  KdTreePtr tree_;
+
+  /** \brief A pointer to the spatial search object of the source. */
+  KdTreeReciprocalPtr tree_reciprocal_;
+
+  /** \brief The number of iterations the internal optimization ran for (used
+   * internally). */
+  int nr_iterations_;
+
+  /** \brief The maximum number of iterations the internal optimization should run for.
+   * The default value is 10.
+   */
+  int max_iterations_;
+
+  /** \brief The number of iterations RANSAC should run for. */
+  int ransac_iterations_;
+
+  /** \brief The input point cloud dataset target. */
+  PointCloudTargetConstPtr target_;
+
+  /** \brief The final transformation matrix estimated by the registration method after
+   * N iterations. */
+  Matrix4 final_transformation_;
+
+  /** \brief The transformation matrix estimated by the registration method. */
+  Matrix4 transformation_;
+
+  /** \brief The previous transformation matrix estimated by the registration method
+   * (used internally). */
+  Matrix4 previous_transformation_;
+
+  /** \brief The maximum difference between two consecutive transformations in order to
+   * consider convergence (user defined).
+   */
+  double transformation_epsilon_;
+
+  /** \brief The maximum rotation difference between two consecutive transformations in
+   * order to consider convergence (user defined).
+   */
+  double transformation_rotation_epsilon_;
+
+  /** \brief The maximum allowed Euclidean error between two consecutive steps in the
+   * ICP loop, before the algorithm is considered to have converged. The error is
+   * estimated as the sum of the differences between correspondences in an Euclidean
+   * sense, divided by the number of correspondences.
+   */
+  double euclidean_fitness_epsilon_;
+
+  /** \brief The maximum distance threshold between two correspondent points in source
+   * <-> target. If the distance is larger than this threshold, the points will be
+   * ignored in the alignment process.
+   */
+  double corr_dist_threshold_;
+
+  /** \brief The inlier distance threshold for the internal RANSAC outlier rejection
+   * loop. The method considers a point to be an inlier, if the distance between the
+   * target data index and the transformed source index is smaller than the given inlier
+   * distance threshold. The default value is 0.05.
+   */
+  double inlier_threshold_;
+
+  /** \brief Holds internal convergence state, given user parameters. */
+  bool converged_;
+
+  /** \brief The minimum number of correspondences that the algorithm needs before
+   * attempting to estimate the transformation. The default value is 3.
+   */
+  int min_number_correspondences_;
+
+  /** \brief The set of correspondences determined at this ICP step. */
+  CorrespondencesPtr correspondences_;
+
+  /** \brief A TransformationEstimation object, used to calculate the 4x4 rigid
+   * transformation. */
+  TransformationEstimationPtr transformation_estimation_;
+
+  /** \brief A CorrespondenceEstimation object, used to estimate correspondences between
+   * the source and the target cloud. */
+  CorrespondenceEstimationPtr correspondence_estimation_;
+
+  /** \brief The list of correspondence rejectors to use. */
+  std::vector<CorrespondenceRejectorPtr> correspondence_rejectors_;
+
+  /** \brief Variable that stores whether we have a new target cloud, meaning we need to
+   * pre-process it again. This way, we avoid rebuilding the kd-tree for the target
+   * cloud every time the determineCorrespondences () method is called. */
+  bool target_cloud_updated_;
+  /** \brief Variable that stores whether we have a new source cloud, meaning we need to
+   * pre-process it again. This way, we avoid rebuilding the reciprocal kd-tree for the
+   * source cloud every time the determineCorrespondences () method is called. */
+  bool source_cloud_updated_;
+  /** \brief A flag which, if set, means the tree operating on the target cloud
+   * will never be recomputed*/
+  bool force_no_recompute_;
+
+  /** \brief A flag which, if set, means the tree operating on the source cloud
+   * will never be recomputed*/
+  bool force_no_recompute_reciprocal_;
+
+  /** \brief Callback function to update intermediate source point cloud position during
+   * it's registration to the target point cloud.
+   */
+  std::function<UpdateVisualizerCallbackSignature> update_visualizer_;
+
+  /** \brief Search for the closest nearest neighbor of a given point.
+   * \param cloud the point cloud dataset to use for nearest neighbor search
+   * \param index the index of the query point
+   * \param indices the resultant vector of indices representing the k-nearest neighbors
+   * \param distances the resultant distances from the query point to the k-nearest
+   * neighbors
+   */
+  inline bool
+  searchForNeighbors(const PointCloudSource& cloud,
+                     int index,
+                     pcl::Indices& indices,
+                     std::vector<float>& distances)
+  {
+    int k = tree_->nearestKSearch(cloud, index, 1, indices, distances);
+    if (k == 0)
+      return (false);
+    return (true);
+  }
+
+  /** \brief Abstract transformation computation method with initial guess */
+  virtual void
+  computeTransformation(PointCloudSource& output, const Matrix4& guess) = 0;
+
+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
+};
+} // namespace pcl
 
 #include <pcl/registration/impl/registration.hpp>
diff --git a/registration/include/pcl/registration/sample_consensus_prerejective.h b/registration/include/pcl/registration/sample_consensus_prerejective.h
index 0f0a2ea5..d3388224 100644
--- a/registration/include/pcl/registration/sample_consensus_prerejective.h
+++ b/registration/include/pcl/registration/sample_consensus_prerejective.h
@@ -40,278 +40,290 @@
 
 #pragma once
 
+#include <pcl/registration/correspondence_rejection_poly.h>
 #include <pcl/registration/registration.h>
 #include <pcl/registration/transformation_estimation_svd.h>
 #include <pcl/registration/transformation_validation.h>
-#include <pcl/registration/correspondence_rejection_poly.h>
 
-namespace pcl
-{
-  /** \brief Pose estimation and alignment class using a prerejective RANSAC routine.
-   * 
-   * This class inserts a simple, yet effective "prerejection" step into the standard
-   * RANSAC pose estimation loop in order to avoid verification of pose hypotheses
-   * that are likely to be wrong. This is achieved by local pose-invariant geometric
-   * constraints, as also implemented in the class
-   * \ref registration::CorrespondenceRejectorPoly "CorrespondenceRejectorPoly".
-   * 
-   * In order to robustly align partial/occluded models, this routine performs
-   * fit error evaluation using only inliers, i.e. points closer than a
-   * Euclidean threshold, which is specifiable using \ref setInlierFraction().
-   * 
-   * The amount of prerejection or "greedyness" of the algorithm can be specified
-   * using \ref setSimilarityThreshold() in [0,1[, where a value of 0 means disabled,
-   * and 1 is maximally rejective.
-   * 
-   * If you use this in academic work, please cite:
-   * 
-   * A. G. Buch, D. Kraft, J.-K. Kämäräinen, H. G. Petersen and N. Krüger.
-   * Pose Estimation using Local Structure-Specific Shape and Appearance Context.
-   * International Conference on Robotics and Automation (ICRA), 2013.
-   *  
-   * \author Anders Glent Buch (andersgb1@gmail.com)
-   * \ingroup registration
+namespace pcl {
+/** \brief Pose estimation and alignment class using a prerejective RANSAC routine.
+ *
+ * This class inserts a simple, yet effective "prerejection" step into the standard
+ * RANSAC pose estimation loop in order to avoid verification of pose hypotheses
+ * that are likely to be wrong. This is achieved by local pose-invariant geometric
+ * constraints, as also implemented in the class
+ * \ref registration::CorrespondenceRejectorPoly "CorrespondenceRejectorPoly".
+ *
+ * In order to robustly align partial/occluded models, this routine performs
+ * fit error evaluation using only inliers, i.e. points closer than a
+ * Euclidean threshold, which is specifiable using \ref setInlierFraction().
+ *
+ * The amount of prerejection or "greedyness" of the algorithm can be specified
+ * using \ref setSimilarityThreshold() in [0,1[, where a value of 0 means disabled,
+ * and 1 is maximally rejective.
+ *
+ * If you use this in academic work, please cite:
+ *
+ * A. G. Buch, D. Kraft, J.-K. Kämäräinen, H. G. Petersen and N. Krüger.
+ * Pose Estimation using Local Structure-Specific Shape and Appearance Context.
+ * International Conference on Robotics and Automation (ICRA), 2013.
+ *
+ * \author Anders Glent Buch (andersgb1@gmail.com)
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename FeatureT>
+class SampleConsensusPrerejective : public Registration<PointSource, PointTarget> {
+public:
+  using Matrix4 = typename Registration<PointSource, PointTarget>::Matrix4;
+
+  using Registration<PointSource, PointTarget>::reg_name_;
+  using Registration<PointSource, PointTarget>::getClassName;
+  using Registration<PointSource, PointTarget>::input_;
+  using Registration<PointSource, PointTarget>::target_;
+  using Registration<PointSource, PointTarget>::tree_;
+  using Registration<PointSource, PointTarget>::max_iterations_;
+  using Registration<PointSource, PointTarget>::corr_dist_threshold_;
+  using Registration<PointSource, PointTarget>::transformation_;
+  using Registration<PointSource, PointTarget>::final_transformation_;
+  using Registration<PointSource, PointTarget>::transformation_estimation_;
+  using Registration<PointSource, PointTarget>::getFitnessScore;
+  using Registration<PointSource, PointTarget>::converged_;
+
+  using PointCloudSource =
+      typename Registration<PointSource, PointTarget>::PointCloudSource;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+
+  using PointCloudTarget =
+      typename Registration<PointSource, PointTarget>::PointCloudTarget;
+
+  using PointIndicesPtr = PointIndices::Ptr;
+  using PointIndicesConstPtr = PointIndices::ConstPtr;
+
+  using FeatureCloud = pcl::PointCloud<FeatureT>;
+  using FeatureCloudPtr = typename FeatureCloud::Ptr;
+  using FeatureCloudConstPtr = typename FeatureCloud::ConstPtr;
+
+  using Ptr =
+      shared_ptr<SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>>;
+  using ConstPtr =
+      shared_ptr<const SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>>;
+
+  using FeatureKdTreePtr = typename KdTreeFLANN<FeatureT>::Ptr;
+
+  using CorrespondenceRejectorPoly =
+      pcl::registration::CorrespondenceRejectorPoly<PointSource, PointTarget>;
+  using CorrespondenceRejectorPolyPtr = typename CorrespondenceRejectorPoly::Ptr;
+  using CorrespondenceRejectorPolyConstPtr =
+      typename CorrespondenceRejectorPoly::ConstPtr;
+
+  /** \brief Constructor */
+  SampleConsensusPrerejective()
+  : input_features_()
+  , target_features_()
+  , nr_samples_(3)
+  , k_correspondences_(2)
+  , feature_tree_(new pcl::KdTreeFLANN<FeatureT>)
+  , correspondence_rejector_poly_(new CorrespondenceRejectorPoly)
+  , inlier_fraction_(0.0f)
+  {
+    reg_name_ = "SampleConsensusPrerejective";
+    correspondence_rejector_poly_->setSimilarityThreshold(0.6f);
+    max_iterations_ = 5000;
+    transformation_estimation_.reset(
+        new pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>);
+  };
+
+  /** \brief Destructor */
+  ~SampleConsensusPrerejective() {}
+
+  /** \brief Provide a boost shared pointer to the source point cloud's feature
+   * descriptors \param features the source point cloud's features
+   */
+  void
+  setSourceFeatures(const FeatureCloudConstPtr& features);
+
+  /** \brief Get a pointer to the source point cloud's features */
+  inline const FeatureCloudConstPtr
+  getSourceFeatures() const
+  {
+    return (input_features_);
+  }
+
+  /** \brief Provide a boost shared pointer to the target point cloud's feature
+   * descriptors \param features the target point cloud's features
+   */
+  void
+  setTargetFeatures(const FeatureCloudConstPtr& features);
+
+  /** \brief Get a pointer to the target point cloud's features */
+  inline const FeatureCloudConstPtr
+  getTargetFeatures() const
+  {
+    return (target_features_);
+  }
+
+  /** \brief Set the number of samples to use during each iteration
+   * \param nr_samples the number of samples to use during each iteration
+   */
+  inline void
+  setNumberOfSamples(int nr_samples)
+  {
+    nr_samples_ = nr_samples;
+  }
+
+  /** \brief Get the number of samples to use during each iteration, as set by the user
    */
-  template <typename PointSource, typename PointTarget, typename FeatureT>
-  class SampleConsensusPrerejective : public Registration<PointSource, PointTarget>
+  inline int
+  getNumberOfSamples() const
   {
-    public:
-      using Matrix4 = typename Registration<PointSource, PointTarget>::Matrix4;
-      
-      using Registration<PointSource, PointTarget>::reg_name_;
-      using Registration<PointSource, PointTarget>::getClassName;
-      using Registration<PointSource, PointTarget>::input_;
-      using Registration<PointSource, PointTarget>::target_;
-      using Registration<PointSource, PointTarget>::tree_;
-      using Registration<PointSource, PointTarget>::max_iterations_;
-      using Registration<PointSource, PointTarget>::corr_dist_threshold_;
-      using Registration<PointSource, PointTarget>::transformation_;
-      using Registration<PointSource, PointTarget>::final_transformation_;
-      using Registration<PointSource, PointTarget>::transformation_estimation_;
-      using Registration<PointSource, PointTarget>::getFitnessScore;
-      using Registration<PointSource, PointTarget>::converged_;
-
-      using PointCloudSource = typename Registration<PointSource, PointTarget>::PointCloudSource;
-      using PointCloudSourcePtr = typename PointCloudSource::Ptr;
-      using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
-
-      using PointCloudTarget = typename Registration<PointSource, PointTarget>::PointCloudTarget;
-
-      using PointIndicesPtr = PointIndices::Ptr;
-      using PointIndicesConstPtr = PointIndices::ConstPtr;
-
-      using FeatureCloud = pcl::PointCloud<FeatureT>;
-      using FeatureCloudPtr = typename FeatureCloud::Ptr;
-      using FeatureCloudConstPtr = typename FeatureCloud::ConstPtr;
-
-      using Ptr = shared_ptr<SampleConsensusPrerejective<PointSource, PointTarget, FeatureT> >;
-      using ConstPtr = shared_ptr<const SampleConsensusPrerejective<PointSource, PointTarget, FeatureT> >;
-
-      using FeatureKdTreePtr = typename KdTreeFLANN<FeatureT>::Ptr;
-      
-      using CorrespondenceRejectorPoly = pcl::registration::CorrespondenceRejectorPoly<PointSource, PointTarget>;
-      using CorrespondenceRejectorPolyPtr = typename CorrespondenceRejectorPoly::Ptr;
-      using CorrespondenceRejectorPolyConstPtr = typename CorrespondenceRejectorPoly::ConstPtr;
-      
-      /** \brief Constructor */
-      SampleConsensusPrerejective ()
-        : input_features_ ()
-        , target_features_ ()
-        , nr_samples_(3)
-        , k_correspondences_ (2)
-        , feature_tree_ (new pcl::KdTreeFLANN<FeatureT>)
-        , correspondence_rejector_poly_ (new CorrespondenceRejectorPoly)
-        , inlier_fraction_ (0.0f)
-      {
-        reg_name_ = "SampleConsensusPrerejective";
-        correspondence_rejector_poly_->setSimilarityThreshold (0.6f);
-        max_iterations_ = 5000;
-        transformation_estimation_.reset (new pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>);
-      };
-      
-      /** \brief Destructor */
-      ~SampleConsensusPrerejective ()
-      {
-      }
-
-      /** \brief Provide a boost shared pointer to the source point cloud's feature descriptors
-        * \param features the source point cloud's features
-        */
-      void 
-      setSourceFeatures (const FeatureCloudConstPtr &features);
-
-      /** \brief Get a pointer to the source point cloud's features */
-      inline const FeatureCloudConstPtr
-      getSourceFeatures () const
-      { 
-        return (input_features_);
-      }
-
-      /** \brief Provide a boost shared pointer to the target point cloud's feature descriptors
-        * \param features the target point cloud's features
-        */
-      void 
-      setTargetFeatures (const FeatureCloudConstPtr &features);
-
-      /** \brief Get a pointer to the target point cloud's features */
-      inline const FeatureCloudConstPtr 
-      getTargetFeatures () const
-      {
-        return (target_features_);
-      }
-
-      /** \brief Set the number of samples to use during each iteration
-        * \param nr_samples the number of samples to use during each iteration
-        */
-      inline void 
-      setNumberOfSamples (int nr_samples)
-      {
-        nr_samples_ = nr_samples;
-      }
-
-      /** \brief Get the number of samples to use during each iteration, as set by the user */
-      inline int 
-      getNumberOfSamples () const
-      {
-        return (nr_samples_);
-      }
-
-      /** \brief Set the number of neighbors to use when selecting a random feature correspondence.  A higher value will
-        * add more randomness to the feature matching.
-        * \param k the number of neighbors to use when selecting a random feature correspondence.
-        */
-      inline void
-      setCorrespondenceRandomness (int k)
-      {
-        k_correspondences_ = k;
-      }
-
-      /** \brief Get the number of neighbors used when selecting a random feature correspondence, as set by the user */
-      inline int
-      getCorrespondenceRandomness () const
-      {
-        return (k_correspondences_);
-      }
-      
-      /** \brief Set the similarity threshold in [0,1[ between edge lengths of the underlying polygonal correspondence rejector object,
-       * where 1 is a perfect match
-       * \param similarity_threshold edge length similarity threshold
-       */
-      inline void
-      setSimilarityThreshold (float similarity_threshold)
-      {
-        correspondence_rejector_poly_->setSimilarityThreshold (similarity_threshold);
-      }
-      
-      /** \brief Get the similarity threshold between edge lengths of the underlying polygonal correspondence rejector object,
-       * \return edge length similarity threshold
-       */
-      inline float
-      getSimilarityThreshold () const
-      {
-        return correspondence_rejector_poly_->getSimilarityThreshold ();
-      }
-      
-      /** \brief Set the required inlier fraction (of the input)
-       * \param inlier_fraction required inlier fraction, must be in [0,1]
-       */
-      inline void
-      setInlierFraction (float inlier_fraction)
-      {
-        inlier_fraction_ = inlier_fraction;
-      }
-      
-      /** \brief Get the required inlier fraction
-       * \return required inlier fraction in [0,1]
-       */
-      inline float
-      getInlierFraction () const
-      {
-        return inlier_fraction_;
-      }
-      
-      /** \brief Get the inlier indices of the source point cloud under the final transformation
-       * @return inlier indices
-       */
-      inline const std::vector<int>&
-      getInliers () const
-      {
-        return inliers_;
-      }
-
-    protected:
-      /** \brief Choose a random index between 0 and n-1
-        * \param n the number of possible indices to choose from
-        */
-      inline int 
-      getRandomIndex (int n) const
-      {
-        return (static_cast<int> (n * (rand () / (RAND_MAX + 1.0))));
-      };
-      
-      /** \brief Select \a nr_samples sample points from cloud while making sure that their pairwise distances are 
-        * greater than a user-defined minimum distance, \a min_sample_distance.
-        * \param cloud the input point cloud
-        * \param nr_samples the number of samples to select
-        * \param sample_indices the resulting sample indices
-        */
-      void 
-      selectSamples (const PointCloudSource &cloud, int nr_samples, std::vector<int> &sample_indices);
-
-      /** \brief For each of the sample points, find a list of points in the target cloud whose features are similar to 
-        * the sample points' features. From these, select one randomly which will be considered that sample point's 
-        * correspondence.
-        * \param sample_indices the indices of each sample point
-        * \param similar_features correspondence cache, which is used to read/write already computed correspondences
-        * \param corresponding_indices the resulting indices of each sample's corresponding point in the target cloud
-        */
-      void 
-      findSimilarFeatures (const std::vector<int> &sample_indices,
-              std::vector<std::vector<int> >& similar_features,
-              std::vector<int> &corresponding_indices);
-
-      /** \brief Rigid transformation computation method.
-        * \param output the transformed input point cloud dataset using the rigid transformation found
-        * \param guess The computed transformation
-        */
-      void 
-      computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) override;
-
-      /** \brief Obtain the fitness of a transformation
-        * The following metrics are calculated, based on
-        * \b final_transformation_ and \b corr_dist_threshold_:
-        *   - Inliers: the number of transformed points which are closer than threshold to NN
-        *   - Error score: the MSE of the inliers  
-        * \param inliers indices of source point cloud inliers
-        * \param fitness_score output fitness score as RMSE 
-        */
-      void 
-      getFitness (std::vector<int>& inliers, float& fitness_score);
-
-      /** \brief The source point cloud's feature descriptors. */
-      FeatureCloudConstPtr input_features_;
-
-      /** \brief The target point cloud's feature descriptors. */
-      FeatureCloudConstPtr target_features_;  
-
-      /** \brief The number of samples to use during each iteration. */
-      int nr_samples_;
-
-      /** \brief The number of neighbors to use when selecting a random feature correspondence. */
-      int k_correspondences_;
-     
-      /** \brief The KdTree used to compare feature descriptors. */
-      FeatureKdTreePtr feature_tree_;
-      
-      /** \brief The polygonal correspondence rejector used for prerejection */
-      CorrespondenceRejectorPolyPtr correspondence_rejector_poly_;
-      
-      /** \brief The fraction [0,1] of inlier points required for accepting a transformation */
-      float inlier_fraction_;
-      
-      /** \brief Inlier points of final transformation as indices into source */
-      std::vector<int> inliers_;
+    return (nr_samples_);
+  }
+
+  /** \brief Set the number of neighbors to use when selecting a random feature
+   * correspondence.  A higher value will add more randomness to the feature matching.
+   * \param k the number of neighbors to use when selecting a random feature
+   * correspondence.
+   */
+  inline void
+  setCorrespondenceRandomness(int k)
+  {
+    k_correspondences_ = k;
+  }
+
+  /** \brief Get the number of neighbors used when selecting a random feature
+   * correspondence, as set by the user */
+  inline int
+  getCorrespondenceRandomness() const
+  {
+    return (k_correspondences_);
+  }
+
+  /** \brief Set the similarity threshold in [0,1[ between edge lengths of the
+   * underlying polygonal correspondence rejector object, where 1 is a perfect match
+   * \param similarity_threshold edge length similarity threshold
+   */
+  inline void
+  setSimilarityThreshold(float similarity_threshold)
+  {
+    correspondence_rejector_poly_->setSimilarityThreshold(similarity_threshold);
+  }
+
+  /** \brief Get the similarity threshold between edge lengths of the underlying
+   * polygonal correspondence rejector object, \return edge length similarity threshold
+   */
+  inline float
+  getSimilarityThreshold() const
+  {
+    return correspondence_rejector_poly_->getSimilarityThreshold();
+  }
+
+  /** \brief Set the required inlier fraction (of the input)
+   * \param inlier_fraction required inlier fraction, must be in [0,1]
+   */
+  inline void
+  setInlierFraction(float inlier_fraction)
+  {
+    inlier_fraction_ = inlier_fraction;
+  }
+
+  /** \brief Get the required inlier fraction
+   * \return required inlier fraction in [0,1]
+   */
+  inline float
+  getInlierFraction() const
+  {
+    return inlier_fraction_;
+  }
+
+  /** \brief Get the inlier indices of the source point cloud under the final
+   * transformation
+   * @return inlier indices
+   */
+  inline const pcl::Indices&
+  getInliers() const
+  {
+    return inliers_;
+  }
+
+protected:
+  /** \brief Choose a random index between 0 and n-1
+   * \param n the number of possible indices to choose from
+   */
+  inline int
+  getRandomIndex(int n) const
+  {
+    return (static_cast<int>(n * (rand() / (RAND_MAX + 1.0))));
   };
-}
+
+  /** \brief Select \a nr_samples sample points from cloud while making sure that their
+   * pairwise distances are greater than a user-defined minimum distance, \a
+   * min_sample_distance. \param cloud the input point cloud \param nr_samples the
+   * number of samples to select \param sample_indices the resulting sample indices
+   */
+  void
+  selectSamples(const PointCloudSource& cloud,
+                int nr_samples,
+                pcl::Indices& sample_indices);
+
+  /** \brief For each of the sample points, find a list of points in the target cloud
+   * whose features are similar to the sample points' features. From these, select one
+   * randomly which will be considered that sample point's correspondence. \param
+   * sample_indices the indices of each sample point \param similar_features
+   * correspondence cache, which is used to read/write already computed correspondences
+   * \param corresponding_indices the resulting indices of each sample's corresponding
+   * point in the target cloud
+   */
+  void
+  findSimilarFeatures(const pcl::Indices& sample_indices,
+                      std::vector<pcl::Indices>& similar_features,
+                      pcl::Indices& corresponding_indices);
+
+  /** \brief Rigid transformation computation method.
+   * \param output the transformed input point cloud dataset using the rigid
+   * transformation found \param guess The computed transformation
+   */
+  void
+  computeTransformation(PointCloudSource& output,
+                        const Eigen::Matrix4f& guess) override;
+
+  /** \brief Obtain the fitness of a transformation
+   * The following metrics are calculated, based on
+   * \b final_transformation_ and \b corr_dist_threshold_:
+   *   - Inliers: the number of transformed points which are closer than threshold to NN
+   *   - Error score: the MSE of the inliers
+   * \param inliers indices of source point cloud inliers
+   * \param fitness_score output fitness score as RMSE
+   */
+  void
+  getFitness(pcl::Indices& inliers, float& fitness_score);
+
+  /** \brief The source point cloud's feature descriptors. */
+  FeatureCloudConstPtr input_features_;
+
+  /** \brief The target point cloud's feature descriptors. */
+  FeatureCloudConstPtr target_features_;
+
+  /** \brief The number of samples to use during each iteration. */
+  int nr_samples_;
+
+  /** \brief The number of neighbors to use when selecting a random feature
+   * correspondence. */
+  int k_correspondences_;
+
+  /** \brief The KdTree used to compare feature descriptors. */
+  FeatureKdTreePtr feature_tree_;
+
+  /** \brief The polygonal correspondence rejector used for prerejection */
+  CorrespondenceRejectorPolyPtr correspondence_rejector_poly_;
+
+  /** \brief The fraction [0,1] of inlier points required for accepting a transformation
+   */
+  float inlier_fraction_;
+
+  /** \brief Inlier points of final transformation as indices into source */
+  pcl::Indices inliers_;
+};
+} // namespace pcl
 
 #include <pcl/registration/impl/sample_consensus_prerejective.hpp>
diff --git a/registration/include/pcl/registration/transformation_estimation.h b/registration/include/pcl/registration/transformation_estimation.h
index 14114441..d77c8931 100644
--- a/registration/include/pcl/registration/transformation_estimation.h
+++ b/registration/include/pcl/registration/transformation_estimation.h
@@ -40,88 +40,84 @@
 
 #pragma once
 
-#include <pcl/correspondence.h>
 #include <pcl/common/transforms.h>
 #include <pcl/registration/correspondence_types.h>
+#include <pcl/correspondence.h>
 
-namespace pcl
-{
-  namespace registration
-  {
-    /** \brief TransformationEstimation represents the base class for methods for transformation estimation based on:
-      *   - correspondence vectors
-      *   - two point clouds (source and target) of the same size
-      *   - a point cloud with a set of indices (source), and another point cloud (target)
-      *   - two point clouds with two sets of indices (source and target) of the same size
-      *
-      * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float.
-      * \author Dirk Holz, Radu B. Rusu
-      * \ingroup registration
-      */
-    template <typename PointSource, typename PointTarget, typename Scalar = float>
-    class TransformationEstimation
-    {
-      public:
-        using Matrix4 = Eigen::Matrix<Scalar, 4, 4>;
-
-        TransformationEstimation () {};
-        virtual ~TransformationEstimation () {};
+namespace pcl {
+namespace registration {
+/** \brief TransformationEstimation represents the base class for methods for
+ * transformation estimation based on:
+ *   - correspondence vectors
+ *   - two point clouds (source and target) of the same size
+ *   - a point cloud with a set of indices (source), and another point cloud (target)
+ *   - two point clouds with two sets of indices (source and target) of the same size
+ *
+ * \note The class is templated on the source and target point types as well as on the
+ * output scalar of the transformation matrix (i.e., float or double). Default: float.
+ * \author Dirk Holz, Radu B. Rusu
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename Scalar = float>
+class TransformationEstimation {
+public:
+  using Matrix4 = Eigen::Matrix<Scalar, 4, 4>;
 
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        virtual void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            Matrix4 &transformation_matrix) const = 0;
+  TransformationEstimation(){};
+  virtual ~TransformationEstimation(){};
 
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        virtual void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const std::vector<int> &indices_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            Matrix4 &transformation_matrix) const = 0;
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud. \param[in] cloud_src the source point cloud dataset \param[in]
+   * cloud_tgt the target point cloud dataset \param[out] transformation_matrix the
+   * resultant transformation matrix
+   */
+  virtual void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              Matrix4& transformation_matrix) const = 0;
 
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from \a indices_src
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        virtual void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const std::vector<int> &indices_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            const std::vector<int> &indices_tgt,
-            Matrix4 &transformation_matrix) const = 0;
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud. \param[in] cloud_src the source point cloud dataset \param[in]
+   * indices_src the vector of indices describing the points of interest in \a cloud_src
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  virtual void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::Indices& indices_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              Matrix4& transformation_matrix) const = 0;
 
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[in] correspondences the vector of correspondences between source and target point cloud
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        virtual void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            const pcl::Correspondences &correspondences,
-            Matrix4 &transformation_matrix) const = 0;
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud. \param[in] cloud_src the source point cloud dataset \param[in]
+   * indices_src the vector of indices describing the points of interest in \a cloud_src
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[in] indices_tgt the vector of indices describing the correspondences of the
+   * interest points from \a indices_src
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  virtual void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::Indices& indices_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              const pcl::Indices& indices_tgt,
+                              Matrix4& transformation_matrix) const = 0;
 
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud. \param[in] cloud_src the source point cloud dataset \param[in]
+   * cloud_tgt the target point cloud dataset \param[in] correspondences the vector of
+   * correspondences between source and target point cloud \param[out]
+   * transformation_matrix the resultant transformation matrix
+   */
+  virtual void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              const pcl::Correspondences& correspondences,
+                              Matrix4& transformation_matrix) const = 0;
 
-        using Ptr = shared_ptr<TransformationEstimation<PointSource, PointTarget, Scalar> >;
-        using ConstPtr = shared_ptr<const TransformationEstimation<PointSource, PointTarget, Scalar> >;
-    };
-  }
-}
+  using Ptr = shared_ptr<TransformationEstimation<PointSource, PointTarget, Scalar>>;
+  using ConstPtr =
+      shared_ptr<const TransformationEstimation<PointSource, PointTarget, Scalar>>;
+};
+} // namespace registration
+} // namespace pcl
diff --git a/registration/include/pcl/registration/transformation_estimation_2D.h b/registration/include/pcl/registration/transformation_estimation_2D.h
index 3ced4e0d..6772f872 100644
--- a/registration/include/pcl/registration/transformation_estimation_2D.h
+++ b/registration/include/pcl/registration/transformation_estimation_2D.h
@@ -40,114 +40,111 @@
 
 #include <pcl/registration/transformation_estimation.h>
 
-namespace pcl
-{
-  namespace registration
-  {
-    /** @b TransformationEstimation2D implements a simple 2D rigid transformation 
-      * estimation (x, y, theta) for a given pair of datasets. 
-      *
-      * The two datasets should already be transformed so that the reference plane 
-      * equals z = 0.
-      *
-      * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float.
-      *
-      * \author Suat Gedikli
-      * \ingroup registration
-      */
-    template <typename PointSource, typename PointTarget, typename Scalar = float>
-    class TransformationEstimation2D : public TransformationEstimation<PointSource, PointTarget, Scalar>
-    {
-      public:
-        using Ptr = shared_ptr<TransformationEstimation2D<PointSource, PointTarget, Scalar> >;
-        using ConstPtr = shared_ptr<const TransformationEstimation2D<PointSource, PointTarget, Scalar> >;
-
-        using Matrix4 = typename TransformationEstimation<PointSource, PointTarget, Scalar>::Matrix4;
+namespace pcl {
+namespace registration {
+/** @b TransformationEstimation2D implements a simple 2D rigid transformation
+ * estimation (x, y, theta) for a given pair of datasets.
+ *
+ * The two datasets should already be transformed so that the reference plane
+ * equals z = 0.
+ *
+ * \note The class is templated on the source and target point types as well as on the
+ * output scalar of the transformation matrix (i.e., float or double). Default: float.
+ *
+ * \author Suat Gedikli
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename Scalar = float>
+class TransformationEstimation2D
+: public TransformationEstimation<PointSource, PointTarget, Scalar> {
+public:
+  using Ptr = shared_ptr<TransformationEstimation2D<PointSource, PointTarget, Scalar>>;
+  using ConstPtr =
+      shared_ptr<const TransformationEstimation2D<PointSource, PointTarget, Scalar>>;
 
-        TransformationEstimation2D () {};
-        virtual ~TransformationEstimation2D () {};
+  using Matrix4 =
+      typename TransformationEstimation<PointSource, PointTarget, Scalar>::Matrix4;
 
-        /** \brief Estimate a rigid transformation between a source and a target point cloud in 2D.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            Matrix4 &transformation_matrix) const;
+  TransformationEstimation2D(){};
+  virtual ~TransformationEstimation2D(){};
 
-        /** \brief Estimate a rigid transformation between a source and a target point cloud in 2D.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const std::vector<int> &indices_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            Matrix4 &transformation_matrix) const;
+  /** \brief Estimate a rigid transformation between a source and a target point cloud
+   * in 2D. \param[in] cloud_src the source point cloud dataset \param[in] cloud_tgt the
+   * target point cloud dataset \param[out] transformation_matrix the resultant
+   * transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              Matrix4& transformation_matrix) const;
 
-        /** \brief Estimate a rigid transformation between a source and a target point cloud in 2D.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from \a indices_src
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        virtual void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const std::vector<int> &indices_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            const std::vector<int> &indices_tgt,
-            Matrix4 &transformation_matrix) const;
+  /** \brief Estimate a rigid transformation between a source and a target point cloud
+   * in 2D. \param[in] cloud_src the source point cloud dataset \param[in] indices_src
+   * the vector of indices describing the points of interest in \a cloud_src
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::Indices& indices_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              Matrix4& transformation_matrix) const;
 
-        /** \brief Estimate a rigid transformation between a source and a target point cloud in 2D.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[in] correspondences the vector of correspondences between source and target point cloud
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        virtual void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            const pcl::Correspondences &correspondences,
-            Matrix4 &transformation_matrix) const;
+  /** \brief Estimate a rigid transformation between a source and a target point cloud
+   * in 2D. \param[in] cloud_src the source point cloud dataset \param[in] indices_src
+   * the vector of indices describing the points of interest in \a cloud_src
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[in] indices_tgt the vector of  indices describing the correspondences of the
+   * interest points from \a indices_src
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  virtual void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::Indices& indices_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              const pcl::Indices& indices_tgt,
+                              Matrix4& transformation_matrix) const;
 
-      protected:
+  /** \brief Estimate a rigid transformation between a source and a target point cloud
+   * in 2D. \param[in] cloud_src the source point cloud dataset \param[in] cloud_tgt the
+   * target point cloud dataset \param[in] correspondences the vector of correspondences
+   * between source and target point cloud \param[out] transformation_matrix the
+   * resultant transformation matrix
+   */
+  virtual void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              const pcl::Correspondences& correspondences,
+                              Matrix4& transformation_matrix) const;
 
-        /** \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<PointSource>& source_it,
-                                     ConstCloudIterator<PointTarget>& target_it,
-                                     Matrix4 &transformation_matrix) const;
+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<PointSource>& source_it,
+                              ConstCloudIterator<PointTarget>& target_it,
+                              Matrix4& transformation_matrix) const;
 
-        /** \brief Obtain a 4x4 rigid transformation matrix from a correlation matrix H = src * tgt'
-          * \param[in] cloud_src_demean the input source cloud, demeaned, in Eigen format
-          * \param[in] centroid_src the input source centroid, in Eigen format
-          * \param[in] cloud_tgt_demean the input target cloud, demeaned, in Eigen format
-          * \param[in] centroid_tgt the input target cloud, in Eigen format
-          * \param[out] transformation_matrix the resultant 4x4 rigid transformation matrix
-          */ 
-        void
-        getTransformationFromCorrelation (
-            const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_src_demean,
-            const Eigen::Matrix<Scalar, 4, 1> &centroid_src,
-            const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean,
-            const Eigen::Matrix<Scalar, 4, 1> &centroid_tgt,
-            Matrix4 &transformation_matrix) const;
-    };
+  /** \brief Obtain a 4x4 rigid transformation matrix from a correlation matrix H = src
+   * * tgt' \param[in] cloud_src_demean the input source cloud, demeaned, in Eigen
+   * format \param[in] centroid_src the input source centroid, in Eigen format
+   * \param[in] cloud_tgt_demean the input target cloud, demeaned, in Eigen format
+   * \param[in] centroid_tgt the input target cloud, in Eigen format
+   * \param[out] transformation_matrix the resultant 4x4 rigid transformation matrix
+   */
+  void
+  getTransformationFromCorrelation(
+      const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_src_demean,
+      const Eigen::Matrix<Scalar, 4, 1>& centroid_src,
+      const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_tgt_demean,
+      const Eigen::Matrix<Scalar, 4, 1>& centroid_tgt,
+      Matrix4& transformation_matrix) const;
+};
 
-  }
-}
+} // namespace registration
+} // namespace pcl
 
 #include <pcl/registration/impl/transformation_estimation_2D.hpp>
diff --git a/registration/include/pcl/registration/transformation_estimation_3point.h b/registration/include/pcl/registration/transformation_estimation_3point.h
index f9b80295..2441fd93 100644
--- a/registration/include/pcl/registration/transformation_estimation_3point.h
+++ b/registration/include/pcl/registration/transformation_estimation_3point.h
@@ -39,105 +39,101 @@
 
 #include <pcl/registration/transformation_estimation.h>
 
-namespace pcl
-{
-  namespace registration
-  {
-    /** \brief TransformationEstimation3Points represents the class for transformation estimation based on:
-      *   - correspondence vectors of 3 pairs (planar case)
-      *   - two point clouds (source and target) of size 3
-      *   - a point cloud with a set of 3 indices (source), and another point cloud (target)
-      *   - two point clouds with two sets of indices (source and target) of the size 3
-      *
-      * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix 
-      * (i.e., float or double). Default: float.
-      *
-      * \author P.W.Theiler
-      * \ingroup registration
-      */
-    template <typename PointSource, typename PointTarget, typename Scalar = float>
-    class TransformationEstimation3Point : public TransformationEstimation <PointSource, PointTarget, Scalar>
-    {
-      public:
-        /** \cond */
-        using Ptr = shared_ptr<TransformationEstimation3Point<PointSource, PointTarget, Scalar> >;
-        using ConstPtr = shared_ptr<const TransformationEstimation3Point<PointSource, PointTarget, Scalar> >;        
-        using Matrix4 = typename TransformationEstimation<PointSource, PointTarget, Scalar>::Matrix4;
-        /** \endcond */
-
-        /** \brief Constructor */
-        TransformationEstimation3Point () 
-        {};
+namespace pcl {
+namespace registration {
+/** \brief TransformationEstimation3Points represents the class for transformation
+ * estimation based on:
+ *   - correspondence vectors of 3 pairs (planar case)
+ *   - two point clouds (source and target) of size 3
+ *   - a point cloud with a set of 3 indices (source), and another point cloud (target)
+ *   - two point clouds with two sets of indices (source and target) of the size 3
+ *
+ * \note The class is templated on the source and target point types as well as on the
+ * output scalar of the transformation matrix (i.e., float or double). Default: float.
+ *
+ * \author P.W.Theiler
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename Scalar = float>
+class TransformationEstimation3Point
+: public TransformationEstimation<PointSource, PointTarget, Scalar> {
+public:
+  /** \cond */
+  using Ptr =
+      shared_ptr<TransformationEstimation3Point<PointSource, PointTarget, Scalar>>;
+  using ConstPtr = shared_ptr<
+      const TransformationEstimation3Point<PointSource, PointTarget, Scalar>>;
+  using Matrix4 =
+      typename TransformationEstimation<PointSource, PointTarget, Scalar>::Matrix4;
+  /** \endcond */
 
-        /** \brief Destructor */
-        ~TransformationEstimation3Point () 
-        {};
+  /** \brief Constructor */
+  TransformationEstimation3Point(){};
 
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            Matrix4 &transformation_matrix) const override;
+  /** \brief Destructor */
+  ~TransformationEstimation3Point(){};
 
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const std::vector<int> &indices_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            Matrix4 &transformation_matrix) const override;
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud. \param[in] cloud_src the source point cloud dataset \param[in]
+   * cloud_tgt the target point cloud dataset \param[out] transformation_matrix the
+   * resultant transformation matrix
+   */
+  void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              Matrix4& transformation_matrix) const override;
 
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from \a indices_src
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const std::vector<int> &indices_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            const std::vector<int> &indices_tgt,
-            Matrix4 &transformation_matrix) const override;
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud. \param[in] cloud_src the source point cloud dataset \param[in]
+   * indices_src the vector of indices describing the points of interest in \a cloud_src
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::Indices& indices_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              Matrix4& transformation_matrix) const override;
 
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[in] correspondences the vector of correspondences between source and target point cloud
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            const pcl::Correspondences &correspondences,
-            Matrix4 &transformation_matrix) const override;
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud. \param[in] cloud_src the source point cloud dataset \param[in]
+   * indices_src the vector of indices describing the points of interest in \a cloud_src
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[in] indices_tgt the vector of indices describing the correspondences of the
+   * interest points from \a indices_src
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::Indices& indices_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              const pcl::Indices& indices_tgt,
+                              Matrix4& transformation_matrix) const override;
 
-        protected:
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud. \param[in] cloud_src the source point cloud dataset \param[in]
+   * cloud_tgt the target point cloud dataset \param[in] correspondences the vector of
+   * correspondences between source and target point cloud \param[out]
+   * transformation_matrix the resultant transformation matrix
+   */
+  void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              const pcl::Correspondences& correspondences,
+                              Matrix4& transformation_matrix) const override;
 
-        /** \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<PointSource>& source_it,
-                                     ConstCloudIterator<PointTarget>& target_it,
-                                     Matrix4 &transformation_matrix) const;
-    };
-  }; // namespace registration  
-}; // namespace registration  
+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<PointSource>& source_it,
+                              ConstCloudIterator<PointTarget>& target_it,
+                              Matrix4& transformation_matrix) const;
+};
+}; // namespace registration
+}; // namespace pcl
 
 #include <pcl/registration/impl/transformation_estimation_3point.hpp>
diff --git a/registration/include/pcl/registration/transformation_estimation_dq.h b/registration/include/pcl/registration/transformation_estimation_dq.h
index 40077579..955fcfa2 100644
--- a/registration/include/pcl/registration/transformation_estimation_dq.h
+++ b/registration/include/pcl/registration/transformation_estimation_dq.h
@@ -41,100 +41,99 @@
 
 #include <pcl/registration/transformation_estimation.h>
 #include <pcl/cloud_iterator.h>
+PCL_DEPRECATED_HEADER(1,
+                      15,
+                      "TransformationEstimationDQ has been renamed to "
+                      "TransformationEstimationDualQuaternion.");
 
-namespace pcl
-{
-  namespace registration
-  {
-    /** @b TransformationEstimationDQ implements dual quaternion based estimation of
-      * the transformation aligning the given correspondences.
-      *
-      * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float.
-      * \author Sergey Zagoruyko
-      * \ingroup registration
-      */
-    template <typename PointSource, typename PointTarget, typename Scalar = float>
-    class TransformationEstimationDQ : public TransformationEstimation<PointSource, PointTarget, Scalar>
-    {
-      public:
-        using Ptr = shared_ptr<TransformationEstimationDQ<PointSource, PointTarget, Scalar> >;
-        using ConstPtr = shared_ptr<const TransformationEstimationDQ<PointSource, PointTarget, Scalar> >;
-
-        using Matrix4 = typename TransformationEstimation<PointSource, PointTarget, Scalar>::Matrix4;
+namespace pcl {
+namespace registration {
+/** @b TransformationEstimationDQ implements dual quaternion based estimation of
+ * the transformation aligning the given correspondences.
+ *
+ * \note The class is templated on the source and target point types as well as on the
+ * output scalar of the transformation matrix (i.e., float or double). Default: float.
+ * \author Sergey Zagoruyko
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename Scalar = float>
+class TransformationEstimationDQ
+: public TransformationEstimation<PointSource, PointTarget, Scalar> {
+public:
+  using Ptr = shared_ptr<TransformationEstimationDQ<PointSource, PointTarget, Scalar>>;
+  using ConstPtr =
+      shared_ptr<const TransformationEstimationDQ<PointSource, PointTarget, Scalar>>;
 
-        TransformationEstimationDQ () {};
-        virtual ~TransformationEstimationDQ () {};
+  using Matrix4 =
+      typename TransformationEstimation<PointSource, PointTarget, Scalar>::Matrix4;
 
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using
-          * dual quaternion optimization
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            Matrix4 &transformation_matrix) const;
+  TransformationEstimationDQ(){};
+  virtual ~TransformationEstimationDQ(){};
 
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using
-          * dual quaternion optimization
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const std::vector<int> &indices_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            Matrix4 &transformation_matrix) const;
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using dual quaternion optimization \param[in] cloud_src the source
+   * point cloud dataset \param[in] cloud_tgt the target point cloud dataset \param[out]
+   * transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              Matrix4& transformation_matrix) const;
 
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using
-          * dual quaternion optimization
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from \a indices_src
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const std::vector<int> &indices_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            const std::vector<int> &indices_tgt,
-            Matrix4 &transformation_matrix) const;
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using dual quaternion optimization \param[in] cloud_src the source
+   * point cloud dataset \param[in] indices_src the vector of indices describing the
+   * points of interest in \a cloud_src
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::Indices& indices_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              Matrix4& transformation_matrix) const;
 
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using
-          * dual quaternion optimization
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[in] correspondences the vector of correspondences between source and target point cloud
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            const pcl::Correspondences &correspondences,
-            Matrix4 &transformation_matrix) const;
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using dual quaternion optimization \param[in] cloud_src the source
+   * point cloud dataset \param[in] indices_src the vector of indices describing the
+   * points of interest in \a cloud_src
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[in] indices_tgt the vector of indices describing the correspondences of the
+   * interest points from \a indices_src
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::Indices& indices_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              const pcl::Indices& indices_tgt,
+                              Matrix4& transformation_matrix) const;
 
-      protected:
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using dual quaternion optimization \param[in] cloud_src the source
+   * point cloud dataset \param[in] cloud_tgt the target point cloud dataset \param[in]
+   * correspondences the vector of correspondences between source and target point cloud
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              const pcl::Correspondences& correspondences,
+                              Matrix4& transformation_matrix) const;
 
-        /** \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<PointSource>& source_it,
-                                     ConstCloudIterator<PointTarget>& target_it,
-                                     Matrix4 &transformation_matrix) const;
-     };
+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<PointSource>& source_it,
+                              ConstCloudIterator<PointTarget>& target_it,
+                              Matrix4& transformation_matrix) const;
+};
 
-  }
-}
+} // namespace registration
+} // namespace pcl
 
 #include <pcl/registration/impl/transformation_estimation_dq.hpp>
diff --git a/registration/include/pcl/registration/transformation_estimation_dual_quaternion.h b/registration/include/pcl/registration/transformation_estimation_dual_quaternion.h
index 1da67c8c..2b154120 100644
--- a/registration/include/pcl/registration/transformation_estimation_dual_quaternion.h
+++ b/registration/include/pcl/registration/transformation_estimation_dual_quaternion.h
@@ -42,99 +42,95 @@
 #include <pcl/registration/transformation_estimation.h>
 #include <pcl/cloud_iterator.h>
 
-namespace pcl
-{
-  namespace registration
-  {
-    /** @b TransformationEstimationDualQuaternion implements dual quaternion based estimation of
-      * the transformation aligning the given correspondences.
-      *
-      * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float.
-      * \author Sergey Zagoruyko
-      * \ingroup registration
-      */
-    template <typename PointSource, typename PointTarget, typename Scalar = float>
-    class TransformationEstimationDualQuaternion : public TransformationEstimation<PointSource, PointTarget, Scalar>
-    {
-      public:
-        using Ptr = shared_ptr<TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar> >;
-        using ConstPtr = shared_ptr<const TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar> >;
-
-        using Matrix4 = typename TransformationEstimation<PointSource, PointTarget, Scalar>::Matrix4;
+namespace pcl {
+namespace registration {
+/** @b TransformationEstimationDualQuaternion implements dual quaternion based
+ * estimation of the transformation aligning the given correspondences.
+ *
+ * \note The class is templated on the source and target point types as well as on the
+ * output scalar of the transformation matrix (i.e., float or double). Default: float.
+ * \author Sergey Zagoruyko
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename Scalar = float>
+class TransformationEstimationDualQuaternion
+: public TransformationEstimation<PointSource, PointTarget, Scalar> {
+public:
+  using Ptr = shared_ptr<
+      TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar>>;
+  using ConstPtr = shared_ptr<
+      const TransformationEstimationDualQuaternion<PointSource, PointTarget, Scalar>>;
 
-        TransformationEstimationDualQuaternion () {};
-        ~TransformationEstimationDualQuaternion () {};
+  using Matrix4 =
+      typename TransformationEstimation<PointSource, PointTarget, Scalar>::Matrix4;
 
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using
-          * dual quaternion optimization
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            Matrix4 &transformation_matrix) const override;
+  TransformationEstimationDualQuaternion(){};
+  ~TransformationEstimationDualQuaternion(){};
 
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using
-          * dual quaternion optimization
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const std::vector<int> &indices_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            Matrix4 &transformation_matrix) const override;
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using dual quaternion optimization \param[in] cloud_src the source
+   * point cloud dataset \param[in] cloud_tgt the target point cloud dataset \param[out]
+   * transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              Matrix4& transformation_matrix) const override;
 
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using
-          * dual quaternion optimization
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from \a indices_src
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const std::vector<int> &indices_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            const std::vector<int> &indices_tgt,
-            Matrix4 &transformation_matrix) const override;
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using dual quaternion optimization \param[in] cloud_src the source
+   * point cloud dataset \param[in] indices_src the vector of indices describing the
+   * points of interest in \a cloud_src
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::Indices& indices_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              Matrix4& transformation_matrix) const override;
 
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using
-          * dual quaternion optimization
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[in] correspondences the vector of correspondences between source and target point cloud
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            const pcl::Correspondences &correspondences,
-            Matrix4 &transformation_matrix) const override;
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using dual quaternion optimization \param[in] cloud_src the source
+   * point cloud dataset \param[in] indices_src the vector of indices describing the
+   * points of interest in \a cloud_src
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[in] indices_tgt the vector of indices describing the correspondences of the
+   * interest points from \a indices_src
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::Indices& indices_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              const pcl::Indices& indices_tgt,
+                              Matrix4& transformation_matrix) const override;
 
-      protected:
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using dual quaternion optimization \param[in] cloud_src the source
+   * point cloud dataset \param[in] cloud_tgt the target point cloud dataset \param[in]
+   * correspondences the vector of correspondences between source and target point cloud
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              const pcl::Correspondences& correspondences,
+                              Matrix4& transformation_matrix) const override;
 
-        /** \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<PointSource>& source_it,
-                                     ConstCloudIterator<PointTarget>& target_it,
-                                     Matrix4 &transformation_matrix) const;
-     };
+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<PointSource>& source_it,
+                              ConstCloudIterator<PointTarget>& target_it,
+                              Matrix4& transformation_matrix) const;
+};
 
-  }
-}
+} // namespace registration
+} // namespace pcl
 
 #include <pcl/registration/impl/transformation_estimation_dual_quaternion.hpp>
diff --git a/registration/include/pcl/registration/transformation_estimation_lm.h b/registration/include/pcl/registration/transformation_estimation_lm.h
index 9d525ac9..b2a0e185 100644
--- a/registration/include/pcl/registration/transformation_estimation_lm.h
+++ b/registration/include/pcl/registration/transformation_estimation_lm.h
@@ -40,314 +40,311 @@
 
 #pragma once
 
-#include <pcl/memory.h>
 #include <pcl/registration/transformation_estimation.h>
 #include <pcl/registration/warp_point_rigid.h>
+#include <pcl/memory.h>
 
-namespace pcl
-{
-  namespace registration
+namespace pcl {
+namespace registration {
+/** @b TransformationEstimationLM implements Levenberg Marquardt-based
+ * estimation of the transformation aligning the given correspondences.
+ *
+ * \note The class is templated on the source and target point types as well as on the
+ * output scalar of the transformation matrix (i.e., float or double). Default: float.
+ * \author Radu B. Rusu
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename MatScalar = float>
+class TransformationEstimationLM
+: public TransformationEstimation<PointSource, PointTarget, MatScalar> {
+  using PointCloudSource = pcl::PointCloud<PointSource>;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+
+  using PointCloudTarget = pcl::PointCloud<PointTarget>;
+
+  using PointIndicesPtr = PointIndices::Ptr;
+  using PointIndicesConstPtr = PointIndices::ConstPtr;
+
+public:
+  using Ptr =
+      shared_ptr<TransformationEstimationLM<PointSource, PointTarget, MatScalar>>;
+  using ConstPtr =
+      shared_ptr<const TransformationEstimationLM<PointSource, PointTarget, MatScalar>>;
+
+  using VectorX = Eigen::Matrix<MatScalar, Eigen::Dynamic, 1>;
+  using Vector4 = Eigen::Matrix<MatScalar, 4, 1>;
+  using Matrix4 =
+      typename TransformationEstimation<PointSource, PointTarget, MatScalar>::Matrix4;
+
+  /** \brief Constructor. */
+  TransformationEstimationLM();
+
+  /** \brief Copy constructor.
+   * \param[in] src the TransformationEstimationLM object to copy into this
+   */
+  TransformationEstimationLM(const TransformationEstimationLM& src)
+  : tmp_src_(src.tmp_src_)
+  , tmp_tgt_(src.tmp_tgt_)
+  , tmp_idx_src_(src.tmp_idx_src_)
+  , tmp_idx_tgt_(src.tmp_idx_tgt_)
+  , warp_point_(src.warp_point_){};
+
+  /** \brief Copy operator.
+   * \param[in] src the TransformationEstimationLM object to copy into this
+   */
+  TransformationEstimationLM&
+  operator=(const TransformationEstimationLM& src)
   {
-    /** @b TransformationEstimationLM implements Levenberg Marquardt-based
-      * estimation of the transformation aligning the given correspondences.
-      *
-      * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float.
-      * \author Radu B. Rusu
-      * \ingroup registration
-      */
-    template <typename PointSource, typename PointTarget, typename MatScalar = float>
-    class TransformationEstimationLM : public TransformationEstimation<PointSource, PointTarget, MatScalar>
-    {
-      using PointCloudSource = pcl::PointCloud<PointSource>;
-      using PointCloudSourcePtr = typename PointCloudSource::Ptr;
-      using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
-
-      using PointCloudTarget = pcl::PointCloud<PointTarget>;
-
-      using PointIndicesPtr = PointIndices::Ptr;
-      using PointIndicesConstPtr = PointIndices::ConstPtr;
-
-      public:
-        using Ptr = shared_ptr<TransformationEstimationLM<PointSource, PointTarget, MatScalar> >;
-        using ConstPtr = shared_ptr<const TransformationEstimationLM<PointSource, PointTarget, MatScalar> >;
-
-        using VectorX = Eigen::Matrix<MatScalar, Eigen::Dynamic, 1>;
-        using Vector4 = Eigen::Matrix<MatScalar, 4, 1>;
-        using Matrix4 = typename TransformationEstimation<PointSource, PointTarget, MatScalar>::Matrix4;
-        
-        /** \brief Constructor. */
-        TransformationEstimationLM ();
-
-        /** \brief Copy constructor. 
-          * \param[in] src the TransformationEstimationLM object to copy into this 
-          */
-        TransformationEstimationLM (const TransformationEstimationLM &src) : 
-          tmp_src_ (src.tmp_src_), 
-          tmp_tgt_ (src.tmp_tgt_), 
-          tmp_idx_src_ (src.tmp_idx_src_), 
-          tmp_idx_tgt_ (src.tmp_idx_tgt_), 
-          warp_point_ (src.warp_point_)
-        {};
-
-        /** \brief Copy operator. 
-          * \param[in] src the TransformationEstimationLM object to copy into this 
-          */
-        TransformationEstimationLM&
-        operator = (const TransformationEstimationLM &src)
-        {
-          tmp_src_ = src.tmp_src_; 
-          tmp_tgt_ = src.tmp_tgt_; 
-          tmp_idx_src_ = src.tmp_idx_src_;
-          tmp_idx_tgt_ = src.tmp_idx_tgt_; 
-          warp_point_ = src.warp_point_;
-        }
-
-         /** \brief Destructor. */
-        ~TransformationEstimationLM () {};
-
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            Matrix4 &transformation_matrix) const override;
-
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const std::vector<int> &indices_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            Matrix4 &transformation_matrix) const override;
-
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from 
-          * \a indices_src
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const std::vector<int> &indices_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            const std::vector<int> &indices_tgt,
-            Matrix4 &transformation_matrix) const override;
-
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[in] correspondences the vector of correspondences between source and target point cloud
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            const pcl::Correspondences &correspondences,
-            Matrix4 &transformation_matrix) const override;
-
-        /** \brief Set the function we use to warp points. Defaults to rigid 6D warp.
-          * \param[in] warp_fcn a shared pointer to an object that warps points
-          */
-        void
-        setWarpFunction (const typename WarpPointRigid<PointSource, PointTarget, MatScalar>::Ptr &warp_fcn)
-        {
-          warp_point_ = warp_fcn;
-        }
-
-      protected:
-        /** \brief Compute the distance between a source point and its corresponding target point
-          * \param[in] p_src The source point
-          * \param[in] p_tgt The target point
-          * \return The distance between \a p_src and \a p_tgt
-          *
-          * \note Older versions of PCL used this method internally for calculating the
-          * optimization gradient. Since PCL 1.7, a switch has been made to the 
-          * computeDistance method using Vector4 types instead. This method is only 
-          * kept for API compatibility reasons.
-          */
-        virtual MatScalar
-        computeDistance (const PointSource &p_src, const PointTarget &p_tgt) const
-        {
-          Vector4 s (p_src.x, p_src.y, p_src.z, 0);
-          Vector4 t (p_tgt.x, p_tgt.y, p_tgt.z, 0);
-          return ((s - t).norm ());
-        }
-
-        /** \brief Compute the distance between a source point and its corresponding target point
-          * \param[in] p_src The source point
-          * \param[in] p_tgt The target point
-          * \return The distance between \a p_src and \a p_tgt
-          *
-          * \note A different distance function can be defined by creating a subclass of 
-          * TransformationEstimationLM and overriding this method. 
-          * (See \a TransformationEstimationPointToPlane)
-          */
-        virtual MatScalar
-        computeDistance (const Vector4 &p_src, const PointTarget &p_tgt) const
-        {
-          Vector4 t (p_tgt.x, p_tgt.y, p_tgt.z, 0);
-          return ((p_src - t).norm ());
-        }
-
-        /** \brief Temporary pointer to the source dataset. */
-        mutable const PointCloudSource *tmp_src_;
-
-        /** \brief Temporary pointer to the target dataset. */
-        mutable const PointCloudTarget  *tmp_tgt_;
-
-        /** \brief Temporary pointer to the source dataset indices. */
-        mutable const std::vector<int> *tmp_idx_src_;
-
-        /** \brief Temporary pointer to the target dataset indices. */
-        mutable const std::vector<int> *tmp_idx_tgt_;
-
-        /** \brief The parameterized function used to warp the source to the target. */
-        typename pcl::registration::WarpPointRigid<PointSource, PointTarget, MatScalar>::Ptr warp_point_;
-        
-        /** Base functor all the models that need non linear optimization must
-          * define their own one and implement operator() (const Eigen::VectorXd& x, Eigen::VectorXd& fvec)
-          * or operator() (const Eigen::VectorXf& x, Eigen::VectorXf& fvec) depending on the chosen _Scalar
-          */
-        template<typename _Scalar, int NX=Eigen::Dynamic, int NY=Eigen::Dynamic>
-        struct Functor
-        {
-          using Scalar = _Scalar;
-          enum 
-          {
-            InputsAtCompileTime = NX,
-            ValuesAtCompileTime = NY
-          };
-          using InputType = Eigen::Matrix<_Scalar,InputsAtCompileTime,1>;
-          using ValueType = Eigen::Matrix<_Scalar,ValuesAtCompileTime,1>;
-          using JacobianType = Eigen::Matrix<_Scalar,ValuesAtCompileTime,InputsAtCompileTime>;
-
-          /** \brief Empty Constructor. */
-          Functor () : m_data_points_ (ValuesAtCompileTime) {}
-
-          /** \brief Constructor
-            * \param[in] m_data_points number of data points to evaluate.
-            */
-          Functor (int m_data_points) : m_data_points_ (m_data_points) {}
-        
-          /** \brief Destructor. */
-          virtual ~Functor () {}
-
-          /** \brief Get the number of values. */ 
-          int
-          values () const { return (m_data_points_); }
-
-          protected:
-            int m_data_points_;
-        };
-
-        struct OptimizationFunctor : public Functor<MatScalar>
-        {
-          using Functor<MatScalar>::values;
-
-          /** Functor constructor
-            * \param[in] m_data_points the number of data points to evaluate
-            * \param[in,out] estimator pointer to the estimator object
-            */
-          OptimizationFunctor (int m_data_points, 
-                               const TransformationEstimationLM *estimator) 
-            :  Functor<MatScalar> (m_data_points), estimator_ (estimator) 
-          {}
-
-          /** Copy constructor
-            * \param[in] src the optimization functor to copy into this
-            */
-          inline OptimizationFunctor (const OptimizationFunctor &src) : 
-            Functor<MatScalar> (src.m_data_points_), estimator_ ()
-          {
-            *this = src;
-          }
-
-          /** Copy operator
-            * \param[in] src the optimization functor to copy into this
-            */
-          inline OptimizationFunctor& 
-          operator = (const OptimizationFunctor &src) 
-          { 
-            Functor<MatScalar>::operator=(src);
-            estimator_ = src.estimator_; 
-            return (*this); 
-          }
-
-          /** \brief Destructor. */
-          ~OptimizationFunctor () {}
-
-          /** Fill fvec from x. For the current state vector x fill the f values
-            * \param[in] x state vector
-            * \param[out] fvec f values vector
-            */
-          int 
-          operator () (const VectorX &x, VectorX &fvec) const;
-
-          const TransformationEstimationLM<PointSource, PointTarget, MatScalar> *estimator_;
-        };
-
-        struct OptimizationFunctorWithIndices : public Functor<MatScalar>
-        {
-          using Functor<MatScalar>::values;
-
-          /** Functor constructor
-            * \param[in] m_data_points the number of data points to evaluate
-            * \param[in,out] estimator pointer to the estimator object
-            */
-          OptimizationFunctorWithIndices (int m_data_points, 
-                                          const TransformationEstimationLM *estimator) 
-            : Functor<MatScalar> (m_data_points), estimator_ (estimator) 
-          {}
-
-          /** Copy constructor
-            * \param[in] src the optimization functor to copy into this
-            */
-          inline OptimizationFunctorWithIndices (const OptimizationFunctorWithIndices &src)
-            : Functor<MatScalar> (src.m_data_points_), estimator_ ()
-          {
-            *this = src;
-          }
-
-          /** Copy operator
-            * \param[in] src the optimization functor to copy into this
-            */
-          inline OptimizationFunctorWithIndices& 
-          operator = (const OptimizationFunctorWithIndices &src) 
-          { 
-            Functor<MatScalar>::operator=(src);
-            estimator_ = src.estimator_; 
-            return (*this); 
-          }
-
-          /** \brief Destructor. */
-          ~OptimizationFunctorWithIndices () {}
-
-          /** Fill fvec from x. For the current state vector x fill the f values
-            * \param[in] x state vector
-            * \param[out] fvec f values vector
-            */
-          int 
-          operator () (const VectorX &x, VectorX &fvec) const;
-
-          const TransformationEstimationLM<PointSource, PointTarget, MatScalar> *estimator_;
-        };
-      public:
-        PCL_MAKE_ALIGNED_OPERATOR_NEW
-    };
+    tmp_src_ = src.tmp_src_;
+    tmp_tgt_ = src.tmp_tgt_;
+    tmp_idx_src_ = src.tmp_idx_src_;
+    tmp_idx_tgt_ = src.tmp_idx_tgt_;
+    warp_point_ = src.warp_point_;
+  }
+
+  /** \brief Destructor. */
+  ~TransformationEstimationLM(){};
+
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using LM. \param[in] cloud_src the source point cloud dataset
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              Matrix4& transformation_matrix) const override;
+
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using LM. \param[in] cloud_src the source point cloud dataset
+   * \param[in] indices_src the vector of indices describing the points of interest in
+   * \a cloud_src
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::Indices& indices_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              Matrix4& transformation_matrix) const override;
+
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using LM. \param[in] cloud_src the source point cloud dataset
+   * \param[in] indices_src the vector of indices describing the points of interest in
+   * \a cloud_src
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[in] indices_tgt the vector of indices describing the correspondences of the
+   * interest points from \a indices_src
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::Indices& indices_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              const pcl::Indices& indices_tgt,
+                              Matrix4& transformation_matrix) const override;
+
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using LM. \param[in] cloud_src the source point cloud dataset
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[in] correspondences the vector of correspondences between source and target
+   * point cloud \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              const pcl::Correspondences& correspondences,
+                              Matrix4& transformation_matrix) const override;
+
+  /** \brief Set the function we use to warp points. Defaults to rigid 6D warp.
+   * \param[in] warp_fcn a shared pointer to an object that warps points
+   */
+  void
+  setWarpFunction(
+      const typename WarpPointRigid<PointSource, PointTarget, MatScalar>::Ptr& warp_fcn)
+  {
+    warp_point_ = warp_fcn;
+  }
+
+protected:
+  /** \brief Compute the distance between a source point and its corresponding target
+   * point \param[in] p_src The source point \param[in] p_tgt The target point \return
+   * The distance between \a p_src and \a p_tgt
+   *
+   * \note Older versions of PCL used this method internally for calculating the
+   * optimization gradient. Since PCL 1.7, a switch has been made to the
+   * computeDistance method using Vector4 types instead. This method is only
+   * kept for API compatibility reasons.
+   */
+  virtual MatScalar
+  computeDistance(const PointSource& p_src, const PointTarget& p_tgt) const
+  {
+    Vector4 s(p_src.x, p_src.y, p_src.z, 0);
+    Vector4 t(p_tgt.x, p_tgt.y, p_tgt.z, 0);
+    return ((s - t).norm());
   }
-}
+
+  /** \brief Compute the distance between a source point and its corresponding target
+   * point \param[in] p_src The source point \param[in] p_tgt The target point \return
+   * The distance between \a p_src and \a p_tgt
+   *
+   * \note A different distance function can be defined by creating a subclass of
+   * TransformationEstimationLM and overriding this method.
+   * (See \a TransformationEstimationPointToPlane)
+   */
+  virtual MatScalar
+  computeDistance(const Vector4& p_src, const PointTarget& p_tgt) const
+  {
+    Vector4 t(p_tgt.x, p_tgt.y, p_tgt.z, 0);
+    return ((p_src - t).norm());
+  }
+
+  /** \brief Temporary pointer to the source dataset. */
+  mutable const PointCloudSource* tmp_src_;
+
+  /** \brief Temporary pointer to the target dataset. */
+  mutable const PointCloudTarget* tmp_tgt_;
+
+  /** \brief Temporary pointer to the source dataset indices. */
+  mutable const pcl::Indices* tmp_idx_src_;
+
+  /** \brief Temporary pointer to the target dataset indices. */
+  mutable const pcl::Indices* tmp_idx_tgt_;
+
+  /** \brief The parameterized function used to warp the source to the target. */
+  typename pcl::registration::WarpPointRigid<PointSource, PointTarget, MatScalar>::Ptr
+      warp_point_;
+
+  /** Base functor all the models that need non linear optimization must
+   * define their own one and implement operator() (const Eigen::VectorXd& x,
+   * Eigen::VectorXd& fvec) or operator() (const Eigen::VectorXf& x, Eigen::VectorXf&
+   * fvec) depending on the chosen _Scalar
+   */
+  template <typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
+  struct Functor {
+    using Scalar = _Scalar;
+    enum { InputsAtCompileTime = NX, ValuesAtCompileTime = NY };
+    using InputType = Eigen::Matrix<_Scalar, InputsAtCompileTime, 1>;
+    using ValueType = Eigen::Matrix<_Scalar, ValuesAtCompileTime, 1>;
+    using JacobianType =
+        Eigen::Matrix<_Scalar, ValuesAtCompileTime, InputsAtCompileTime>;
+
+    /** \brief Empty Constructor. */
+    Functor() : m_data_points_(ValuesAtCompileTime) {}
+
+    /** \brief Constructor
+     * \param[in] m_data_points number of data points to evaluate.
+     */
+    Functor(int m_data_points) : m_data_points_(m_data_points) {}
+
+    /** \brief Destructor. */
+    virtual ~Functor() {}
+
+    /** \brief Get the number of values. */
+    int
+    values() const
+    {
+      return (m_data_points_);
+    }
+
+  protected:
+    int m_data_points_;
+  };
+
+  struct OptimizationFunctor : public Functor<MatScalar> {
+    using Functor<MatScalar>::values;
+
+    /** Functor constructor
+     * \param[in] m_data_points the number of data points to evaluate
+     * \param[in,out] estimator pointer to the estimator object
+     */
+    OptimizationFunctor(int m_data_points, const TransformationEstimationLM* estimator)
+    : Functor<MatScalar>(m_data_points), estimator_(estimator)
+    {}
+
+    /** Copy constructor
+     * \param[in] src the optimization functor to copy into this
+     */
+    inline OptimizationFunctor(const OptimizationFunctor& src)
+    : Functor<MatScalar>(src.m_data_points_), estimator_()
+    {
+      *this = src;
+    }
+
+    /** Copy operator
+     * \param[in] src the optimization functor to copy into this
+     */
+    inline OptimizationFunctor&
+    operator=(const OptimizationFunctor& src)
+    {
+      Functor<MatScalar>::operator=(src);
+      estimator_ = src.estimator_;
+      return (*this);
+    }
+
+    /** \brief Destructor. */
+    ~OptimizationFunctor() {}
+
+    /** Fill fvec from x. For the current state vector x fill the f values
+     * \param[in] x state vector
+     * \param[out] fvec f values vector
+     */
+    int
+    operator()(const VectorX& x, VectorX& fvec) const;
+
+    const TransformationEstimationLM<PointSource, PointTarget, MatScalar>* estimator_;
+  };
+
+  struct OptimizationFunctorWithIndices : public Functor<MatScalar> {
+    using Functor<MatScalar>::values;
+
+    /** Functor constructor
+     * \param[in] m_data_points the number of data points to evaluate
+     * \param[in,out] estimator pointer to the estimator object
+     */
+    OptimizationFunctorWithIndices(int m_data_points,
+                                   const TransformationEstimationLM* estimator)
+    : Functor<MatScalar>(m_data_points), estimator_(estimator)
+    {}
+
+    /** Copy constructor
+     * \param[in] src the optimization functor to copy into this
+     */
+    inline OptimizationFunctorWithIndices(const OptimizationFunctorWithIndices& src)
+    : Functor<MatScalar>(src.m_data_points_), estimator_()
+    {
+      *this = src;
+    }
+
+    /** Copy operator
+     * \param[in] src the optimization functor to copy into this
+     */
+    inline OptimizationFunctorWithIndices&
+    operator=(const OptimizationFunctorWithIndices& src)
+    {
+      Functor<MatScalar>::operator=(src);
+      estimator_ = src.estimator_;
+      return (*this);
+    }
+
+    /** \brief Destructor. */
+    ~OptimizationFunctorWithIndices() {}
+
+    /** Fill fvec from x. For the current state vector x fill the f values
+     * \param[in] x state vector
+     * \param[out] fvec f values vector
+     */
+    int
+    operator()(const VectorX& x, VectorX& fvec) const;
+
+    const TransformationEstimationLM<PointSource, PointTarget, MatScalar>* estimator_;
+  };
+
+public:
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+};
+} // namespace registration
+} // namespace pcl
 
 #include <pcl/registration/impl/transformation_estimation_lm.hpp>
diff --git a/registration/include/pcl/registration/transformation_estimation_point_to_plane.h b/registration/include/pcl/registration/transformation_estimation_point_to_plane.h
index 3d26df7c..36f3a932 100644
--- a/registration/include/pcl/registration/transformation_estimation_point_to_plane.h
+++ b/registration/include/pcl/registration/transformation_estimation_point_to_plane.h
@@ -44,55 +44,55 @@
 #include <pcl/registration/transformation_estimation_lm.h>
 #include <pcl/registration/warp_point_rigid.h>
 
-namespace pcl
-{
-  namespace registration
-  {
-    /** @b TransformationEstimationPointToPlane uses Levenberg Marquardt optimization to find the
-      * transformation that minimizes the point-to-plane distance between the given correspondences.
-      *
-      * \author Michael Dixon
-      * \ingroup registration
-      */
-    template <typename PointSource, typename PointTarget, typename Scalar = float>
-    class TransformationEstimationPointToPlane : public TransformationEstimationLM<PointSource, PointTarget, Scalar>
-    {
-      public:
-        using Ptr = shared_ptr<TransformationEstimationPointToPlane<PointSource, PointTarget, Scalar> >;
-        using ConstPtr = shared_ptr<const TransformationEstimationPointToPlane<PointSource, PointTarget, Scalar> >;
-
-        using PointCloudSource = pcl::PointCloud<PointSource>;
-        using PointCloudSourcePtr = typename PointCloudSource::Ptr;
-        using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
-        using PointCloudTarget = pcl::PointCloud<PointTarget>;
-        using PointIndicesPtr = PointIndices::Ptr;
-        using PointIndicesConstPtr = PointIndices::ConstPtr;
+namespace pcl {
+namespace registration {
+/** @b TransformationEstimationPointToPlane uses Levenberg Marquardt optimization to
+ * find the transformation that minimizes the point-to-plane distance between the given
+ * correspondences.
+ *
+ * \author Michael Dixon
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename Scalar = float>
+class TransformationEstimationPointToPlane
+: public TransformationEstimationLM<PointSource, PointTarget, Scalar> {
+public:
+  using Ptr = shared_ptr<
+      TransformationEstimationPointToPlane<PointSource, PointTarget, Scalar>>;
+  using ConstPtr = shared_ptr<
+      const TransformationEstimationPointToPlane<PointSource, PointTarget, Scalar>>;
 
-        using Vector4 = Eigen::Matrix<Scalar, 4, 1>;
+  using PointCloudSource = pcl::PointCloud<PointSource>;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+  using PointCloudTarget = pcl::PointCloud<PointTarget>;
+  using PointIndicesPtr = PointIndices::Ptr;
+  using PointIndicesConstPtr = PointIndices::ConstPtr;
 
-        TransformationEstimationPointToPlane () {};
-        ~TransformationEstimationPointToPlane () {};
+  using Vector4 = Eigen::Matrix<Scalar, 4, 1>;
 
-      protected:
-        Scalar
-        computeDistance (const PointSource &p_src, const PointTarget &p_tgt) const override
-        {
-          // Compute the point-to-plane distance
-          Vector4 s (p_src.x, p_src.y, p_src.z, 0);
-          Vector4 t (p_tgt.x, p_tgt.y, p_tgt.z, 0);
-          Vector4 n (p_tgt.normal_x, p_tgt.normal_y, p_tgt.normal_z, 0);
-          return ((s - t).dot (n));
-        }
+  TransformationEstimationPointToPlane(){};
+  ~TransformationEstimationPointToPlane(){};
 
-        Scalar
-        computeDistance (const Vector4 &p_src, const PointTarget &p_tgt) const override
-        {
-          // Compute the point-to-plane distance
-          Vector4 t (p_tgt.x, p_tgt.y, p_tgt.z, 0);
-          Vector4 n (p_tgt.normal_x, p_tgt.normal_y, p_tgt.normal_z, 0);
-          return ((p_src - t).dot (n));
-        }
+protected:
+  Scalar
+  computeDistance(const PointSource& p_src, const PointTarget& p_tgt) const override
+  {
+    // Compute the point-to-plane distance
+    Vector4 s(p_src.x, p_src.y, p_src.z, 0);
+    Vector4 t(p_tgt.x, p_tgt.y, p_tgt.z, 0);
+    Vector4 n(p_tgt.normal_x, p_tgt.normal_y, p_tgt.normal_z, 0);
+    return ((s - t).dot(n));
+  }
 
-    };
+  Scalar
+  computeDistance(const Vector4& p_src, const PointTarget& p_tgt) const override
+  {
+    // Compute the point-to-plane distance
+    Vector4 t(p_tgt.x, p_tgt.y, p_tgt.z, 0);
+    Vector4 n(p_tgt.normal_x, p_tgt.normal_y, p_tgt.normal_z, 0);
+    return ((p_src - t).dot(n));
   }
-}
+};
+} // namespace registration
+} // namespace pcl
diff --git a/registration/include/pcl/registration/transformation_estimation_point_to_plane_lls.h b/registration/include/pcl/registration/transformation_estimation_point_to_plane_lls.h
index 0733eb12..a1f6bdbd 100644
--- a/registration/include/pcl/registration/transformation_estimation_point_to_plane_lls.h
+++ b/registration/include/pcl/registration/transformation_estimation_point_to_plane_lls.h
@@ -44,113 +44,114 @@
 #include <pcl/registration/warp_point_rigid.h>
 #include <pcl/cloud_iterator.h>
 
-namespace pcl
-{
-  namespace registration
-  {
-    /** \brief @b TransformationEstimationPointToPlaneLLS implements a Linear Least Squares (LLS) approximation
-      * for minimizing the point-to-plane distance between two clouds of corresponding points with normals.
-      *
-      * For additional details, see 
-      *   "Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration", Kok-Lim Low, 2004
-      *
-      * \note The class is templated on the source and target point types as well as on the output scalar of the
-      * transformation matrix (i.e., float or double). Default: float.
-      * \author Michael Dixon
-      * \ingroup registration
-      */
-    template <typename PointSource, typename PointTarget, typename Scalar = float>
-    class TransformationEstimationPointToPlaneLLS : public TransformationEstimation<PointSource, PointTarget, Scalar>
-    {
-      public:
-        using Ptr = shared_ptr<TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar> >;
-        using ConstPtr = shared_ptr<const TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar> >;
+namespace pcl {
+namespace registration {
+/** \brief @b TransformationEstimationPointToPlaneLLS implements a Linear Least Squares
+ * (LLS) approximation for minimizing the point-to-plane distance between two clouds of
+ * corresponding points with normals.
+ *
+ * For additional details, see
+ *   "Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration",
+ * Kok-Lim Low, 2004
+ *
+ * \note The class is templated on the source and target point types as well as on the
+ * output scalar of the transformation matrix (i.e., float or double). Default: float.
+ * \author Michael Dixon
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename Scalar = float>
+class TransformationEstimationPointToPlaneLLS
+: public TransformationEstimation<PointSource, PointTarget, Scalar> {
+public:
+  using Ptr = shared_ptr<
+      TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>>;
+  using ConstPtr = shared_ptr<
+      const TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>>;
 
-        using Matrix4 = typename TransformationEstimation<PointSource, PointTarget, Scalar>::Matrix4;
-        
-        TransformationEstimationPointToPlaneLLS () {};
-        ~TransformationEstimationPointToPlaneLLS () {};
+  using Matrix4 =
+      typename TransformationEstimation<PointSource, PointTarget, Scalar>::Matrix4;
 
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            Matrix4 &transformation_matrix) const override;
+  TransformationEstimationPointToPlaneLLS(){};
+  ~TransformationEstimationPointToPlaneLLS(){};
 
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const std::vector<int> &indices_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            Matrix4 &transformation_matrix) const override;
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using SVD. \param[in] cloud_src the source point cloud dataset
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              Matrix4& transformation_matrix) const override;
 
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from \a indices_src
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const std::vector<int> &indices_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            const std::vector<int> &indices_tgt,
-            Matrix4 &transformation_matrix) const override;
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using SVD. \param[in] cloud_src the source point cloud dataset
+   * \param[in] indices_src the vector of indices describing the points of interest in
+   * \a cloud_src
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::Indices& indices_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              Matrix4& transformation_matrix) const override;
 
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[in] correspondences the vector of correspondences between source and target point cloud
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            const pcl::Correspondences &correspondences,
-            Matrix4 &transformation_matrix) const override;
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using SVD. \param[in] cloud_src the source point cloud dataset
+   * \param[in] indices_src the vector of indices describing the points of interest in
+   * \a cloud_src
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[in] indices_tgt the vector of indices describing the correspondences of the
+   * interest points from \a indices_src
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::Indices& indices_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              const pcl::Indices& indices_tgt,
+                              Matrix4& transformation_matrix) const override;
 
-      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<PointSource>& source_it, 
-                                     ConstCloudIterator<PointTarget>& target_it, 
-                                     Matrix4 &transformation_matrix) const;
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using SVD. \param[in] cloud_src the source point cloud dataset
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[in] correspondences the vector of correspondences between source and target
+   * point cloud \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              const pcl::Correspondences& correspondences,
+                              Matrix4& transformation_matrix) const override;
 
-        /** \brief Construct a 4 by 4 transformation matrix from the provided rotation and translation.
-          * \param[in] alpha the rotation about the x-axis
-          * \param[in] beta the rotation about the y-axis
-          * \param[in] gamma the rotation about the z-axis
-          * \param[in] tx the x translation
-          * \param[in] ty the y translation
-          * \param[in] tz the z translation
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        constructTransformationMatrix (const double & alpha, const double & beta, const double & gamma,
-                                       const double & tx,    const double & ty,   const double & tz,
-                                       Matrix4 &transformation_matrix) const;
+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<PointSource>& source_it,
+                              ConstCloudIterator<PointTarget>& target_it,
+                              Matrix4& transformation_matrix) const;
 
-    };
-  }
-}
+  /** \brief Construct a 4 by 4 transformation matrix from the provided rotation and
+   * translation. \param[in] alpha the rotation about the x-axis \param[in] beta the
+   * rotation about the y-axis \param[in] gamma the rotation about the z-axis \param[in]
+   * tx the x translation \param[in] ty the y translation \param[in] tz the z
+   * translation \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  constructTransformationMatrix(const double& alpha,
+                                const double& beta,
+                                const double& gamma,
+                                const double& tx,
+                                const double& ty,
+                                const double& tz,
+                                Matrix4& transformation_matrix) const;
+};
+} // namespace registration
+} // namespace pcl
 
 #include <pcl/registration/impl/transformation_estimation_point_to_plane_lls.hpp>
diff --git a/registration/include/pcl/registration/transformation_estimation_point_to_plane_lls_weighted.h b/registration/include/pcl/registration/transformation_estimation_point_to_plane_lls_weighted.h
index 0229f9ed..07825ed4 100644
--- a/registration/include/pcl/registration/transformation_estimation_point_to_plane_lls_weighted.h
+++ b/registration/include/pcl/registration/transformation_estimation_point_to_plane_lls_weighted.h
@@ -43,125 +43,131 @@
 #include <pcl/registration/warp_point_rigid.h>
 #include <pcl/cloud_iterator.h>
 
-namespace pcl
-{
-  namespace registration
-  {
-    /** \brief @b TransformationEstimationPointToPlaneLLSWeighted implements a Linear Least Squares (LLS) approximation
-      * for minimizing the point-to-plane distance between two clouds of corresponding points with normals, with the
-      * possibility of assigning weights to the correspondences.
-      *
-      * For additional details, see 
-      *   "Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration", Kok-Lim Low, 2004
-      *
-      * \note The class is templated on the source and target point types as well as on the output scalar of the
-      * transformation matrix (i.e., float or double). Default: float.
-      * \author Alex Ichim
-      * \ingroup registration
-      */
-    template <typename PointSource, typename PointTarget, typename Scalar = float>
-    class TransformationEstimationPointToPlaneLLSWeighted : public TransformationEstimation<PointSource, PointTarget, Scalar>
-    {
-      public:
-        using Ptr = shared_ptr<TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar> >;
-        using ConstPtr = shared_ptr<const TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar> >;
+namespace pcl {
+namespace registration {
+/** \brief @b TransformationEstimationPointToPlaneLLSWeighted implements a Linear Least
+ * Squares (LLS) approximation for minimizing the point-to-plane distance between two
+ * clouds of corresponding points with normals, with the possibility of assigning
+ * weights to the correspondences.
+ *
+ * For additional details, see
+ *   "Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration",
+ * Kok-Lim Low, 2004
+ *
+ * \note The class is templated on the source and target point types as well as on the
+ * output scalar of the transformation matrix (i.e., float or double). Default: float.
+ * \author Alex Ichim
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename Scalar = float>
+class TransformationEstimationPointToPlaneLLSWeighted
+: public TransformationEstimation<PointSource, PointTarget, Scalar> {
+public:
+  using Ptr = shared_ptr<TransformationEstimationPointToPlaneLLSWeighted<PointSource,
+                                                                         PointTarget,
+                                                                         Scalar>>;
+  using ConstPtr =
+      shared_ptr<const TransformationEstimationPointToPlaneLLSWeighted<PointSource,
+                                                                       PointTarget,
+                                                                       Scalar>>;
 
-        using Matrix4 = typename TransformationEstimation<PointSource, PointTarget, Scalar>::Matrix4;
-        
-        TransformationEstimationPointToPlaneLLSWeighted () { };
-        virtual ~TransformationEstimationPointToPlaneLLSWeighted () { };
+  using Matrix4 =
+      typename TransformationEstimation<PointSource, PointTarget, Scalar>::Matrix4;
 
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            Matrix4 &transformation_matrix) const;
+  TransformationEstimationPointToPlaneLLSWeighted(){};
+  virtual ~TransformationEstimationPointToPlaneLLSWeighted(){};
 
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const std::vector<int> &indices_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            Matrix4 &transformation_matrix) const;
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using SVD. \param[in] cloud_src the source point cloud dataset
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              Matrix4& transformation_matrix) const;
 
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from \a indices_src
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const std::vector<int> &indices_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            const std::vector<int> &indices_tgt,
-            Matrix4 &transformation_matrix) const;
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using SVD. \param[in] cloud_src the source point cloud dataset
+   * \param[in] indices_src the vector of indices describing the points of interest in
+   * \a cloud_src
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::Indices& indices_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              Matrix4& transformation_matrix) const;
 
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[in] correspondences the vector of correspondences between source and target point cloud
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            const pcl::Correspondences &correspondences,
-            Matrix4 &transformation_matrix) const;
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using SVD. \param[in] cloud_src the source point cloud dataset
+   * \param[in] indices_src the vector of indices describing the points of interest in
+   * \a cloud_src
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[in] indices_tgt the vector of indices describing the correspondences of the
+   * interest points from \a indices_src
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::Indices& indices_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              const pcl::Indices& indices_tgt,
+                              Matrix4& transformation_matrix) const;
 
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using SVD. \param[in] cloud_src the source point cloud dataset
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[in] correspondences the vector of correspondences between source and target
+   * point cloud \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              const pcl::Correspondences& correspondences,
+                              Matrix4& transformation_matrix) const;
 
-        /** \brief Set the weights for the correspondences.
-          * \param[in] weights the weights for each correspondence
-          */
-        inline void
-        setCorrespondenceWeights (const std::vector<Scalar> &weights)
-        { weights_ = weights; }
+  /** \brief Set the weights for the correspondences.
+   * \param[in] weights the weights for each correspondence
+   */
+  inline void
+  setCorrespondenceWeights(const std::vector<Scalar>& weights)
+  {
+    weights_ = weights;
+  }
 
-      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 weights_it
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        void 
-        estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it, 
-                                     ConstCloudIterator<PointTarget>& target_it, 
-                                     typename std::vector<Scalar>::const_iterator& weights_it,
-                                     Matrix4 &transformation_matrix) const;
+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 weights_it
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  void
+  estimateRigidTransformation(ConstCloudIterator<PointSource>& source_it,
+                              ConstCloudIterator<PointTarget>& target_it,
+                              typename std::vector<Scalar>::const_iterator& weights_it,
+                              Matrix4& transformation_matrix) const;
 
-        /** \brief Construct a 4 by 4 transformation matrix from the provided rotation and translation.
-          * \param[in] alpha the rotation about the x-axis
-          * \param[in] beta the rotation about the y-axis
-          * \param[in] gamma the rotation about the z-axis
-          * \param[in] tx the x translation
-          * \param[in] ty the y translation
-          * \param[in] tz the z translation
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        constructTransformationMatrix (const double & alpha, const double & beta, const double & gamma,
-                                       const double & tx,    const double & ty,   const double & tz,
-                                       Matrix4 &transformation_matrix) const;
+  /** \brief Construct a 4 by 4 transformation matrix from the provided rotation and
+   * translation. \param[in] alpha the rotation about the x-axis \param[in] beta the
+   * rotation about the y-axis \param[in] gamma the rotation about the z-axis \param[in]
+   * tx the x translation \param[in] ty the y translation \param[in] tz the z
+   * translation \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  constructTransformationMatrix(const double& alpha,
+                                const double& beta,
+                                const double& gamma,
+                                const double& tx,
+                                const double& ty,
+                                const double& tz,
+                                Matrix4& transformation_matrix) const;
 
-        std::vector<Scalar> weights_;
-    };
-  }
-}
+  std::vector<Scalar> weights_;
+};
+} // namespace registration
+} // namespace pcl
 
 #include <pcl/registration/impl/transformation_estimation_point_to_plane_lls_weighted.hpp>
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 4430702b..29996d45 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,301 +38,319 @@
 
 #pragma once
 
-#include <pcl/memory.h>
-#include <pcl/pcl_macros.h>
+#include <pcl/registration/distances.h>
 #include <pcl/registration/transformation_estimation_point_to_plane.h>
 #include <pcl/registration/warp_point_rigid.h>
-#include <pcl/registration/distances.h>
+#include <pcl/memory.h>
+#include <pcl/pcl_macros.h>
 
-namespace pcl
-{
-  namespace registration
+namespace pcl {
+namespace registration {
+/** @b TransformationEstimationPointToPlaneWeighted uses Levenberg Marquardt
+ * optimization to find the transformation that minimizes the point-to-plane distance
+ * between the given correspondences. In addition to the
+ * TransformationEstimationPointToPlane class, this version takes per-correspondence
+ * weights and optimizes accordingly.
+ *
+ * \author Alexandru-Eugen Ichim
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename MatScalar = float>
+class TransformationEstimationPointToPlaneWeighted
+: public TransformationEstimationPointToPlane<PointSource, PointTarget, MatScalar> {
+  using PointCloudSource = pcl::PointCloud<PointSource>;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+
+  using PointCloudTarget = pcl::PointCloud<PointTarget>;
+
+  using PointIndicesPtr = PointIndices::Ptr;
+  using PointIndicesConstPtr = PointIndices::ConstPtr;
+
+public:
+  using Ptr = shared_ptr<TransformationEstimationPointToPlaneWeighted<PointSource,
+                                                                      PointTarget,
+                                                                      MatScalar>>;
+  using ConstPtr =
+      shared_ptr<const TransformationEstimationPointToPlaneWeighted<PointSource,
+                                                                    PointTarget,
+                                                                    MatScalar>>;
+
+  using VectorX = Eigen::Matrix<MatScalar, Eigen::Dynamic, 1>;
+  using Vector4 = Eigen::Matrix<MatScalar, 4, 1>;
+  using Matrix4 =
+      typename TransformationEstimation<PointSource, PointTarget, MatScalar>::Matrix4;
+
+  /** \brief Constructor. */
+  TransformationEstimationPointToPlaneWeighted();
+
+  /** \brief Copy constructor.
+   * \param[in] src the TransformationEstimationPointToPlaneWeighted object to copy into
+   * this
+   */
+  TransformationEstimationPointToPlaneWeighted(
+      const TransformationEstimationPointToPlaneWeighted& src)
+  : tmp_src_(src.tmp_src_)
+  , tmp_tgt_(src.tmp_tgt_)
+  , tmp_idx_src_(src.tmp_idx_src_)
+  , tmp_idx_tgt_(src.tmp_idx_tgt_)
+  , warp_point_(src.warp_point_)
+  , correspondence_weights_(src.correspondence_weights_)
+  , use_correspondence_weights_(src.use_correspondence_weights_){};
+
+  /** \brief Copy operator.
+   * \param[in] src the TransformationEstimationPointToPlaneWeighted object to copy into
+   * this
+   */
+  TransformationEstimationPointToPlaneWeighted&
+  operator=(const TransformationEstimationPointToPlaneWeighted& src)
   {
-    /** @b TransformationEstimationPointToPlaneWeighted uses Levenberg Marquardt optimization to find the transformation
-      * that minimizes the point-to-plane distance between the given correspondences. In addition to the
-      * TransformationEstimationPointToPlane class, this version takes per-correspondence weights and optimizes accordingly.
-      *
-      * \author Alexandru-Eugen Ichim
-      * \ingroup registration
-      */
-    template <typename PointSource, typename PointTarget, typename MatScalar = float>
-    class TransformationEstimationPointToPlaneWeighted : public TransformationEstimationPointToPlane<PointSource, PointTarget, MatScalar>
-    {
-      using PointCloudSource = pcl::PointCloud<PointSource>;
-      using PointCloudSourcePtr = typename PointCloudSource::Ptr;
-      using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
-
-      using PointCloudTarget = pcl::PointCloud<PointTarget>;
-
-      using PointIndicesPtr = PointIndices::Ptr;
-      using PointIndicesConstPtr = PointIndices::ConstPtr;
-
-      public:
-        using Ptr = shared_ptr<TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar> >;
-        using ConstPtr = shared_ptr<const TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar> >;
-
-        using VectorX = Eigen::Matrix<MatScalar, Eigen::Dynamic, 1>;
-        using Vector4 = Eigen::Matrix<MatScalar, 4, 1>;
-        using Matrix4 = typename TransformationEstimation<PointSource, PointTarget, MatScalar>::Matrix4;
-        
-        /** \brief Constructor. */
-        TransformationEstimationPointToPlaneWeighted ();
-
-        /** \brief Copy constructor. 
-          * \param[in] src the TransformationEstimationPointToPlaneWeighted object to copy into this
-          */
-        TransformationEstimationPointToPlaneWeighted (const TransformationEstimationPointToPlaneWeighted &src) :
-          tmp_src_ (src.tmp_src_), 
-          tmp_tgt_ (src.tmp_tgt_), 
-          tmp_idx_src_ (src.tmp_idx_src_), 
-          tmp_idx_tgt_ (src.tmp_idx_tgt_), 
-          warp_point_ (src.warp_point_),
-          correspondence_weights_ (src.correspondence_weights_),
-          use_correspondence_weights_ (src.use_correspondence_weights_)
-        {};
-
-        /** \brief Copy operator. 
-          * \param[in] src the TransformationEstimationPointToPlaneWeighted object to copy into this
-          */
-        TransformationEstimationPointToPlaneWeighted&
-        operator = (const TransformationEstimationPointToPlaneWeighted &src)
-        {
-          tmp_src_ = src.tmp_src_; 
-          tmp_tgt_ = src.tmp_tgt_; 
-          tmp_idx_src_ = src.tmp_idx_src_;
-          tmp_idx_tgt_ = src.tmp_idx_tgt_; 
-          warp_point_ = src.warp_point_;
-          correspondence_weights_ = src.correspondence_weights_;
-          use_correspondence_weights_ = src.use_correspondence_weights_;
-        }
-
-         /** \brief Destructor. */
-        virtual ~TransformationEstimationPointToPlaneWeighted () {};
-
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the resultant transformation matrix
-          * \note Uses the weights given by setWeights.
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            Matrix4 &transformation_matrix) const;
-
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the resultant transformation matrix
-          * \note Uses the weights given by setWeights.
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const std::vector<int> &indices_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            Matrix4 &transformation_matrix) const;
-
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from 
-          * \a indices_src
-          * \param[out] transformation_matrix the resultant transformation matrix
-          * \note Uses the weights given by setWeights.
-          */
-        void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const std::vector<int> &indices_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            const std::vector<int> &indices_tgt,
-            Matrix4 &transformation_matrix) const;
-
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[in] correspondences the vector of correspondences between source and target point cloud
-          * \param[out] transformation_matrix the resultant transformation matrix
-          * \note Uses the weights given by setWeights.
-          */
-        void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            const pcl::Correspondences &correspondences,
-            Matrix4 &transformation_matrix) const;  
-
-
-        inline void
-        setWeights (const std::vector<double> &weights)
-        { correspondence_weights_ = weights; }
-
-        /// use the weights given in the pcl::CorrespondencesPtr for one of the estimateTransformation (...) methods
-        inline void
-        setUseCorrespondenceWeights (bool use_correspondence_weights)
-        { use_correspondence_weights_ = use_correspondence_weights; }
-
-        /** \brief Set the function we use to warp points. Defaults to rigid 6D warp.
-          * \param[in] warp_fcn a shared pointer to an object that warps points
-          */
-        void
-        setWarpFunction (const typename WarpPointRigid<PointSource, PointTarget, MatScalar>::Ptr &warp_fcn)
-        { warp_point_ = warp_fcn; }
-
-      protected:
-        bool use_correspondence_weights_;
-        mutable std::vector<double> correspondence_weights_;
-
-        /** \brief Temporary pointer to the source dataset. */
-        mutable const PointCloudSource *tmp_src_;
-
-        /** \brief Temporary pointer to the target dataset. */
-        mutable const PointCloudTarget  *tmp_tgt_;
-
-        /** \brief Temporary pointer to the source dataset indices. */
-        mutable const std::vector<int> *tmp_idx_src_;
-
-        /** \brief Temporary pointer to the target dataset indices. */
-        mutable const std::vector<int> *tmp_idx_tgt_;
-
-        /** \brief The parameterized function used to warp the source to the target. */
-        typename pcl::registration::WarpPointRigid<PointSource, PointTarget, MatScalar>::Ptr warp_point_;
-        
-        /** Base functor all the models that need non linear optimization must
-          * define their own one and implement operator() (const Eigen::VectorXd& x, Eigen::VectorXd& fvec)
-          * or operator() (const Eigen::VectorXf& x, Eigen::VectorXf& fvec) depending on the chosen _Scalar
-          */
-        template<typename _Scalar, int NX=Eigen::Dynamic, int NY=Eigen::Dynamic>
-        struct Functor
-        {
-          using Scalar = _Scalar;
-          enum 
-          {
-            InputsAtCompileTime = NX,
-            ValuesAtCompileTime = NY
-          };
-          using InputType = Eigen::Matrix<_Scalar,InputsAtCompileTime,1>;
-          using ValueType = Eigen::Matrix<_Scalar,ValuesAtCompileTime,1>;
-          using JacobianType = Eigen::Matrix<_Scalar,ValuesAtCompileTime,InputsAtCompileTime>;
-
-          /** \brief Empty Constructor. */
-          Functor () : m_data_points_ (ValuesAtCompileTime) {}
-
-          /** \brief Constructor
-            * \param[in] m_data_points number of data points to evaluate.
-            */
-          Functor (int m_data_points) : m_data_points_ (m_data_points) {}
-        
-          /** \brief Destructor. */
-          virtual ~Functor () {}
-
-          /** \brief Get the number of values. */ 
-          int
-          values () const { return (m_data_points_); }
-
-          protected:
-            int m_data_points_;
-        };
-
-        struct OptimizationFunctor : public Functor<MatScalar>
-        {
-          using Functor<MatScalar>::values;
-
-          /** Functor constructor
-            * \param[in] m_data_points the number of data points to evaluate
-            * \param[in,out] estimator pointer to the estimator object
-            */
-          OptimizationFunctor (int m_data_points, 
-                               const TransformationEstimationPointToPlaneWeighted *estimator)
-            :  Functor<MatScalar> (m_data_points), estimator_ (estimator) 
-          {}
-
-          /** Copy constructor
-            * \param[in] src the optimization functor to copy into this
-            */
-          inline OptimizationFunctor (const OptimizationFunctor &src) : 
-            Functor<MatScalar> (src.m_data_points_), estimator_ ()
-          {
-            *this = src;
-          }
-
-          /** Copy operator
-            * \param[in] src the optimization functor to copy into this
-            */
-          inline OptimizationFunctor& 
-          operator = (const OptimizationFunctor &src) 
-          { 
-            Functor<MatScalar>::operator=(src);
-            estimator_ = src.estimator_; 
-            return (*this); 
-          }
-
-          /** \brief Destructor. */
-          virtual ~OptimizationFunctor () {}
-
-          /** Fill fvec from x. For the current state vector x fill the f values
-            * \param[in] x state vector
-            * \param[out] fvec f values vector
-            */
-          int 
-          operator () (const VectorX &x, VectorX &fvec) const;
-
-          const TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar> *estimator_;
-        };
-
-        struct OptimizationFunctorWithIndices : public Functor<MatScalar>
-        {
-          using Functor<MatScalar>::values;
-
-          /** Functor constructor
-            * \param[in] m_data_points the number of data points to evaluate
-            * \param[in,out] estimator pointer to the estimator object
-            */
-          OptimizationFunctorWithIndices (int m_data_points, 
-                                          const TransformationEstimationPointToPlaneWeighted *estimator)
-            : Functor<MatScalar> (m_data_points), estimator_ (estimator) 
-          {}
-
-          /** Copy constructor
-            * \param[in] src the optimization functor to copy into this
-            */
-          inline OptimizationFunctorWithIndices (const OptimizationFunctorWithIndices &src)
-            : Functor<MatScalar> (src.m_data_points_), estimator_ ()
-          {
-            *this = src;
-          }
-
-          /** Copy operator
-            * \param[in] src the optimization functor to copy into this
-            */
-          inline OptimizationFunctorWithIndices& 
-          operator = (const OptimizationFunctorWithIndices &src) 
-          { 
-            Functor<MatScalar>::operator=(src);
-            estimator_ = src.estimator_; 
-            return (*this); 
-          }
-
-          /** \brief Destructor. */
-          virtual ~OptimizationFunctorWithIndices () {}
-
-          /** Fill fvec from x. For the current state vector x fill the f values
-            * \param[in] x state vector
-            * \param[out] fvec f values vector
-            */
-          int 
-          operator () (const VectorX &x, VectorX &fvec) const;
-
-          const TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar> *estimator_;
-        };
-      public:
-        PCL_MAKE_ALIGNED_OPERATOR_NEW
-    };
+    tmp_src_ = src.tmp_src_;
+    tmp_tgt_ = src.tmp_tgt_;
+    tmp_idx_src_ = src.tmp_idx_src_;
+    tmp_idx_tgt_ = src.tmp_idx_tgt_;
+    warp_point_ = src.warp_point_;
+    correspondence_weights_ = src.correspondence_weights_;
+    use_correspondence_weights_ = src.use_correspondence_weights_;
+  }
+
+  /** \brief Destructor. */
+  virtual ~TransformationEstimationPointToPlaneWeighted(){};
+
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using LM. \param[in] cloud_src the source point cloud dataset
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[out] transformation_matrix the resultant transformation matrix
+   * \note Uses the weights given by setWeights.
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              Matrix4& transformation_matrix) const;
+
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using LM. \param[in] cloud_src the source point cloud dataset
+   * \param[in] indices_src the vector of indices describing the points of interest in
+   * \a cloud_src
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[out] transformation_matrix the resultant transformation matrix
+   * \note Uses the weights given by setWeights.
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::Indices& indices_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              Matrix4& transformation_matrix) const;
+
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using LM. \param[in] cloud_src the source point cloud dataset
+   * \param[in] indices_src the vector of indices describing the points of interest in
+   * \a cloud_src
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[in] indices_tgt the vector of indices describing the correspondences of the
+   * interest points from \a indices_src
+   * \param[out] transformation_matrix the resultant transformation matrix
+   * \note Uses the weights given by setWeights.
+   */
+  void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::Indices& indices_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              const pcl::Indices& indices_tgt,
+                              Matrix4& transformation_matrix) const;
+
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using LM. \param[in] cloud_src the source point cloud dataset
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[in] correspondences the vector of correspondences between source and target
+   * point cloud \param[out] transformation_matrix the resultant transformation matrix
+   * \note Uses the weights given by setWeights.
+   */
+  void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              const pcl::Correspondences& correspondences,
+                              Matrix4& transformation_matrix) const;
+
+  inline void
+  setWeights(const std::vector<double>& weights)
+  {
+    correspondence_weights_ = weights;
   }
-}
+
+  /// use the weights given in the pcl::CorrespondencesPtr for one of the
+  /// estimateTransformation (...) methods
+  inline void
+  setUseCorrespondenceWeights(bool use_correspondence_weights)
+  {
+    use_correspondence_weights_ = use_correspondence_weights;
+  }
+
+  /** \brief Set the function we use to warp points. Defaults to rigid 6D warp.
+   * \param[in] warp_fcn a shared pointer to an object that warps points
+   */
+  void
+  setWarpFunction(
+      const typename WarpPointRigid<PointSource, PointTarget, MatScalar>::Ptr& warp_fcn)
+  {
+    warp_point_ = warp_fcn;
+  }
+
+protected:
+  bool use_correspondence_weights_;
+  mutable std::vector<double> correspondence_weights_;
+
+  /** \brief Temporary pointer to the source dataset. */
+  mutable const PointCloudSource* tmp_src_;
+
+  /** \brief Temporary pointer to the target dataset. */
+  mutable const PointCloudTarget* tmp_tgt_;
+
+  /** \brief Temporary pointer to the source dataset indices. */
+  mutable const pcl::Indices* tmp_idx_src_;
+
+  /** \brief Temporary pointer to the target dataset indices. */
+  mutable const pcl::Indices* tmp_idx_tgt_;
+
+  /** \brief The parameterized function used to warp the source to the target. */
+  typename pcl::registration::WarpPointRigid<PointSource, PointTarget, MatScalar>::Ptr
+      warp_point_;
+
+  /** Base functor all the models that need non linear optimization must
+   * define their own one and implement operator() (const Eigen::VectorXd& x,
+   * Eigen::VectorXd& fvec) or operator() (const Eigen::VectorXf& x, Eigen::VectorXf&
+   * fvec) depending on the chosen _Scalar
+   */
+  template <typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
+  struct Functor {
+    using Scalar = _Scalar;
+    enum { InputsAtCompileTime = NX, ValuesAtCompileTime = NY };
+    using InputType = Eigen::Matrix<_Scalar, InputsAtCompileTime, 1>;
+    using ValueType = Eigen::Matrix<_Scalar, ValuesAtCompileTime, 1>;
+    using JacobianType =
+        Eigen::Matrix<_Scalar, ValuesAtCompileTime, InputsAtCompileTime>;
+
+    /** \brief Empty Constructor. */
+    Functor() : m_data_points_(ValuesAtCompileTime) {}
+
+    /** \brief Constructor
+     * \param[in] m_data_points number of data points to evaluate.
+     */
+    Functor(int m_data_points) : m_data_points_(m_data_points) {}
+
+    /** \brief Destructor. */
+    virtual ~Functor() {}
+
+    /** \brief Get the number of values. */
+    int
+    values() const
+    {
+      return (m_data_points_);
+    }
+
+  protected:
+    int m_data_points_;
+  };
+
+  struct OptimizationFunctor : public Functor<MatScalar> {
+    using Functor<MatScalar>::values;
+
+    /** Functor constructor
+     * \param[in] m_data_points the number of data points to evaluate
+     * \param[in,out] estimator pointer to the estimator object
+     */
+    OptimizationFunctor(int m_data_points,
+                        const TransformationEstimationPointToPlaneWeighted* estimator)
+    : Functor<MatScalar>(m_data_points), estimator_(estimator)
+    {}
+
+    /** Copy constructor
+     * \param[in] src the optimization functor to copy into this
+     */
+    inline OptimizationFunctor(const OptimizationFunctor& src)
+    : Functor<MatScalar>(src.m_data_points_), estimator_()
+    {
+      *this = src;
+    }
+
+    /** Copy operator
+     * \param[in] src the optimization functor to copy into this
+     */
+    inline OptimizationFunctor&
+    operator=(const OptimizationFunctor& src)
+    {
+      Functor<MatScalar>::operator=(src);
+      estimator_ = src.estimator_;
+      return (*this);
+    }
+
+    /** \brief Destructor. */
+    virtual ~OptimizationFunctor() {}
+
+    /** Fill fvec from x. For the current state vector x fill the f values
+     * \param[in] x state vector
+     * \param[out] fvec f values vector
+     */
+    int
+    operator()(const VectorX& x, VectorX& fvec) const;
+
+    const TransformationEstimationPointToPlaneWeighted<PointSource,
+                                                       PointTarget,
+                                                       MatScalar>* estimator_;
+  };
+
+  struct OptimizationFunctorWithIndices : public Functor<MatScalar> {
+    using Functor<MatScalar>::values;
+
+    /** Functor constructor
+     * \param[in] m_data_points the number of data points to evaluate
+     * \param[in,out] estimator pointer to the estimator object
+     */
+    OptimizationFunctorWithIndices(
+        int m_data_points,
+        const TransformationEstimationPointToPlaneWeighted* estimator)
+    : Functor<MatScalar>(m_data_points), estimator_(estimator)
+    {}
+
+    /** Copy constructor
+     * \param[in] src the optimization functor to copy into this
+     */
+    inline OptimizationFunctorWithIndices(const OptimizationFunctorWithIndices& src)
+    : Functor<MatScalar>(src.m_data_points_), estimator_()
+    {
+      *this = src;
+    }
+
+    /** Copy operator
+     * \param[in] src the optimization functor to copy into this
+     */
+    inline OptimizationFunctorWithIndices&
+    operator=(const OptimizationFunctorWithIndices& src)
+    {
+      Functor<MatScalar>::operator=(src);
+      estimator_ = src.estimator_;
+      return (*this);
+    }
+
+    /** \brief Destructor. */
+    virtual ~OptimizationFunctorWithIndices() {}
+
+    /** Fill fvec from x. For the current state vector x fill the f values
+     * \param[in] x state vector
+     * \param[out] fvec f values vector
+     */
+    int
+    operator()(const VectorX& x, VectorX& fvec) const;
+
+    const TransformationEstimationPointToPlaneWeighted<PointSource,
+                                                       PointTarget,
+                                                       MatScalar>* estimator_;
+  };
+
+public:
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+};
+} // namespace registration
+} // namespace pcl
 
 #include <pcl/registration/impl/transformation_estimation_point_to_plane_weighted.hpp>
diff --git a/registration/include/pcl/registration/transformation_estimation_svd.h b/registration/include/pcl/registration/transformation_estimation_svd.h
index 60150d45..414c7bb2 100644
--- a/registration/include/pcl/registration/transformation_estimation_svd.h
+++ b/registration/include/pcl/registration/transformation_estimation_svd.h
@@ -43,117 +43,114 @@
 #include <pcl/registration/transformation_estimation.h>
 #include <pcl/cloud_iterator.h>
 
-namespace pcl
-{
-  namespace registration
-  {
-    /** @b TransformationEstimationSVD implements SVD-based estimation of
-      * the transformation aligning the given correspondences.
-      *
-      * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float.
-      * \author Dirk Holz, Radu B. Rusu
-      * \ingroup registration
-      */
-    template <typename PointSource, typename PointTarget, typename Scalar = float>
-    class TransformationEstimationSVD : public TransformationEstimation<PointSource, PointTarget, Scalar>
-    {
-      public:
-        using Ptr = shared_ptr<TransformationEstimationSVD<PointSource, PointTarget, Scalar> >;
-        using ConstPtr = shared_ptr<const TransformationEstimationSVD<PointSource, PointTarget, Scalar> >;
-
-        using Matrix4 = typename TransformationEstimation<PointSource, PointTarget, Scalar>::Matrix4;
-
-        /** \brief Constructor
-          * \param[in] use_umeyama Toggles whether or not to use 3rd party software*/
-        TransformationEstimationSVD (bool use_umeyama=true):
-          use_umeyama_ (use_umeyama)
-        {}
-
-        ~TransformationEstimationSVD () {};
-
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            Matrix4 &transformation_matrix) const override;
-
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const std::vector<int> &indices_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            Matrix4 &transformation_matrix) const override;
-
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from \a indices_src
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const std::vector<int> &indices_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            const std::vector<int> &indices_tgt,
-            Matrix4 &transformation_matrix) const override;
-
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[in] correspondences the vector of correspondences between source and target point cloud
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            const pcl::Correspondences &correspondences,
-            Matrix4 &transformation_matrix) const override;
-
-      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<PointSource>& source_it,
-                                     ConstCloudIterator<PointTarget>& target_it,
-                                     Matrix4 &transformation_matrix) const;
-
-        /** \brief Obtain a 4x4 rigid transformation matrix from a correlation matrix H = src * tgt'
-          * \param[in] cloud_src_demean the input source cloud, demeaned, in Eigen format
-          * \param[in] centroid_src the input source centroid, in Eigen format
-          * \param[in] cloud_tgt_demean the input target cloud, demeaned, in Eigen format
-          * \param[in] centroid_tgt the input target cloud, in Eigen format
-          * \param[out] transformation_matrix the resultant 4x4 rigid transformation matrix
-          */
-        virtual void
-        getTransformationFromCorrelation (
-            const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_src_demean,
-            const Eigen::Matrix<Scalar, 4, 1> &centroid_src,
-            const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean,
-            const Eigen::Matrix<Scalar, 4, 1> &centroid_tgt,
-            Matrix4 &transformation_matrix) const;
-
-        bool use_umeyama_;
-     };
-
-  }
-}
+namespace pcl {
+namespace registration {
+/** @b TransformationEstimationSVD implements SVD-based estimation of
+ * the transformation aligning the given correspondences.
+ *
+ * \note The class is templated on the source and target point types as well as on the
+ * output scalar of the transformation matrix (i.e., float or double). Default: float.
+ * \author Dirk Holz, Radu B. Rusu
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename Scalar = float>
+class TransformationEstimationSVD
+: public TransformationEstimation<PointSource, PointTarget, Scalar> {
+public:
+  using Ptr = shared_ptr<TransformationEstimationSVD<PointSource, PointTarget, Scalar>>;
+  using ConstPtr =
+      shared_ptr<const TransformationEstimationSVD<PointSource, PointTarget, Scalar>>;
+
+  using Matrix4 =
+      typename TransformationEstimation<PointSource, PointTarget, Scalar>::Matrix4;
+
+  /** \brief Constructor
+   * \param[in] use_umeyama Toggles whether or not to use 3rd party software*/
+  TransformationEstimationSVD(bool use_umeyama = true) : use_umeyama_(use_umeyama) {}
+
+  ~TransformationEstimationSVD(){};
+
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using SVD. \param[in] cloud_src the source point cloud dataset
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              Matrix4& transformation_matrix) const override;
+
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using SVD. \param[in] cloud_src the source point cloud dataset
+   * \param[in] indices_src the vector of indices describing the points of interest in
+   * \a cloud_src
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::Indices& indices_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              Matrix4& transformation_matrix) const override;
+
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using SVD. \param[in] cloud_src the source point cloud dataset
+   * \param[in] indices_src the vector of indices describing the points of interest in
+   * \a cloud_src
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[in] indices_tgt the vector of indices describing the correspondences of the
+   * interest points from \a indices_src
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::Indices& indices_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              const pcl::Indices& indices_tgt,
+                              Matrix4& transformation_matrix) const override;
+
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using SVD. \param[in] cloud_src the source point cloud dataset
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[in] correspondences the vector of correspondences between source and target
+   * point cloud \param[out] transformation_matrix the resultant transformation matrix
+   */
+  void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              const pcl::Correspondences& correspondences,
+                              Matrix4& transformation_matrix) const override;
+
+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<PointSource>& source_it,
+                              ConstCloudIterator<PointTarget>& target_it,
+                              Matrix4& transformation_matrix) const;
+
+  /** \brief Obtain a 4x4 rigid transformation matrix from a correlation matrix H = src
+   * * tgt' \param[in] cloud_src_demean the input source cloud, demeaned, in Eigen
+   * format \param[in] centroid_src the input source centroid, in Eigen format
+   * \param[in] cloud_tgt_demean the input target cloud, demeaned, in Eigen format
+   * \param[in] centroid_tgt the input target cloud, in Eigen format
+   * \param[out] transformation_matrix the resultant 4x4 rigid transformation matrix
+   */
+  virtual void
+  getTransformationFromCorrelation(
+      const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_src_demean,
+      const Eigen::Matrix<Scalar, 4, 1>& centroid_src,
+      const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_tgt_demean,
+      const Eigen::Matrix<Scalar, 4, 1>& centroid_tgt,
+      Matrix4& transformation_matrix) const;
+
+  bool use_umeyama_;
+};
+
+} // namespace registration
+} // namespace pcl
 
 #include <pcl/registration/impl/transformation_estimation_svd.hpp>
diff --git a/registration/include/pcl/registration/transformation_estimation_svd_scale.h b/registration/include/pcl/registration/transformation_estimation_svd_scale.h
index 1df9a9f3..b7804042 100644
--- a/registration/include/pcl/registration/transformation_estimation_svd_scale.h
+++ b/registration/include/pcl/registration/transformation_estimation_svd_scale.h
@@ -42,49 +42,54 @@
 
 #include <pcl/registration/transformation_estimation_svd.h>
 
-namespace pcl
-{
-  namespace registration
-  {
-    /** @b TransformationEstimationSVD implements SVD-based estimation of
-      * the transformation aligning the given correspondences.
-      * Optionally the scale is estimated. Note that the similarity transform might not be optimal for the underlying Frobenius Norm.
-      *
-      * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float.
-      * \author Suat Gedikli
-      * \ingroup registration
-      */
-    template <typename PointSource, typename PointTarget, typename Scalar = float>
-    class TransformationEstimationSVDScale : public TransformationEstimationSVD<PointSource, PointTarget, Scalar>
-    {
-      public:
-        using Ptr = shared_ptr<TransformationEstimationSVDScale<PointSource, PointTarget, Scalar> >;
-        using ConstPtr = shared_ptr<const TransformationEstimationSVDScale<PointSource, PointTarget, Scalar> >;
+namespace pcl {
+namespace registration {
+/** @b TransformationEstimationSVD implements SVD-based estimation of
+ * the transformation aligning the given correspondences.
+ * Optionally the scale is estimated. Note that the similarity transform might not be
+ * optimal for the underlying Frobenius Norm.
+ *
+ * \note The class is templated on the source and target point types as well as on the
+ * output scalar of the transformation matrix (i.e., float or double). Default: float.
+ * \author Suat Gedikli
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename Scalar = float>
+class TransformationEstimationSVDScale
+: public TransformationEstimationSVD<PointSource, PointTarget, Scalar> {
+public:
+  using Ptr =
+      shared_ptr<TransformationEstimationSVDScale<PointSource, PointTarget, Scalar>>;
+  using ConstPtr = shared_ptr<
+      const TransformationEstimationSVDScale<PointSource, PointTarget, Scalar>>;
 
-        using Matrix4 = typename TransformationEstimationSVD<PointSource, PointTarget, Scalar>::Matrix4;
+  using Matrix4 =
+      typename TransformationEstimationSVD<PointSource, PointTarget, Scalar>::Matrix4;
 
-        /** \brief Inherits from TransformationEstimationSVD, but forces it to not use the Umeyama method */
-        TransformationEstimationSVDScale ():
-          TransformationEstimationSVD<PointSource, PointTarget, Scalar> (false)
-      {}
+  /** \brief Inherits from TransformationEstimationSVD, but forces it to not use the
+   * Umeyama method */
+  TransformationEstimationSVDScale()
+  : TransformationEstimationSVD<PointSource, PointTarget, Scalar>(false)
+  {}
 
-      protected:
-        /** \brief Obtain a 4x4 rigid transformation matrix from a correlation matrix H = src * tgt'
-          * \param[in] cloud_src_demean the input source cloud, demeaned, in Eigen format
-          * \param[in] centroid_src the input source centroid, in Eigen format
-          * \param[in] cloud_tgt_demean the input target cloud, demeaned, in Eigen format
-          * \param[in] centroid_tgt the input target cloud, in Eigen format
-          * \param[out] transformation_matrix the resultant 4x4 rigid transformation matrix
-          */ 
-        void
-        getTransformationFromCorrelation (const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_src_demean,
-                                          const Eigen::Matrix<Scalar, 4, 1> &centroid_src,
-                                          const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean,
-                                          const Eigen::Matrix<Scalar, 4, 1> &centroid_tgt,
-                                          Matrix4 &transformation_matrix) const;
-    };
+protected:
+  /** \brief Obtain a 4x4 rigid transformation matrix from a correlation matrix H = src
+   * * tgt' \param[in] cloud_src_demean the input source cloud, demeaned, in Eigen
+   * format \param[in] centroid_src the input source centroid, in Eigen format
+   * \param[in] cloud_tgt_demean the input target cloud, demeaned, in Eigen format
+   * \param[in] centroid_tgt the input target cloud, in Eigen format
+   * \param[out] transformation_matrix the resultant 4x4 rigid transformation matrix
+   */
+  void
+  getTransformationFromCorrelation(
+      const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_src_demean,
+      const Eigen::Matrix<Scalar, 4, 1>& centroid_src,
+      const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_tgt_demean,
+      const Eigen::Matrix<Scalar, 4, 1>& centroid_tgt,
+      Matrix4& transformation_matrix) const;
+};
 
-  }
-}
+} // namespace registration
+} // namespace pcl
 
 #include <pcl/registration/impl/transformation_estimation_svd_scale.hpp>
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 f70bdc78..076281e2 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
@@ -38,125 +38,132 @@
 #pragma once
 
 #include <pcl/registration/transformation_estimation.h>
-#include <pcl/registration/warp_point_rigid.h>
 #include <pcl/cloud_iterator.h>
 
-namespace pcl
-{
-  namespace registration
-  {
-    /** \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
-      *   "Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration", Kok-Lim Low, 2004
-      *   "A Symmetric Objective Function for ICP", Szymon Rusinkiewicz, 2019
-      *
-      * \note The class is templated on the source and target point types as well as on the output scalar of the
-      * transformation matrix (i.e., float or double). Default: float.
-      * \author Matthew Cong
-      * \ingroup registration
-      */
-    template <typename PointSource, typename PointTarget, typename Scalar = float>
-    class TransformationEstimationSymmetricPointToPlaneLLS : public TransformationEstimation<PointSource, PointTarget, Scalar>
-    {
-      public:
-        using Ptr = shared_ptr<TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar> >;
-        using ConstPtr = shared_ptr<const TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar> >;
-
-        using Matrix4 = typename TransformationEstimation<PointSource, PointTarget, Scalar>::Matrix4;
-        using Vector6 = Eigen::Matrix<Scalar, 6, 1>;
-
-        TransformationEstimationSymmetricPointToPlaneLLS () : enforce_same_direction_normals_ (true) {};
-        ~TransformationEstimationSymmetricPointToPlaneLLS () {};
-
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            Matrix4 &transformation_matrix) const override;
-
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const std::vector<int> &indices_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            Matrix4 &transformation_matrix) const override;
-
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from \a indices_src
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const std::vector<int> &indices_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            const std::vector<int> &indices_tgt,
-            Matrix4 &transformation_matrix) const override;
-
-        /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD.
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[in] correspondences the vector of correspondences between source and target point cloud
-          * \param[out] transformation_matrix the resultant transformation matrix
-          */
-        inline void
-        estimateRigidTransformation (
-            const pcl::PointCloud<PointSource> &cloud_src,
-            const pcl::PointCloud<PointTarget> &cloud_tgt,
-            const pcl::Correspondences &correspondences,
-            Matrix4 &transformation_matrix) const override;
-
-        /** \brief Set whether or not to negate source or target normals on a per-point basis such that they point in the same direction.
-        * \param[in] enforce_same_direction_normals whether to negate source or target normals on a per-point basis such that they point in the same direction.
-        */
-        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<PointSource>& source_it,
-                                     ConstCloudIterator<PointTarget>& target_it,
-                                     Matrix4 &transformation_matrix) const;
-
-        /** \brief Construct a 4 by 4 transformation matrix from the provided rotation and translation.
-          * \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
-        constructTransformationMatrix (const Vector6 &parameters,
-                                       Matrix4 &transformation_matrix) const;
-
-      /** \brief Whether or not to negate source and/or target normals such that they point in the same direction */
-        bool enforce_same_direction_normals_;
-
-    };
-  }
-}
+namespace pcl {
+namespace registration {
+/** \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
+ *   "Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration",
+ * Kok-Lim Low, 2004 "A Symmetric Objective Function for ICP", Szymon Rusinkiewicz, 2019
+ *
+ * \note The class is templated on the source and target point types as well as on the
+ * output scalar of the transformation matrix (i.e., float or double). Default: float.
+ * \author Matthew Cong
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename Scalar = float>
+class TransformationEstimationSymmetricPointToPlaneLLS
+: public TransformationEstimation<PointSource, PointTarget, Scalar> {
+public:
+  using Ptr = shared_ptr<TransformationEstimationSymmetricPointToPlaneLLS<PointSource,
+                                                                          PointTarget,
+                                                                          Scalar>>;
+  using ConstPtr =
+      shared_ptr<const TransformationEstimationSymmetricPointToPlaneLLS<PointSource,
+                                                                        PointTarget,
+                                                                        Scalar>>;
+
+  using Matrix4 =
+      typename TransformationEstimation<PointSource, PointTarget, Scalar>::Matrix4;
+  using Vector6 = Eigen::Matrix<Scalar, 6, 1>;
+
+  TransformationEstimationSymmetricPointToPlaneLLS()
+  : enforce_same_direction_normals_(true){};
+  ~TransformationEstimationSymmetricPointToPlaneLLS(){};
+
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using SVD. \param[in] cloud_src the source point cloud dataset
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              Matrix4& transformation_matrix) const override;
+
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using SVD. \param[in] cloud_src the source point cloud dataset
+   * \param[in] indices_src the vector of indices describing the points of interest in
+   * \a cloud_src
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::Indices& indices_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              Matrix4& transformation_matrix) const override;
+
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using SVD. \param[in] cloud_src the source point cloud dataset
+   * \param[in] indices_src the vector of indices describing the points of interest in
+   * \a cloud_src
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[in] indices_tgt the vector of indices describing the correspondences of the
+   * interest points from \a indices_src
+   * \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::Indices& indices_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              const pcl::Indices& indices_tgt,
+                              Matrix4& transformation_matrix) const override;
+
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using SVD. \param[in] cloud_src the source point cloud dataset
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[in] correspondences the vector of correspondences between source and target
+   * point cloud \param[out] transformation_matrix the resultant transformation matrix
+   */
+  inline void
+  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
+                              const pcl::PointCloud<PointTarget>& cloud_tgt,
+                              const pcl::Correspondences& correspondences,
+                              Matrix4& transformation_matrix) const override;
+
+  /** \brief Set whether or not to negate source or target normals on a per-point basis
+   * such that they point in the same direction. \param[in]
+   * enforce_same_direction_normals whether to negate source or target normals on a
+   * per-point basis such that they point in the same direction.
+   */
+  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<PointSource>& source_it,
+                              ConstCloudIterator<PointTarget>& target_it,
+                              Matrix4& transformation_matrix) const;
+
+  /** \brief Construct a 4 by 4 transformation matrix from the provided rotation and
+   * translation. \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
+  constructTransformationMatrix(const Vector6& parameters,
+                                Matrix4& transformation_matrix) const;
+
+  /** \brief Whether or not to negate source and/or target normals such that they point
+   * in the same direction */
+  bool enforce_same_direction_normals_;
+};
+} // namespace registration
+} // namespace pcl
 
 #include <pcl/registration/impl/transformation_estimation_symmetric_point_to_plane_lls.hpp>
diff --git a/registration/include/pcl/registration/transformation_validation.h b/registration/include/pcl/registration/transformation_validation.h
index 586473cc..a6234802 100644
--- a/registration/include/pcl/registration/transformation_validation.h
+++ b/registration/include/pcl/registration/transformation_validation.h
@@ -40,90 +40,90 @@
 
 #pragma once
 
-#include <pcl/correspondence.h>
-#include <pcl/features/feature.h>
 #include <pcl/common/transforms.h>
+#include <pcl/features/feature.h>
 #include <pcl/registration/correspondence_types.h>
+#include <pcl/correspondence.h>
 
-namespace pcl
-{
-  namespace registration
-  {
-    /** \brief TransformationValidation represents the base class for methods
-      * that validate the correctness of a transformation found through \ref TransformationEstimation.
-      *
-      * The inputs for a validation estimation can take any or all of the following:
-      *
-      *   - source point cloud
-      *   - target point cloud
-      *   - estimated transformation between source and target
-      *
-      * The output is in the form of a score or a confidence measure.
-      *
-      * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float.
-      * \author Radu B. Rusu
-      * \ingroup registration
-      */
-    template <typename PointSource, typename PointTarget, typename Scalar = float>
-    class TransformationValidation
-    {
-      public:
-        using Matrix4 = Eigen::Matrix<Scalar, 4, 4>;
-        using Ptr = shared_ptr<TransformationValidation<PointSource, PointTarget, Scalar> >;
-        using ConstPtr = shared_ptr<const TransformationValidation<PointSource, PointTarget, Scalar> >;
+namespace pcl {
+namespace registration {
+/** \brief TransformationValidation represents the base class for methods
+ * that validate the correctness of a transformation found through \ref
+ * TransformationEstimation.
+ *
+ * The inputs for a validation estimation can take any or all of the following:
+ *
+ *   - source point cloud
+ *   - target point cloud
+ *   - estimated transformation between source and target
+ *
+ * The output is in the form of a score or a confidence measure.
+ *
+ * \note The class is templated on the source and target point types as well as on the
+ * output scalar of the transformation matrix (i.e., float or double). Default: float.
+ * \author Radu B. Rusu
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename Scalar = float>
+class TransformationValidation {
+public:
+  using Matrix4 = Eigen::Matrix<Scalar, 4, 4>;
+  using Ptr = shared_ptr<TransformationValidation<PointSource, PointTarget, Scalar>>;
+  using ConstPtr =
+      shared_ptr<const TransformationValidation<PointSource, PointTarget, Scalar>>;
 
-        using PointCloudSource = pcl::PointCloud<PointSource>;
-        using PointCloudSourcePtr = typename PointCloudSource::Ptr;
-        using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+  using PointCloudSource = pcl::PointCloud<PointSource>;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
 
-        using PointCloudTarget = pcl::PointCloud<PointTarget>;
-        using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
-        using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
+  using PointCloudTarget = pcl::PointCloud<PointTarget>;
+  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
+  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
 
-        TransformationValidation () {};
-        virtual ~TransformationValidation () {};
+  TransformationValidation(){};
+  virtual ~TransformationValidation(){};
 
-        /** \brief Validate the given transformation with respect to the input cloud data, and return a score. Pure virtual.
-          *
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the transformation matrix
-          *
-          * \return the score or confidence measure for the given
-          * transformation_matrix with respect to the input data
-          */
-        virtual double
-        validateTransformation (
-            const PointCloudSourceConstPtr &cloud_src,
-            const PointCloudTargetConstPtr &cloud_tgt,
-            const Matrix4 &transformation_matrix) const = 0;
+  /** \brief Validate the given transformation with respect to the input cloud data, and
+   * return a score. Pure virtual.
+   *
+   * \param[in] cloud_src the source point cloud dataset
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[out] transformation_matrix the transformation matrix
+   *
+   * \return the score or confidence measure for the given
+   * transformation_matrix with respect to the input data
+   */
+  virtual double
+  validateTransformation(const PointCloudSourceConstPtr& cloud_src,
+                         const PointCloudTargetConstPtr& cloud_tgt,
+                         const Matrix4& transformation_matrix) const = 0;
 
-        /** \brief Comparator function for deciding which score is better after running the 
-          * validation on multiple transforms. Pure virtual.
-          *
-          * \note For example, for Euclidean distances smaller is better, for inliers the opposite.
-          *
-          * \param[in] score1 the first value
-          * \param[in] score2 the second value
-          *
-          * \return true if score1 is better than score2
-          */
-        virtual bool
-        operator() (const double &score1, const double &score2) const = 0;
+  /** \brief Comparator function for deciding which score is better after running the
+   * validation on multiple transforms. Pure virtual.
+   *
+   * \note For example, for Euclidean distances smaller is better, for inliers the
+   * opposite.
+   *
+   * \param[in] score1 the first value
+   * \param[in] score2 the second value
+   *
+   * \return true if score1 is better than score2
+   */
+  virtual bool
+  operator()(const double& score1, const double& score2) const = 0;
 
-        /** \brief Check if the score is valid for a specific transformation. Pure virtual.
-          *
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the transformation matrix
-          *
-          * \return true if the transformation is valid, false otherwise.
-          */
-        virtual bool
-        isValid (
-            const PointCloudSourceConstPtr &cloud_src,
-            const PointCloudTargetConstPtr &cloud_tgt,
-            const Matrix4 &transformation_matrix) const = 0;
-    };
-  }
-}
+  /** \brief Check if the score is valid for a specific transformation. Pure virtual.
+   *
+   * \param[in] cloud_src the source point cloud dataset
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[out] transformation_matrix the transformation matrix
+   *
+   * \return true if the transformation is valid, false otherwise.
+   */
+  virtual bool
+  isValid(const PointCloudSourceConstPtr& cloud_src,
+          const PointCloudTargetConstPtr& cloud_tgt,
+          const Matrix4& transformation_matrix) const = 0;
+};
+} // namespace registration
+} // namespace pcl
diff --git a/registration/include/pcl/registration/transformation_validation_euclidean.h b/registration/include/pcl/registration/transformation_validation_euclidean.h
index 5d2cb4ad..34bbc088 100644
--- a/registration/include/pcl/registration/transformation_validation_euclidean.h
+++ b/registration/include/pcl/registration/transformation_validation_euclidean.h
@@ -40,229 +40,228 @@
 
 #pragma once
 
+#include <pcl/kdtree/kdtree.h>
+#include <pcl/registration/transformation_validation.h>
+#include <pcl/search/kdtree.h>
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
 #include <pcl/point_representation.h>
-#include <pcl/search/kdtree.h>
-#include <pcl/kdtree/kdtree.h>
-#include <pcl/registration/transformation_validation.h>
 
-namespace pcl
-{
-  namespace registration
-  {
-    /** \brief TransformationValidationEuclidean computes an L2SQR norm between a source and target
-      * dataset.
-      * 
-      * To prevent points with bad correspondences to contribute to the overall score, the class also 
-      * accepts a maximum_range parameter given via \ref setMaxRange that is used as a cutoff value for
-      * nearest neighbor distance comparisons.
-      * 
-      * The output score is normalized with respect to the number of valid correspondences found.
-      *
-      * Usage example:
-      * \code
-      * pcl::TransformationValidationEuclidean<pcl::PointXYZ, pcl::PointXYZ> tve;
-      * tve.setMaxRange (0.01);  // 1cm
-      * double score = tve.validateTransformation (source, target, transformation);
-      * \endcode
-      *
-      * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float.
-      * \author Radu B. Rusu
-      * \ingroup registration
-      */
-    template <typename PointSource, typename PointTarget, typename Scalar = float>
-    class TransformationValidationEuclidean
-    {
-      public:
-        using Matrix4 = typename TransformationValidation<PointSource, PointTarget, Scalar>::Matrix4;
-        
-        using Ptr = shared_ptr<TransformationValidation<PointSource, PointTarget, Scalar> >;
-        using ConstPtr = shared_ptr<const TransformationValidation<PointSource, PointTarget, Scalar> >;
+namespace pcl {
+namespace registration {
+/** \brief TransformationValidationEuclidean computes an L2SQR norm between a source and
+ * target dataset.
+ *
+ * To prevent points with bad correspondences to contribute to the overall score, the
+ * class also accepts a maximum_range parameter given via \ref setMaxRange that is used
+ * as a cutoff value for nearest neighbor distance comparisons.
+ *
+ * The output score is normalized with respect to the number of valid correspondences
+ * found.
+ *
+ * Usage example:
+ * \code
+ * pcl::TransformationValidationEuclidean<pcl::PointXYZ, pcl::PointXYZ> tve;
+ * tve.setMaxRange (0.01);  // 1cm
+ * double score = tve.validateTransformation (source, target, transformation);
+ * \endcode
+ *
+ * \note The class is templated on the source and target point types as well as on the
+ * output scalar of the transformation matrix (i.e., float or double). Default: float.
+ * \author Radu B. Rusu
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename Scalar = float>
+class TransformationValidationEuclidean {
+public:
+  using Matrix4 =
+      typename TransformationValidation<PointSource, PointTarget, Scalar>::Matrix4;
+
+  using Ptr = shared_ptr<TransformationValidation<PointSource, PointTarget, Scalar>>;
+  using ConstPtr =
+      shared_ptr<const TransformationValidation<PointSource, PointTarget, Scalar>>;
 
-        using KdTree = pcl::search::KdTree<PointTarget>;
-        using KdTreePtr = typename KdTree::Ptr;
+  using KdTree = pcl::search::KdTree<PointTarget>;
+  using KdTreePtr = typename KdTree::Ptr;
 
-        using PointRepresentationConstPtr = typename KdTree::PointRepresentationConstPtr;
+  using PointRepresentationConstPtr = typename KdTree::PointRepresentationConstPtr;
 
-        using PointCloudSourceConstPtr = typename TransformationValidation<PointSource, PointTarget>::PointCloudSourceConstPtr;
-        using PointCloudTargetConstPtr = typename TransformationValidation<PointSource, PointTarget>::PointCloudTargetConstPtr;
+  using PointCloudSourceConstPtr =
+      typename TransformationValidation<PointSource,
+                                        PointTarget>::PointCloudSourceConstPtr;
+  using PointCloudTargetConstPtr =
+      typename TransformationValidation<PointSource,
+                                        PointTarget>::PointCloudTargetConstPtr;
 
-        /** \brief Constructor.
-          * Sets the \a max_range parameter to double::max, \a threshold_ to NaN
-          * and initializes the internal search \a tree to a FLANN kd-tree.
-          */
-        TransformationValidationEuclidean () : 
-          max_range_ (std::numeric_limits<double>::max ()),
-          threshold_ (std::numeric_limits<double>::quiet_NaN ()),
-          tree_ (new pcl::search::KdTree<PointTarget>),
-          force_no_recompute_ (false)
-        {
-        }
+  /** \brief Constructor.
+   * Sets the \a max_range parameter to double::max, \a threshold_ to NaN
+   * and initializes the internal search \a tree to a FLANN kd-tree.
+   */
+  TransformationValidationEuclidean()
+  : max_range_(std::numeric_limits<double>::max())
+  , threshold_(std::numeric_limits<double>::quiet_NaN())
+  , tree_(new pcl::search::KdTree<PointTarget>)
+  , force_no_recompute_(false)
+  {}
 
-        virtual ~TransformationValidationEuclidean () {};
+  virtual ~TransformationValidationEuclidean(){};
+
+  /** \brief Set the maximum allowable distance between a point and its correspondence
+   * in the target in order for a correspondence to be considered \a valid. Default:
+   * double::max. \param[in] max_range the new maximum allowable distance
+   */
+  inline void
+  setMaxRange(double max_range)
+  {
+    max_range_ = max_range;
+  }
 
-        /** \brief Set the maximum allowable distance between a point and its correspondence in the 
-          * target in order for a correspondence to be considered \a valid. Default: double::max.
-          * \param[in] max_range the new maximum allowable distance
-          */
-        inline void
-        setMaxRange (double max_range)
-        {
-          max_range_ = max_range;
-        }
+  /** \brief Get the maximum allowable distance between a point and its
+   * correspondence, as set by the user.
+   */
+  inline double
+  getMaxRange()
+  {
+    return (max_range_);
+  }
 
-        /** \brief Get the maximum allowable distance between a point and its 
-          * correspondence, as set by the user.
-          */
-        inline double
-        getMaxRange ()
-        {
-          return (max_range_);
-        }
+  /** \brief Provide a pointer to the search object used to find correspondences in
+   * the target cloud.
+   * \param[in] tree a pointer to the spatial search object.
+   * \param[in] force_no_recompute If set to true, this tree will NEVER be
+   * recomputed, regardless of calls to setInputTarget. Only use if you are
+   * confident that the tree will be set correctly.
+   */
+  inline void
+  setSearchMethodTarget(const KdTreePtr& tree, bool force_no_recompute = false)
+  {
+    tree_ = tree;
+    force_no_recompute_ = force_no_recompute;
+  }
 
+  /** \brief Set a threshold for which a specific transformation is considered valid.
+   *
+   * \note Since we're using MSE (Mean Squared Error) as a metric, the threshold
+   * represents the mean Euclidean distance threshold over all nearest neighbors
+   * up to max_range.
+   *
+   * \param[in] threshold the threshold for which a transformation is vali
+   */
+  inline void
+  setThreshold(double threshold)
+  {
+    threshold_ = threshold;
+  }
 
-        /** \brief Provide a pointer to the search object used to find correspondences in
-          * the target cloud.
-          * \param[in] tree a pointer to the spatial search object.
-          * \param[in] force_no_recompute If set to true, this tree will NEVER be 
-          * recomputed, regardless of calls to setInputTarget. Only use if you are 
-          * confident that the tree will be set correctly.
-          */
-        inline void
-        setSearchMethodTarget (const KdTreePtr &tree, 
-                               bool force_no_recompute = false) 
-        { 
-          tree_ = tree; 
-          if (force_no_recompute)
-          {
-            force_no_recompute_ = true;
-          }
-        }
+  /** \brief Get the threshold for which a specific transformation is valid. */
+  inline double
+  getThreshold()
+  {
+    return (threshold_);
+  }
 
-        /** \brief Set a threshold for which a specific transformation is considered valid.
-          *
-          * \note Since we're using MSE (Mean Squared Error) as a metric, the threshold
-          * represents the mean Euclidean distance threshold over all nearest neighbors
-          * up to max_range.
-          *
-          * \param[in] threshold the threshold for which a transformation is vali
-          */
-        inline void
-        setThreshold (double threshold)
-        {
-          threshold_ = threshold;
-        }
+  /** \brief Validate the given transformation with respect to the input cloud data, and
+   * return a score.
+   *
+   * \param[in] cloud_src the source point cloud dataset
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[out] transformation_matrix the resultant transformation matrix
+   *
+   * \return the score or confidence measure for the given
+   * transformation_matrix with respect to the input data
+   */
+  double
+  validateTransformation(const PointCloudSourceConstPtr& cloud_src,
+                         const PointCloudTargetConstPtr& cloud_tgt,
+                         const Matrix4& transformation_matrix) const;
 
-        /** \brief Get the threshold for which a specific transformation is valid. */
-        inline double
-        getThreshold ()
-        {
-          return (threshold_);
-        }
+  /** \brief Comparator function for deciding which score is better after running the
+   * validation on multiple transforms.
+   *
+   * \param[in] score1 the first value
+   * \param[in] score2 the second value
+   *
+   * \return true if score1 is better than score2
+   */
+  virtual bool
+  operator()(const double& score1, const double& score2) const
+  {
+    return (score1 < score2);
+  }
 
-        /** \brief Validate the given transformation with respect to the input cloud data, and return a score.
-          *
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the resultant transformation matrix
-          *
-          * \return the score or confidence measure for the given
-          * transformation_matrix with respect to the input data
-          */
-        double
-        validateTransformation (
-            const PointCloudSourceConstPtr &cloud_src,
-            const PointCloudTargetConstPtr &cloud_tgt,
-            const Matrix4 &transformation_matrix) const;
+  /** \brief Check if the score is valid for a specific transformation.
+   *
+   * \param[in] cloud_src the source point cloud dataset
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[out] transformation_matrix the transformation matrix
+   *
+   * \return true if the transformation is valid, false otherwise.
+   */
+  virtual bool
+  isValid(const PointCloudSourceConstPtr& cloud_src,
+          const PointCloudTargetConstPtr& cloud_tgt,
+          const Matrix4& transformation_matrix) const
+  {
+    if (std::isnan(threshold_)) {
+      PCL_ERROR("[pcl::TransformationValidationEuclidean::isValid] Threshold not set! "
+                "Please use setThreshold () before continuing.\n");
+      return (false);
+    }
 
-        /** \brief Comparator function for deciding which score is better after running the 
-          * validation on multiple transforms.
-          *
-          * \param[in] score1 the first value
-          * \param[in] score2 the second value
-          *
-          * \return true if score1 is better than score2
-          */
-        virtual bool
-        operator() (const double &score1, const double &score2) const
-        {
-          return (score1 < score2);
-        }
+    return (validateTransformation(cloud_src, cloud_tgt, transformation_matrix) <
+            threshold_);
+  }
 
-        /** \brief Check if the score is valid for a specific transformation.
-          *
-          * \param[in] cloud_src the source point cloud dataset
-          * \param[in] cloud_tgt the target point cloud dataset
-          * \param[out] transformation_matrix the transformation matrix
-          *
-          * \return true if the transformation is valid, false otherwise.
-          */
-        virtual bool
-        isValid (
-            const PointCloudSourceConstPtr &cloud_src,
-            const PointCloudTargetConstPtr &cloud_tgt,
-            const Matrix4 &transformation_matrix) const
-        {
-          if (std::isnan (threshold_))
-          {
-            PCL_ERROR ("[pcl::TransformationValidationEuclidean::isValid] Threshold not set! Please use setThreshold () before continuing.");
-            return (false);
-          }
+protected:
+  /** \brief The maximum allowable distance between a point and its correspondence in
+   * the target in order for a correspondence to be considered \a valid. Default:
+   * double::max.
+   */
+  double max_range_;
 
-          return (validateTransformation (cloud_src, cloud_tgt, transformation_matrix) < threshold_);
-        }
+  /** \brief The threshold for which a specific transformation is valid.
+   * Set to NaN by default, as we must require the user to set it.
+   */
+  double threshold_;
 
-      protected:
-        /** \brief The maximum allowable distance between a point and its correspondence in the target 
-          * in order for a correspondence to be considered \a valid. Default: double::max.
-          */
-        double max_range_;
+  /** \brief A pointer to the spatial search object. */
+  KdTreePtr tree_;
 
-        /** \brief The threshold for which a specific transformation is valid. 
-          * Set to NaN by default, as we must require the user to set it.
-          */
-        double threshold_;
+  /** \brief A flag which, if set, means the tree operating on the target cloud
+   * will never be recomputed*/
+  bool force_no_recompute_;
 
-        /** \brief A pointer to the spatial search object. */
-        KdTreePtr tree_;
+  /** \brief Internal point representation uses only 3D coordinates for L2 */
+  class MyPointRepresentation : public pcl::PointRepresentation<PointTarget> {
+    using pcl::PointRepresentation<PointTarget>::nr_dimensions_;
+    using pcl::PointRepresentation<PointTarget>::trivial_;
 
-        /** \brief A flag which, if set, means the tree operating on the target cloud 
-         * will never be recomputed*/
-        bool force_no_recompute_;
+  public:
+    using Ptr = shared_ptr<MyPointRepresentation>;
+    using ConstPtr = shared_ptr<const MyPointRepresentation>;
 
+    MyPointRepresentation()
+    {
+      nr_dimensions_ = 3;
+      trivial_ = true;
+    }
 
-        /** \brief Internal point representation uses only 3D coordinates for L2 */
-        class MyPointRepresentation: public pcl::PointRepresentation<PointTarget>
-        {
-          using pcl::PointRepresentation<PointTarget>::nr_dimensions_;
-          using pcl::PointRepresentation<PointTarget>::trivial_;
-          public:
-            using Ptr = shared_ptr<MyPointRepresentation>;
-            using ConstPtr = shared_ptr<const MyPointRepresentation>;
-            
-            MyPointRepresentation ()
-            {
-              nr_dimensions_ = 3;
-              trivial_ = true;
-            }
-      
-            /** \brief Empty destructor */
-            virtual ~MyPointRepresentation () {}
+    /** \brief Empty destructor */
+    virtual ~MyPointRepresentation() {}
 
-            virtual void
-            copyToFloatArray (const PointTarget &p, float * out) const
-            {
-              out[0] = p.x;
-              out[1] = p.y;
-              out[2] = p.z;
-            }
-        };
+    virtual void
+    copyToFloatArray(const PointTarget& p, float* out) const
+    {
+      out[0] = p.x;
+      out[1] = p.y;
+      out[2] = p.z;
+    }
+  };
 
-      public:
-        PCL_MAKE_ALIGNED_OPERATOR_NEW
-    };
-  }
-}
+public:
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+};
+} // namespace registration
+} // namespace pcl
 
 #include <pcl/registration/impl/transformation_validation_euclidean.hpp>
diff --git a/registration/include/pcl/registration/transforms.h b/registration/include/pcl/registration/transforms.h
index 3101974d..2f4bfb79 100644
--- a/registration/include/pcl/registration/transforms.h
+++ b/registration/include/pcl/registration/transforms.h
@@ -39,5 +39,5 @@
  */
 
 #pragma once
-
+PCL_DEPRECATED_HEADER(1, 15, "Please include pcl/common/transforms.h directly.")
 #include <pcl/common/transforms.h>
diff --git a/registration/include/pcl/registration/vertex_estimates.h b/registration/include/pcl/registration/vertex_estimates.h
index 71603533..f7a26343 100644
--- a/registration/include/pcl/registration/vertex_estimates.h
+++ b/registration/include/pcl/registration/vertex_estimates.h
@@ -42,30 +42,28 @@
 
 #include <pcl/point_cloud.h>
 
-namespace pcl
-{
-  namespace registration
-  {
-    /** \brief @b NullEstimate struct
-      * \author Nicola Fioraio
-      * \ingroup registration
-      */
-    struct NullEstimate
-    {};
+namespace pcl {
+namespace registration {
+/** \brief @b NullEstimate struct
+ * \author Nicola Fioraio
+ * \ingroup registration
+ */
+struct NullEstimate {};
 
-    /** \brief @b PoseEstimate struct
-      * \author Nicola Fioraio
-      * \ingroup registration
-      */
-    template <typename PointT>
-    struct PoseEstimate
-    {
-      Eigen::Matrix4f pose;
-      typename pcl::PointCloud<PointT>::ConstPtr cloud;
+/** \brief @b PoseEstimate struct
+ * \author Nicola Fioraio
+ * \ingroup registration
+ */
+template <typename PointT>
+struct PoseEstimate {
+  Eigen::Matrix4f pose;
+  typename pcl::PointCloud<PointT>::ConstPtr cloud;
 
-      PoseEstimate (const Eigen::Matrix4f& p = Eigen::Matrix4f::Identity(),
-                    const typename pcl::PointCloud<PointT>::ConstPtr& c = typename pcl::PointCloud<PointT>::ConstPtr())
-        : pose (p), cloud (c) {}
-    };
-  }
-}
+  PoseEstimate(const Eigen::Matrix4f& p = Eigen::Matrix4f::Identity(),
+               const typename pcl::PointCloud<PointT>::ConstPtr& c =
+                   typename pcl::PointCloud<PointT>::ConstPtr())
+  : pose(p), cloud(c)
+  {}
+};
+} // namespace registration
+} // namespace pcl
diff --git a/registration/include/pcl/registration/warp_point_rigid.h b/registration/include/pcl/registration/warp_point_rigid.h
index 6c1a76d4..a5c5b5c6 100644
--- a/registration/include/pcl/registration/warp_point_rigid.h
+++ b/registration/include/pcl/registration/warp_point_rigid.h
@@ -42,91 +42,104 @@
 
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
-#include <pcl/registration/eigen.h>
 
-namespace pcl
-{
-  namespace registration
-  {
-    /** \brief Base warp point class. 
-      * 
-      * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float.
-      * \author Radu B. Rusu
-      * \ingroup registration
-      */
-    template <typename PointSourceT, typename PointTargetT, typename Scalar = float>
-    class WarpPointRigid
-    {
-      public:
-        using Matrix4 = Eigen::Matrix<Scalar, 4, 4>;
-        using VectorX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
-        using Vector4 = Eigen::Matrix<Scalar, 4, 1>;
+namespace pcl {
+namespace registration {
+/** \brief Base warp point class.
+ *
+ * \note The class is templated on the source and target point types as well as on the
+ * output scalar of the transformation matrix (i.e., float or double). Default: float.
+ * \author Radu B. Rusu
+ * \ingroup registration
+ */
+template <typename PointSourceT, typename PointTargetT, typename Scalar = float>
+class WarpPointRigid {
+public:
+  using Matrix4 = Eigen::Matrix<Scalar, 4, 4>;
+  using VectorX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
+  using Vector4 = Eigen::Matrix<Scalar, 4, 1>;
 
-        using Ptr = shared_ptr<WarpPointRigid<PointSourceT, PointTargetT, Scalar> >;
-        using ConstPtr = shared_ptr<const WarpPointRigid<PointSourceT, PointTargetT, Scalar> >;
+  using Ptr = shared_ptr<WarpPointRigid<PointSourceT, PointTargetT, Scalar>>;
+  using ConstPtr = shared_ptr<const WarpPointRigid<PointSourceT, PointTargetT, Scalar>>;
 
-        /** \brief Constructor
-          * \param[in] nr_dim the number of dimensions
-          */
-        WarpPointRigid (int nr_dim)
-          : nr_dim_ (nr_dim)
-          , transform_matrix_ (Matrix4::Zero ())
-        {
-          transform_matrix_ (3, 3) = 1.0;
-        };
+  /** \brief Constructor
+   * \param[in] nr_dim the number of dimensions
+   */
+  WarpPointRigid(int nr_dim) : nr_dim_(nr_dim), transform_matrix_(Matrix4::Zero())
+  {
+    transform_matrix_(3, 3) = 1.0;
+  };
 
-        /** \brief Destructor. */
-        virtual ~WarpPointRigid () {};
+  /** \brief Destructor. */
+  virtual ~WarpPointRigid(){};
 
-        /** \brief Set warp parameters. Pure virtual.
-          * \param[in] p warp parameters 
-          */
-        virtual void 
-        setParam (const VectorX& p) = 0;
+  /** \brief Set warp parameters. Pure virtual.
+   * \param[in] p warp parameters
+   */
+  virtual void
+  setParam(const VectorX& p) = 0;
 
-        /** \brief Warp a point given a transformation matrix
-          * \param[in] pnt_in the point to warp (transform)
-          * \param[out] pnt_out the warped (transformed) point
-          */
-        inline void 
-        warpPoint (const PointSourceT& pnt_in, PointSourceT& pnt_out) const
-        {
-          pnt_out.x = static_cast<float> (transform_matrix_ (0, 0) * pnt_in.x + transform_matrix_ (0, 1) * pnt_in.y + transform_matrix_ (0, 2) * pnt_in.z + transform_matrix_ (0, 3));
-          pnt_out.y = static_cast<float> (transform_matrix_ (1, 0) * pnt_in.x + transform_matrix_ (1, 1) * pnt_in.y + transform_matrix_ (1, 2) * pnt_in.z + transform_matrix_ (1, 3));
-          pnt_out.z = static_cast<float> (transform_matrix_ (2, 0) * pnt_in.x + transform_matrix_ (2, 1) * pnt_in.y + transform_matrix_ (2, 2) * pnt_in.z + transform_matrix_ (2, 3));
-          //pnt_out.getVector3fMap () = transform_matrix_.topLeftCorner (3, 3) * 
-          //                            pnt_in.getVector3fMap () + 
-          //                            transform_matrix_.block (0, 3, 3, 1);
-          //pnt_out.data[3] = pnt_in.data[3];
-        }
+  /** \brief Warp a point given a transformation matrix
+   * \param[in] pnt_in the point to warp (transform)
+   * \param[out] pnt_out the warped (transformed) point
+   */
+  inline void
+  warpPoint(const PointSourceT& pnt_in, PointSourceT& pnt_out) const
+  {
+    pnt_out.x = static_cast<float>(
+        transform_matrix_(0, 0) * pnt_in.x + transform_matrix_(0, 1) * pnt_in.y +
+        transform_matrix_(0, 2) * pnt_in.z + transform_matrix_(0, 3));
+    pnt_out.y = static_cast<float>(
+        transform_matrix_(1, 0) * pnt_in.x + transform_matrix_(1, 1) * pnt_in.y +
+        transform_matrix_(1, 2) * pnt_in.z + transform_matrix_(1, 3));
+    pnt_out.z = static_cast<float>(
+        transform_matrix_(2, 0) * pnt_in.x + transform_matrix_(2, 1) * pnt_in.y +
+        transform_matrix_(2, 2) * pnt_in.z + transform_matrix_(2, 3));
+    // pnt_out.getVector3fMap () = transform_matrix_.topLeftCorner (3, 3) *
+    //                            pnt_in.getVector3fMap () +
+    //                            transform_matrix_.block (0, 3, 3, 1);
+    // pnt_out.data[3] = pnt_in.data[3];
+  }
 
-        /** \brief Warp a point given a transformation matrix
-          * \param[in] pnt_in the point to warp (transform)
-          * \param[out] pnt_out the warped (transformed) point
-          */
-        inline void 
-        warpPoint (const PointSourceT& pnt_in, Vector4& pnt_out) const
-        {
-          pnt_out[0] = static_cast<Scalar> (transform_matrix_ (0, 0) * pnt_in.x + transform_matrix_ (0, 1) * pnt_in.y + transform_matrix_ (0, 2) * pnt_in.z + transform_matrix_ (0, 3));
-          pnt_out[1] = static_cast<Scalar> (transform_matrix_ (1, 0) * pnt_in.x + transform_matrix_ (1, 1) * pnt_in.y + transform_matrix_ (1, 2) * pnt_in.z + transform_matrix_ (1, 3));
-          pnt_out[2] = static_cast<Scalar> (transform_matrix_ (2, 0) * pnt_in.x + transform_matrix_ (2, 1) * pnt_in.y + transform_matrix_ (2, 2) * pnt_in.z + transform_matrix_ (2, 3));
-          pnt_out[3] = 0.0;
-        }
+  /** \brief Warp a point given a transformation matrix
+   * \param[in] pnt_in the point to warp (transform)
+   * \param[out] pnt_out the warped (transformed) point
+   */
+  inline void
+  warpPoint(const PointSourceT& pnt_in, Vector4& pnt_out) const
+  {
+    pnt_out[0] = static_cast<Scalar>(
+        transform_matrix_(0, 0) * pnt_in.x + transform_matrix_(0, 1) * pnt_in.y +
+        transform_matrix_(0, 2) * pnt_in.z + transform_matrix_(0, 3));
+    pnt_out[1] = static_cast<Scalar>(
+        transform_matrix_(1, 0) * pnt_in.x + transform_matrix_(1, 1) * pnt_in.y +
+        transform_matrix_(1, 2) * pnt_in.z + transform_matrix_(1, 3));
+    pnt_out[2] = static_cast<Scalar>(
+        transform_matrix_(2, 0) * pnt_in.x + transform_matrix_(2, 1) * pnt_in.y +
+        transform_matrix_(2, 2) * pnt_in.z + transform_matrix_(2, 3));
+    pnt_out[3] = 0.0;
+  }
+
+  /** \brief Get the number of dimensions. */
+  inline int
+  getDimension() const
+  {
+    return (nr_dim_);
+  }
 
-        /** \brief Get the number of dimensions. */
-        inline int 
-        getDimension () const { return (nr_dim_); }
+  /** \brief Get the Transform used. */
+  inline const Matrix4&
+  getTransform() const
+  {
+    return (transform_matrix_);
+  }
 
-        /** \brief Get the Transform used. */
-        inline const Matrix4& 
-        getTransform () const { return (transform_matrix_); }
-        
-      public:
-        PCL_MAKE_ALIGNED_OPERATOR_NEW
+public:
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
 
-      protected:
-        int nr_dim_;
-        Matrix4 transform_matrix_;
-    };
-  } // namespace registration
+protected:
+  int nr_dim_;
+  Matrix4 transform_matrix_;
+};
+} // namespace registration
 } // namespace pcl
diff --git a/registration/include/pcl/registration/warp_point_rigid_3d.h b/registration/include/pcl/registration/warp_point_rigid_3d.h
index cc3cd7c1..2e6b4b03 100644
--- a/registration/include/pcl/registration/warp_point_rigid_3d.h
+++ b/registration/include/pcl/registration/warp_point_rigid_3d.h
@@ -40,56 +40,54 @@
 
 #pragma once
 
-#include <pcl/registration/eigen.h>
 #include <pcl/registration/warp_point_rigid.h>
 
-namespace pcl
-{
-  namespace registration
-  {
-    /** \brief @b WarpPointRigid3D enables 3D (1D rotation + 2D translation) 
-      * transformations for points.
-      * 
-      * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float.
-      * \author Radu B. Rusu
-      * \ingroup registration
-      */
-    template <typename PointSourceT, typename PointTargetT, typename Scalar = float>
-    class WarpPointRigid3D : public WarpPointRigid<PointSourceT, PointTargetT, Scalar>
-    {
-      public:
-        using Matrix4 = typename WarpPointRigid<PointSourceT, PointTargetT, Scalar>::Matrix4;
-        using VectorX = typename WarpPointRigid<PointSourceT, PointTargetT, Scalar>::VectorX;
+namespace pcl {
+namespace registration {
+/** \brief @b WarpPointRigid3D enables 3D (1D rotation + 2D translation)
+ * transformations for points.
+ *
+ * \note The class is templated on the source and target point types as well as on the
+ * output scalar of the transformation matrix (i.e., float or double). Default: float.
+ * \author Radu B. Rusu
+ * \ingroup registration
+ */
+template <typename PointSourceT, typename PointTargetT, typename Scalar = float>
+class WarpPointRigid3D : public WarpPointRigid<PointSourceT, PointTargetT, Scalar> {
+public:
+  using Matrix4 = typename WarpPointRigid<PointSourceT, PointTargetT, Scalar>::Matrix4;
+  using VectorX = typename WarpPointRigid<PointSourceT, PointTargetT, Scalar>::VectorX;
 
-        using Ptr = shared_ptr<WarpPointRigid3D<PointSourceT, PointTargetT, Scalar> >;
-        using ConstPtr = shared_ptr<const WarpPointRigid3D<PointSourceT, PointTargetT, Scalar> >;
+  using Ptr = shared_ptr<WarpPointRigid3D<PointSourceT, PointTargetT, Scalar>>;
+  using ConstPtr =
+      shared_ptr<const WarpPointRigid3D<PointSourceT, PointTargetT, Scalar>>;
 
-        /** \brief Constructor. */
-        WarpPointRigid3D () : WarpPointRigid<PointSourceT, PointTargetT, Scalar> (3) {}
-      
-        /** \brief Empty destructor */
-        ~WarpPointRigid3D () {}
+  /** \brief Constructor. */
+  WarpPointRigid3D() : WarpPointRigid<PointSourceT, PointTargetT, Scalar>(3) {}
 
-        /** \brief Set warp parameters. 
-          * \param[in] p warp parameters (tx ty rz)
-          */
-        void 
-        setParam (const VectorX & p) override
-        {
-          assert (p.rows () == this->getDimension ());
-          Matrix4 &trans = this->transform_matrix_;
+  /** \brief Empty destructor */
+  ~WarpPointRigid3D() {}
+
+  /** \brief Set warp parameters.
+   * \param[in] p warp parameters (tx ty rz)
+   */
+  void
+  setParam(const VectorX& p) override
+  {
+    assert(p.rows() == this->getDimension());
+    Matrix4& trans = this->transform_matrix_;
 
-          trans = Matrix4::Zero ();
-          trans (3, 3) = 1;
-          trans (2, 2) = 1; // Rotation around the Z-axis
+    trans = Matrix4::Zero();
+    trans(3, 3) = 1;
+    trans(2, 2) = 1; // Rotation around the Z-axis
 
-          // Copy the rotation and translation components
-          trans.block (0, 3, 4, 1) = Eigen::Matrix<Scalar, 4, 1> (p[0], p[1], 0, 1.0);
+    // Copy the rotation and translation components
+    trans.block(0, 3, 4, 1) = Eigen::Matrix<Scalar, 4, 1>(p[0], p[1], 0, 1.0);
 
-          // Compute w from the unit quaternion
-          Eigen::Rotation2D<Scalar> r (p[2]);
-          trans.topLeftCorner (2, 2) = r.toRotationMatrix ();
-        }
-    };
+    // Compute w from the unit quaternion
+    Eigen::Rotation2D<Scalar> r(p[2]);
+    trans.topLeftCorner(2, 2) = r.toRotationMatrix();
   }
-}
+};
+} // namespace registration
+} // namespace pcl
diff --git a/registration/include/pcl/registration/warp_point_rigid_6d.h b/registration/include/pcl/registration/warp_point_rigid_6d.h
index a8aaa65e..0b5f95c2 100644
--- a/registration/include/pcl/registration/warp_point_rigid_6d.h
+++ b/registration/include/pcl/registration/warp_point_rigid_6d.h
@@ -42,56 +42,55 @@
 
 #include <pcl/registration/warp_point_rigid.h>
 
-namespace pcl
-{
-  namespace registration
-  {
-    /** \brief @b WarpPointRigid3D enables 6D (3D rotation + 3D translation) 
-      * transformations for points.
-      * 
-      * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float.
-      * \author Radu B. Rusu
-      * \ingroup registration
-      */
-    template <typename PointSourceT, typename PointTargetT, typename Scalar = float>
-    class WarpPointRigid6D : public WarpPointRigid<PointSourceT, PointTargetT, Scalar>
-    {
-      public:
-        using WarpPointRigid<PointSourceT, PointTargetT, Scalar>::transform_matrix_;
+namespace pcl {
+namespace registration {
+/** \brief @b WarpPointRigid3D enables 6D (3D rotation + 3D translation)
+ * transformations for points.
+ *
+ * \note The class is templated on the source and target point types as well as on the
+ * output scalar of the transformation matrix (i.e., float or double). Default: float.
+ * \author Radu B. Rusu
+ * \ingroup registration
+ */
+template <typename PointSourceT, typename PointTargetT, typename Scalar = float>
+class WarpPointRigid6D : public WarpPointRigid<PointSourceT, PointTargetT, Scalar> {
+public:
+  using WarpPointRigid<PointSourceT, PointTargetT, Scalar>::transform_matrix_;
 
-        using Matrix4 = typename WarpPointRigid<PointSourceT, PointTargetT, Scalar>::Matrix4;
-        using VectorX = typename WarpPointRigid<PointSourceT, PointTargetT, Scalar>::VectorX;
+  using Matrix4 = typename WarpPointRigid<PointSourceT, PointTargetT, Scalar>::Matrix4;
+  using VectorX = typename WarpPointRigid<PointSourceT, PointTargetT, Scalar>::VectorX;
 
-        using Ptr = shared_ptr<WarpPointRigid6D<PointSourceT, PointTargetT, Scalar> >;
-        using ConstPtr = shared_ptr<const WarpPointRigid6D<PointSourceT, PointTargetT, Scalar> >;
+  using Ptr = shared_ptr<WarpPointRigid6D<PointSourceT, PointTargetT, Scalar>>;
+  using ConstPtr =
+      shared_ptr<const WarpPointRigid6D<PointSourceT, PointTargetT, Scalar>>;
 
-        WarpPointRigid6D () : WarpPointRigid<PointSourceT, PointTargetT, Scalar> (6) {}
-      
-        /** \brief Empty destructor */
-        ~WarpPointRigid6D () {}
+  WarpPointRigid6D() : WarpPointRigid<PointSourceT, PointTargetT, Scalar>(6) {}
+
+  /** \brief Empty destructor */
+  ~WarpPointRigid6D() {}
+
+  /** \brief Set warp parameters.
+   * \note Assumes the quaternion parameters are normalized.
+   * \param[in] p warp parameters (tx ty tz qx qy qz)
+   */
+  void
+  setParam(const VectorX& p) override
+  {
+    assert(p.rows() == this->getDimension());
 
-        /** \brief Set warp parameters. 
-          * \note Assumes the quaternion parameters are normalized. 
-          * \param[in] p warp parameters (tx ty tz qx qy qz)
-          */
-        void 
-        setParam (const VectorX& p) override
-        {
-          assert (p.rows () == this->getDimension ());
+    // Copy the rotation and translation components
+    transform_matrix_.setZero();
+    transform_matrix_(0, 3) = p[0];
+    transform_matrix_(1, 3) = p[1];
+    transform_matrix_(2, 3) = p[2];
+    transform_matrix_(3, 3) = 1;
 
-          // Copy the rotation and translation components
-          transform_matrix_.setZero ();
-          transform_matrix_ (0, 3) = p[0];
-          transform_matrix_ (1, 3) = p[1];
-          transform_matrix_ (2, 3) = p[2];
-          transform_matrix_ (3, 3) = 1;
-          
-          // Compute w from the unit quaternion
-          Eigen::Quaternion<Scalar> q (0, p[3], p[4], p[5]);
-          q.w () = static_cast<Scalar> (std::sqrt (1 - q.dot (q)));
-          q.normalize ();
-          transform_matrix_.topLeftCorner (3, 3) = q.toRotationMatrix ();
-        }
-    };
+    // Compute w from the unit quaternion
+    Eigen::Quaternion<Scalar> q(0, p[3], p[4], p[5]);
+    q.w() = static_cast<Scalar>(std::sqrt(1 - q.dot(q)));
+    q.normalize();
+    transform_matrix_.topLeftCorner(3, 3) = q.toRotationMatrix();
   }
-}
+};
+} // namespace registration
+} // namespace pcl
diff --git a/registration/registration.doxy b/registration/registration.doxy
index 8b6363ce..eb49a954 100644
--- a/registration/registration.doxy
+++ b/registration/registration.doxy
@@ -12,7 +12,7 @@ position and orientation of the data sets. Once the alignment errors fall below
 a given threshold, the registration is said to be complete.
 
 The <b>pcl_registration</b> library implements a plethora of point cloud
-registration algorithms for both organized an unorganized (general purpose)
+registration algorithms for both organized and unorganized (general purpose)
 datasets.
 
 \image html http://www.pointclouds.org/assets/images/contents/documentation/registration_outdoor.png
diff --git a/registration/src/correspondence_rejection_distance.cpp b/registration/src/correspondence_rejection_distance.cpp
index b4a3ad6d..46cafa72 100644
--- a/registration/src/correspondence_rejection_distance.cpp
+++ b/registration/src/correspondence_rejection_distance.cpp
@@ -41,30 +41,28 @@
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl::registration::CorrespondenceRejectorDistance::getRemainingCorrespondences (
+pcl::registration::CorrespondenceRejectorDistance::getRemainingCorrespondences(
     const pcl::Correspondences& original_correspondences,
     pcl::Correspondences& remaining_correspondences)
 {
   unsigned int number_valid_correspondences = 0;
-  remaining_correspondences.resize (original_correspondences.size ());
-  for (const auto &original_correspondence : original_correspondences)
-  {
-    if (data_container_)
-    {
-      if (data_container_->getCorrespondenceScore (original_correspondence) < max_distance_)
-      {
-        remaining_correspondences[number_valid_correspondences] = original_correspondence;
+  remaining_correspondences.resize(original_correspondences.size());
+  for (const auto& original_correspondence : original_correspondences) {
+    if (data_container_) {
+      if (data_container_->getCorrespondenceScore(original_correspondence) <
+          max_distance_) {
+        remaining_correspondences[number_valid_correspondences] =
+            original_correspondence;
         ++number_valid_correspondences;
       }
     }
-    else
-    {
-      if (original_correspondence.distance < max_distance_)
-      {
-        remaining_correspondences[number_valid_correspondences] = original_correspondence;
+    else {
+      if (original_correspondence.distance < max_distance_) {
+        remaining_correspondences[number_valid_correspondences] =
+            original_correspondence;
         ++number_valid_correspondences;
       }
     }
   }
-  remaining_correspondences.resize (number_valid_correspondences);
+  remaining_correspondences.resize(number_valid_correspondences);
 }
diff --git a/registration/src/correspondence_rejection_features.cpp b/registration/src/correspondence_rejection_features.cpp
index 883d728e..8f3bbb76 100644
--- a/registration/src/correspondence_rejection_features.cpp
+++ b/registration/src/correspondence_rejection_features.cpp
@@ -41,38 +41,40 @@
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl::registration::CorrespondenceRejectorFeatures::getRemainingCorrespondences (
+pcl::registration::CorrespondenceRejectorFeatures::getRemainingCorrespondences(
     const pcl::Correspondences& original_correspondences,
     pcl::Correspondences& remaining_correspondences)
 {
   unsigned int number_valid_correspondences = 0;
-  remaining_correspondences.resize (original_correspondences.size ());
+  remaining_correspondences.resize(original_correspondences.size());
   // For each set of features, go over each correspondence from input_correspondences_
-  for (std::size_t i = 0; i < input_correspondences_->size (); ++i)
-  {
+  for (std::size_t i = 0; i < input_correspondences_->size(); ++i) {
     // Go over the map of features
-    for (FeaturesMap::const_iterator it = features_map_.begin (); it != features_map_.end (); ++it)
-    {
+    for (FeaturesMap::const_iterator it = features_map_.begin();
+         it != features_map_.end();
+         ++it) {
       // Check if the score in feature space is above the given threshold
-      // (assume that the number of feature correspondenecs is the same as the number of point correspondences)
-      if (!it->second->isCorrespondenceValid (static_cast<int> (i)))
+      // (assume that the number of feature correspondenecs is the same as the number of
+      // point correspondences)
+      if (!it->second->isCorrespondenceValid(static_cast<int>(i)))
         break;
 
-      remaining_correspondences[number_valid_correspondences] = original_correspondences[i];
+      remaining_correspondences[number_valid_correspondences] =
+          original_correspondences[i];
       ++number_valid_correspondences;
     }
   }
-  remaining_correspondences.resize (number_valid_correspondences);
+  remaining_correspondences.resize(number_valid_correspondences);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 inline bool
-pcl::registration::CorrespondenceRejectorFeatures::hasValidFeatures ()
+pcl::registration::CorrespondenceRejectorFeatures::hasValidFeatures()
 {
-  if (features_map_.empty ())
+  if (features_map_.empty())
     return (false);
-  for (const auto &feature : features_map_)
-    if (!feature.second->isValid ())
+  for (const auto& feature : features_map_)
+    if (!feature.second->isValid())
       return (false);
   return (true);
 }
diff --git a/registration/src/correspondence_rejection_median_distance.cpp b/registration/src/correspondence_rejection_median_distance.cpp
index 919efd17..11913157 100644
--- a/registration/src/correspondence_rejection_median_distance.cpp
+++ b/registration/src/correspondence_rejection_median_distance.cpp
@@ -41,30 +41,30 @@
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl::registration::CorrespondenceRejectorMedianDistance::getRemainingCorrespondences (
+pcl::registration::CorrespondenceRejectorMedianDistance::getRemainingCorrespondences(
     const pcl::Correspondences& original_correspondences,
     pcl::Correspondences& remaining_correspondences)
 {
-  std::vector <double> dists;
-  dists.resize (original_correspondences.size ());
+  std::vector<double> dists;
+  dists.resize(original_correspondences.size());
 
-  for (std::size_t i = 0; i < original_correspondences.size (); ++i)
-  {
+  for (std::size_t i = 0; i < original_correspondences.size(); ++i) {
     if (data_container_)
-      dists[i] = data_container_->getCorrespondenceScore (original_correspondences[i]);
+      dists[i] = data_container_->getCorrespondenceScore(original_correspondences[i]);
     else
       dists[i] = original_correspondences[i].distance;
   }
 
-  std::vector <double> nth (dists);
-  nth_element (nth.begin (), nth.begin () + (nth.size () / 2), nth.end ());
-  median_distance_ = nth [nth.size () / 2];
+  std::vector<double> nth(dists);
+  nth_element(nth.begin(), nth.begin() + (nth.size() / 2), nth.end());
+  median_distance_ = nth[nth.size() / 2];
 
   unsigned int number_valid_correspondences = 0;
-  remaining_correspondences.resize (original_correspondences.size ());
+  remaining_correspondences.resize(original_correspondences.size());
 
-  for (std::size_t i = 0; i < original_correspondences.size (); ++i)
+  for (std::size_t i = 0; i < original_correspondences.size(); ++i)
     if (dists[i] <= median_distance_ * factor_)
-      remaining_correspondences[number_valid_correspondences++] = original_correspondences[i];
-  remaining_correspondences.resize (number_valid_correspondences);
+      remaining_correspondences[number_valid_correspondences++] =
+          original_correspondences[i];
+  remaining_correspondences.resize(number_valid_correspondences);
 }
diff --git a/registration/src/correspondence_rejection_one_to_one.cpp b/registration/src/correspondence_rejection_one_to_one.cpp
index 1bbe5ee7..f94136da 100644
--- a/registration/src/correspondence_rejection_one_to_one.cpp
+++ b/registration/src/correspondence_rejection_one_to_one.cpp
@@ -38,31 +38,32 @@
  */
 
 #include <pcl/registration/correspondence_rejection_one_to_one.h>
+#include <pcl/pcl_base.h> // for UNAVAILABLE
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl::registration::CorrespondenceRejectorOneToOne::getRemainingCorrespondences (
+pcl::registration::CorrespondenceRejectorOneToOne::getRemainingCorrespondences(
     const pcl::Correspondences& original_correspondences,
     pcl::Correspondences& remaining_correspondences)
 {
   /* not really an efficient implementation */
   pcl::Correspondences input = original_correspondences;
 
-  std::sort (input.begin (), input.end (), pcl::registration::sortCorrespondencesByMatchIndexAndDistance ());
+  std::sort(input.begin(),
+            input.end(),
+            pcl::registration::sortCorrespondencesByMatchIndexAndDistance());
 
-  remaining_correspondences.resize (input.size ());
-  int index_last = -1;
+  remaining_correspondences.resize(input.size());
+  pcl::index_t index_last = UNAVAILABLE;
   unsigned int number_valid_correspondences = 0;
-  for (const auto &i : input)
-  {
+  for (const auto& i : input) {
     if (i.index_match < 0)
       continue;
-    if (i.index_match != index_last)
-    {
+    if (i.index_match != index_last) {
       remaining_correspondences[number_valid_correspondences] = i;
       index_last = i.index_match;
       ++number_valid_correspondences;
     }
   }
-  remaining_correspondences.resize (number_valid_correspondences);
+  remaining_correspondences.resize(number_valid_correspondences);
 }
diff --git a/registration/src/correspondence_rejection_organized_boundary.cpp b/registration/src/correspondence_rejection_organized_boundary.cpp
index 7b23e855..b67048e2 100644
--- a/registration/src/correspondence_rejection_organized_boundary.cpp
+++ b/registration/src/correspondence_rejection_organized_boundary.cpp
@@ -37,46 +37,49 @@
  */
 
 #include <pcl/registration/correspondence_rejection_organized_boundary.h>
-#include <pcl/memory.h>  // for static_pointer_cast
-
+#include <pcl/memory.h> // for static_pointer_cast
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl::registration::CorrespondenceRejectionOrganizedBoundary::getRemainingCorrespondences (const pcl::Correspondences& original_correspondences,
-                                                                                          pcl::Correspondences& remaining_correspondences)
+pcl::registration::CorrespondenceRejectionOrganizedBoundary::
+    getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
+                                pcl::Correspondences& remaining_correspondences)
 {
-  pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud = static_pointer_cast<pcl::registration::DataContainer<pcl::PointXYZ, pcl::PointNormal> >(data_container_)->getInputTarget ();
+  pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud =
+      static_pointer_cast<
+          pcl::registration::DataContainer<pcl::PointXYZ, pcl::PointNormal>>(
+          data_container_)
+          ->getInputTarget();
 
-  if (!cloud->isOrganized ())
-  {
-    PCL_ERROR ("[pcl::registration::CorrespondenceRejectionOrganizedBoundary::getRemainingCorrespondences] The target cloud is not organized.\n");
-    remaining_correspondences.clear ();
+  if (!cloud->isOrganized()) {
+    PCL_ERROR("[pcl::registration::CorrespondenceRejectionOrganizedBoundary::"
+              "getRemainingCorrespondences] The target cloud is not organized.\n");
+    remaining_correspondences.clear();
     return;
   }
 
-  remaining_correspondences.reserve (original_correspondences.size ());
-  for (const auto &original_correspondence : original_correspondences)
-  {
+  remaining_correspondences.reserve(original_correspondences.size());
+  for (const auto& original_correspondence : original_correspondences) {
     /// Count how many NaNs bound the target point
     int x = original_correspondence.index_match % cloud->width;
     int y = original_correspondence.index_match / cloud->width;
 
     int nan_count_tgt = 0;
-    for (int x_d = -window_size_/2; x_d <= window_size_/2; ++x_d)
-      for (int y_d = -window_size_/2; y_d <= window_size_/2; ++y_d)
-        if (x + x_d >= 0 && x + x_d < static_cast<int> (cloud->width) &&
-            y + y_d >= 0 && y + y_d < static_cast<int> (cloud->height))
-        {
-          if (!std::isfinite ((*cloud)(x + x_d, y + y_d).z) ||
-              std::fabs ((*cloud)(x, y).z - (*cloud)(x + x_d, y + y_d).z) > depth_step_threshold_)
-            nan_count_tgt ++;
+    for (int x_d = -window_size_ / 2; x_d <= window_size_ / 2; ++x_d)
+      for (int y_d = -window_size_ / 2; y_d <= window_size_ / 2; ++y_d)
+        if (x + x_d >= 0 && x + x_d < static_cast<int>(cloud->width) && y + y_d >= 0 &&
+            y + y_d < static_cast<int>(cloud->height)) {
+          if (!std::isfinite((*cloud)(x + x_d, y + y_d).z) ||
+              std::fabs((*cloud)(x, y).z - (*cloud)(x + x_d, y + y_d).z) >
+                  depth_step_threshold_)
+            nan_count_tgt++;
         }
 
     if (nan_count_tgt >= boundary_nans_threshold_)
       continue;
 
-
-    /// The correspondence passes both tests, add it to the filtered set of correspondences
-    remaining_correspondences.push_back (original_correspondence);
+    /// The correspondence passes both tests, add it to the filtered set of
+    /// correspondences
+    remaining_correspondences.push_back(original_correspondence);
   }
 }
diff --git a/registration/src/correspondence_rejection_surface_normal.cpp b/registration/src/correspondence_rejection_surface_normal.cpp
index 220a20c0..60e67b61 100644
--- a/registration/src/correspondence_rejection_surface_normal.cpp
+++ b/registration/src/correspondence_rejection_surface_normal.cpp
@@ -41,24 +41,26 @@
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl::registration::CorrespondenceRejectorSurfaceNormal::getRemainingCorrespondences (
+pcl::registration::CorrespondenceRejectorSurfaceNormal::getRemainingCorrespondences(
     const pcl::Correspondences& original_correspondences,
     pcl::Correspondences& remaining_correspondences)
 {
-  if (!data_container_)
-  {
-    PCL_ERROR ("[pcl::registratin::%s::getRemainingCorrespondences] DataContainer object is not initialized!\n", getClassName ().c_str ());
+  if (!data_container_) {
+    PCL_ERROR("[pcl::registratin::%s::getRemainingCorrespondences] DataContainer "
+              "object is not initialized!\n",
+              getClassName().c_str());
     return;
   }
 
   unsigned int number_valid_correspondences = 0;
-  remaining_correspondences.resize (original_correspondences.size ());
+  remaining_correspondences.resize(original_correspondences.size());
 
   // Test each correspondence
-  for (const auto &original_correspondence : original_correspondences)
-  {
-    if (data_container_->getCorrespondenceScoreFromNormals (original_correspondence) > threshold_)
-      remaining_correspondences[number_valid_correspondences++] = original_correspondence;
+  for (const auto& original_correspondence : original_correspondences) {
+    if (data_container_->getCorrespondenceScoreFromNormals(original_correspondence) >
+        threshold_)
+      remaining_correspondences[number_valid_correspondences++] =
+          original_correspondence;
   }
-  remaining_correspondences.resize (number_valid_correspondences);
+  remaining_correspondences.resize(number_valid_correspondences);
 }
diff --git a/registration/src/correspondence_rejection_trimmed.cpp b/registration/src/correspondence_rejection_trimmed.cpp
index 40895f13..f305ee82 100644
--- a/registration/src/correspondence_rejection_trimmed.cpp
+++ b/registration/src/correspondence_rejection_trimmed.cpp
@@ -41,19 +41,21 @@
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl::registration::CorrespondenceRejectorTrimmed::getRemainingCorrespondences (
+pcl::registration::CorrespondenceRejectorTrimmed::getRemainingCorrespondences(
     const pcl::Correspondences& original_correspondences,
     pcl::Correspondences& remaining_correspondences)
 {
   // not really an efficient implementation
   remaining_correspondences = original_correspondences;
-  unsigned int number_valid_correspondences = (int (std::floor (overlap_ratio_ * static_cast<float> (remaining_correspondences.size ()))));
-  number_valid_correspondences = std::max (number_valid_correspondences, nr_min_correspondences_);
+  unsigned int number_valid_correspondences = (int(std::floor(
+      overlap_ratio_ * static_cast<float>(remaining_correspondences.size()))));
+  number_valid_correspondences =
+      std::max(number_valid_correspondences, nr_min_correspondences_);
 
-  if (number_valid_correspondences < remaining_correspondences.size ())
-  {
-    std::sort (remaining_correspondences.begin (), remaining_correspondences.end (),
-               pcl::registration::sortCorrespondencesByDistance ());
-    remaining_correspondences.resize (number_valid_correspondences);
+  if (number_valid_correspondences < remaining_correspondences.size()) {
+    std::sort(remaining_correspondences.begin(),
+              remaining_correspondences.end(),
+              pcl::registration::sortCorrespondencesByDistance());
+    remaining_correspondences.resize(number_valid_correspondences);
   }
 }
diff --git a/registration/src/correspondence_rejection_var_trimmed.cpp b/registration/src/correspondence_rejection_var_trimmed.cpp
index 50e6e3a3..e11911fe 100644
--- a/registration/src/correspondence_rejection_var_trimmed.cpp
+++ b/registration/src/correspondence_rejection_var_trimmed.cpp
@@ -34,71 +34,71 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  *
  * $Id$
- * 
+ *
  */
 
 #include <pcl/registration/correspondence_rejection_var_trimmed.h>
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl::registration::CorrespondenceRejectorVarTrimmed::getRemainingCorrespondences (
+pcl::registration::CorrespondenceRejectorVarTrimmed::getRemainingCorrespondences(
     const pcl::Correspondences& original_correspondences,
     pcl::Correspondences& remaining_correspondences)
 {
-  std::vector <double> dists;
-  dists.resize (original_correspondences.size ());
+  std::vector<double> dists;
+  dists.resize(original_correspondences.size());
 
-  for (std::size_t i = 0; i < original_correspondences.size (); ++i)
-  {
-    if (data_container_)
-    {
-      dists[i] = data_container_->getCorrespondenceScore (original_correspondences[i]);
+  for (std::size_t i = 0; i < original_correspondences.size(); ++i) {
+    if (data_container_) {
+      dists[i] = data_container_->getCorrespondenceScore(original_correspondences[i]);
     }
-    else
-    {
+    else {
       dists[i] = original_correspondences[i].distance;
     }
   }
-  factor_ = optimizeInlierRatio (dists);
-  nth_element (dists.begin (), dists.begin () + int (double (dists.size ()) * factor_), dists.end ());
-  trimmed_distance_ = dists [int (double (dists.size ()) * factor_)];
+  factor_ = optimizeInlierRatio(dists);
+  nth_element(
+      dists.begin(), dists.begin() + int(double(dists.size()) * factor_), dists.end());
+  trimmed_distance_ = dists[int(double(dists.size()) * factor_)];
 
   unsigned int number_valid_correspondences = 0;
-  remaining_correspondences.resize (original_correspondences.size ());
+  remaining_correspondences.resize(original_correspondences.size());
 
-  for (std::size_t i = 0; i < original_correspondences.size (); ++i)
-  {
-    if ( dists[i] < trimmed_distance_)
-    {
-      remaining_correspondences[number_valid_correspondences] = original_correspondences[i];
+  for (std::size_t i = 0; i < original_correspondences.size(); ++i) {
+    if (dists[i] < trimmed_distance_) {
+      remaining_correspondences[number_valid_correspondences] =
+          original_correspondences[i];
       ++number_valid_correspondences;
     }
   }
-  remaining_correspondences.resize (number_valid_correspondences);
+  remaining_correspondences.resize(number_valid_correspondences);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 float
-pcl::registration::CorrespondenceRejectorVarTrimmed::optimizeInlierRatio (std::vector <double>&  dists) const
+pcl::registration::CorrespondenceRejectorVarTrimmed::optimizeInlierRatio(
+    std::vector<double>& dists) const
 {
-  unsigned int points_nbr = static_cast<unsigned int> (dists.size ());
-  std::sort (dists.begin (), dists.end ());
+  unsigned int points_nbr = static_cast<unsigned int>(dists.size());
+  std::sort(dists.begin(), dists.end());
 
-  const int min_el = int (std::floor (min_ratio_ * points_nbr));
-  const int max_el = int (std::floor (max_ratio_ * points_nbr));
+  const int min_el = int(std::floor(min_ratio_ * points_nbr));
+  const int max_el = int(std::floor(max_ratio_ * points_nbr));
 
-  using LineArray = Eigen::Array <double, Eigen::Dynamic, 1>;
-  Eigen::Map<LineArray> sorted_dist (&dists[0], points_nbr);
+  using LineArray = Eigen::Array<double, Eigen::Dynamic, 1>;
+  Eigen::Map<LineArray> sorted_dist(&dists[0], points_nbr);
 
-  const LineArray trunk_sorted_dist = sorted_dist.segment (min_el, max_el-min_el);
-  const double lower_sum = sorted_dist.head (min_el).sum ();
-  const LineArray ids = LineArray::LinSpaced (trunk_sorted_dist.rows (), min_el+1, max_el);
+  const LineArray trunk_sorted_dist = sorted_dist.segment(min_el, max_el - min_el);
+  const double lower_sum = sorted_dist.head(min_el).sum();
+  const LineArray ids =
+      LineArray::LinSpaced(trunk_sorted_dist.rows(), min_el + 1, max_el);
   const LineArray ratio = ids / points_nbr;
-  const LineArray deno = ratio.pow (lambda_);
-  const LineArray FRMS = deno.inverse ().square () * ids.inverse () * (lower_sum + trunk_sorted_dist);
-  int min_index (0);
-  FRMS.minCoeff (&min_index);
+  const LineArray deno = ratio.pow(lambda_);
+  const LineArray FRMS =
+      deno.inverse().square() * ids.inverse() * (lower_sum + trunk_sorted_dist);
+  int min_index(0);
+  FRMS.minCoeff(&min_index);
 
-  const float opt_ratio = float (min_index + min_el) / float (points_nbr);
+  const float opt_ratio = float(min_index + min_el) / float(points_nbr);
   return (opt_ratio);
 }
diff --git a/registration/src/correspondence_types.cpp b/registration/src/correspondence_types.cpp
index 050c3a71..cd9a68bc 100644
--- a/registration/src/correspondence_types.cpp
+++ b/registration/src/correspondence_types.cpp
@@ -38,4 +38,3 @@
  */
 
 #include <pcl/registration/correspondence_types.h>
-
diff --git a/registration/src/gicp6d.cpp b/registration/src/gicp6d.cpp
index e7afff72..5c3fa82d 100644
--- a/registration/src/gicp6d.cpp
+++ b/registration/src/gicp6d.cpp
@@ -37,293 +37,205 @@
  */
 
 #include <pcl/registration/gicp6d.h>
+#include <pcl/memory.h>                 // for pcl::make_shared
+#include <pcl/point_types_conversion.h> // for PointXYZRGBtoXYZLAB
 
-#include <pcl/memory.h>  // for pcl::make_shared
+namespace pcl {
 
-namespace pcl
+GeneralizedIterativeClosestPoint6D::GeneralizedIterativeClosestPoint6D(float lab_weight)
+: cloud_lab_(new pcl::PointCloud<PointXYZLAB>)
+, target_lab_(new pcl::PointCloud<PointXYZLAB>)
+, lab_weight_(lab_weight)
 {
-  // convert sRGB to CIELAB
-  Eigen::Vector3f
-  RGB2Lab (const Eigen::Vector3i& colorRGB)
-  {
-    // for sRGB   -> CIEXYZ see http://www.easyrgb.com/index.php?X=MATH&H=02#text2
-    // for CIEXYZ -> CIELAB see http://www.easyrgb.com/index.php?X=MATH&H=07#text7
-
-    double R, G, B, X, Y, Z;
-
-    // map sRGB values to [0, 1]
-    R = colorRGB[0] / 255.0;
-    G = colorRGB[1] / 255.0;
-    B = colorRGB[2] / 255.0;
-
-    // linearize sRGB values
-    if (R > 0.04045)
-      R = pow ( (R + 0.055) / 1.055, 2.4);
-    else
-      R /= 12.92;
-
-    if (G > 0.04045)
-      G = pow ( (G + 0.055) / 1.055, 2.4);
-    else
-      G /= 12.92;
-
-    if (B > 0.04045)
-      B = pow ( (B + 0.055) / 1.055, 2.4);
-    else
-      B /= 12.92;
-
-    // postponed:
-    //    R *= 100.0;
-    //    G *= 100.0;
-    //    B *= 100.0;
-
-    // linear sRGB -> CIEXYZ
-    X = R * 0.4124 + G * 0.3576 + B * 0.1805;
-    Y = R * 0.2126 + G * 0.7152 + B * 0.0722;
-    Z = R * 0.0193 + G * 0.1192 + B * 0.9505;
-
-    // *= 100.0 including:
-    X /= 0.95047;  //95.047;
-    //    Y /= 1;//100.000;
-    Z /= 1.08883;  //108.883;
-
-    // CIEXYZ -> CIELAB
-    if (X > 0.008856)
-      X = pow (X, 1.0 / 3.0);
-    else
-      X = 7.787 * X + 16.0 / 116.0;
-
-    if (Y > 0.008856)
-      Y = pow (Y, 1.0 / 3.0);
-    else
-      Y = 7.787 * Y + 16.0 / 116.0;
-
-    if (Z > 0.008856)
-      Z = pow (Z, 1.0 / 3.0);
-    else
-      Z = 7.787 * Z + 16.0 / 116.0;
-
-    Eigen::Vector3f colorLab;
-    colorLab[0] = static_cast<float> (116.0 * Y - 16.0);
-    colorLab[1] = static_cast<float> (500.0 * (X - Y));
-    colorLab[2] = static_cast<float> (200.0 * (Y - Z));
-
-    return colorLab;
-  }
-
-  // convert a PointXYZRGBA cloud to a PointXYZLAB cloud
-  void
-  convertRGBAToLAB (const PointCloud<pcl::PointXYZRGBA>& in, PointCloud<PointXYZLAB>& out)
-  {
-    out.resize (in.size ());
-
-    for (std::size_t i = 0; i < in.size (); ++i)
-    {
-      out[i].x = in[i].x;
-      out[i].y = in[i].y;
-      out[i].z = in[i].z;
-      out[i].data[3] = 1.0;  // important for homogeneous coordinates
+  // set rescale mask (leave x,y,z unchanged, scale L,a,b by lab_weight)
+  float alpha[6] = {1.0, 1.0, 1.0, lab_weight_, lab_weight_, lab_weight_};
+  point_rep_.setRescaleValues(alpha);
+}
 
-      Eigen::Vector3f lab = RGB2Lab (in[i].getRGBVector3i ());
-      out[i].L = lab[0];
-      out[i].a = lab[1];
-      out[i].b = lab[2];
-    }
-  }
+void
+GeneralizedIterativeClosestPoint6D::setInputSource(
+    const PointCloudSourceConstPtr& cloud)
+{
+  // call corresponding base class method
+  GeneralizedIterativeClosestPoint<PointSource, PointTarget>::setInputSource(cloud);
 
-  GeneralizedIterativeClosestPoint6D::GeneralizedIterativeClosestPoint6D (float lab_weight) :
-      cloud_lab_ (new pcl::PointCloud<PointXYZLAB>), target_lab_ (new pcl::PointCloud<PointXYZLAB>), lab_weight_ (lab_weight)
-  {
-    // set rescale mask (leave x,y,z unchanged, scale L,a,b by lab_weight)
-    float alpha[6] = { 1.0, 1.0, 1.0, lab_weight_, lab_weight_, lab_weight_ };
-    point_rep_.setRescaleValues (alpha);
+  // in addition, convert colors of the cloud to CIELAB
+  cloud_lab_->resize(cloud->size());
+  for (std::size_t point_idx = 0; point_idx < cloud->size(); ++point_idx) {
+    PointXYZRGBtoXYZLAB((*cloud)[point_idx], (*cloud_lab_)[point_idx]);
   }
+}
 
-  void
-  GeneralizedIterativeClosestPoint6D::setInputSource (const PointCloudSourceConstPtr& cloud)
-  {
-    // call corresponding base class method
-    GeneralizedIterativeClosestPoint<PointSource, PointTarget>::setInputSource (cloud);
+void
+GeneralizedIterativeClosestPoint6D::setInputTarget(
+    const PointCloudTargetConstPtr& target)
+{
+  // call corresponding base class method
+  GeneralizedIterativeClosestPoint<PointSource, PointTarget>::setInputTarget(target);
 
-    // in addition, convert colors of the cloud to CIELAB
-    convertRGBAToLAB (*cloud, *cloud_lab_);
+  // in addition, convert colors of the cloud to CIELAB...
+  target_lab_->resize(target->size());
+  for (std::size_t point_idx = 0; point_idx < target->size(); ++point_idx) {
+    PointXYZRGBtoXYZLAB((*target)[point_idx], (*target_lab_)[point_idx]);
   }
 
-  void
-  GeneralizedIterativeClosestPoint6D::setInputTarget (const PointCloudTargetConstPtr& target)
-  {
-    // call corresponding base class method
-    GeneralizedIterativeClosestPoint<PointSource, PointTarget>::setInputTarget (
-        target);
-
-    // in addition, convert colors of the cloud to CIELAB...
-    convertRGBAToLAB (*target, *target_lab_);
-
-    // ...and build 6d-tree
-    target_tree_lab_.setInputCloud (target_lab_);
-    target_tree_lab_.setPointRepresentation (
-        pcl::make_shared<MyPointRepresentation> (point_rep_));
-  }
+  // ...and build 6d-tree
+  target_tree_lab_.setInputCloud(target_lab_);
+  target_tree_lab_.setPointRepresentation(
+      pcl::make_shared<MyPointRepresentation>(point_rep_));
+}
 
-  bool
-  GeneralizedIterativeClosestPoint6D::searchForNeighbors (const PointXYZLAB& query, std::vector<int>& index,
-      std::vector<float>& distance)
-  {
-    int k = target_tree_lab_.nearestKSearch (query, 1, index, distance);
+bool
+GeneralizedIterativeClosestPoint6D::searchForNeighbors(const PointXYZLAB& query,
+                                                       pcl::Indices& index,
+                                                       std::vector<float>& distance)
+{
+  int k = target_tree_lab_.nearestKSearch(query, 1, index, distance);
 
-    // check if neighbor was found
-    return (k != 0);
-  }
+  // check if neighbor was found
+  return (k != 0);
+}
 
 // taken from the original GICP class and modified slightly to make use of color values
-  void
-  GeneralizedIterativeClosestPoint6D::computeTransformation (PointCloudSource& output, const Eigen::Matrix4f& guess)
-  {
-    using namespace pcl;
-
-    IterativeClosestPoint<PointSource, PointTarget>::initComputeReciprocal ();
-
-    // Difference between consecutive transforms
-    double delta = 0;
-    // Get the size of the target
-    const std::size_t N = indices_->size ();
-
-    // Set the mahalanobis matrices to identity
-    mahalanobis_.resize (N, Eigen::Matrix3d::Identity ());
-
-    // Compute target cloud covariance matrices
-    if ((!target_covariances_) || (target_covariances_->empty ()))
-    {
-      target_covariances_.reset (new MatricesVector);
-      computeCovariances<PointTarget> (target_, tree_, *target_covariances_);
-    }
-    // Compute input cloud covariance matrices
-    if ((!input_covariances_) || (input_covariances_->empty ()))
-    {
-      input_covariances_.reset (new MatricesVector);
-      computeCovariances<PointSource> (input_, tree_reciprocal_,
-          *input_covariances_);
-    }
-
-    base_transformation_ = guess;
-    nr_iterations_ = 0;
-    converged_ = false;
-    double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
-    std::vector<int> nn_indices (1);
-    std::vector<float> nn_dists (1);
+void
+GeneralizedIterativeClosestPoint6D::computeTransformation(PointCloudSource& output,
+                                                          const Eigen::Matrix4f& guess)
+{
+  using namespace pcl;
 
-    while (!converged_)
-    {
-      std::size_t cnt = 0;
-      std::vector<int> source_indices (indices_->size ());
-      std::vector<int> target_indices (indices_->size ());
+  IterativeClosestPoint<PointSource, PointTarget>::initComputeReciprocal();
 
-      // guess corresponds to base_t and transformation_ to t
-      Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero ();
-      for (std::size_t i = 0; i < 4; i++)
-        for (std::size_t j = 0; j < 4; j++)
-          for (std::size_t k = 0; k < 4; k++)
-            transform_R (i, j) += double (transformation_ (i, k))
-                * double (guess (k, j));
+  // Difference between consecutive transforms
+  double delta = 0;
+  // Get the size of the target
+  const std::size_t N = indices_->size();
 
-      Eigen::Matrix3d R = transform_R.topLeftCorner<3, 3> ();
+  // Set the mahalanobis matrices to identity
+  mahalanobis_.resize(N, Eigen::Matrix3d::Identity());
 
-      for (std::size_t i = 0; i < N; i++)
-      {
-        // MODIFICATION: take point from the CIELAB cloud instead
-        PointXYZLAB query = (*cloud_lab_)[i];
-        query.getVector4fMap () = guess * query.getVector4fMap ();
-        query.getVector4fMap () = transformation_ * query.getVector4fMap ();
+  // Compute target cloud covariance matrices
+  if ((!target_covariances_) || (target_covariances_->empty())) {
+    target_covariances_.reset(new MatricesVector);
+    computeCovariances<PointTarget>(target_, tree_, *target_covariances_);
+  }
+  // Compute input cloud covariance matrices
+  if ((!input_covariances_) || (input_covariances_->empty())) {
+    input_covariances_.reset(new MatricesVector);
+    computeCovariances<PointSource>(input_, tree_reciprocal_, *input_covariances_);
+  }
 
-        if (!searchForNeighbors (query, nn_indices, nn_dists))
-        {
-          PCL_ERROR(
-              "[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n",
-              getClassName ().c_str (), (*indices_)[i]);
-          return;
-        }
+  base_transformation_ = guess;
+  nr_iterations_ = 0;
+  converged_ = false;
+  double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
+  pcl::Indices nn_indices(1);
+  std::vector<float> nn_dists(1);
+
+  while (!converged_) {
+    std::size_t cnt = 0;
+    pcl::Indices source_indices(indices_->size());
+    pcl::Indices target_indices(indices_->size());
+
+    // guess corresponds to base_t and transformation_ to t
+    Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero();
+    for (std::size_t i = 0; i < 4; i++)
+      for (std::size_t j = 0; j < 4; j++)
+        for (std::size_t k = 0; k < 4; k++)
+          transform_R(i, j) += double(transformation_(i, k)) * double(guess(k, j));
+
+    Eigen::Matrix3d R = transform_R.topLeftCorner<3, 3>();
+
+    for (std::size_t i = 0; i < N; i++) {
+      // MODIFICATION: take point from the CIELAB cloud instead
+      PointXYZLAB query = (*cloud_lab_)[i];
+      query.getVector4fMap() = guess * query.getVector4fMap();
+      query.getVector4fMap() = transformation_ * query.getVector4fMap();
+
+      if (!searchForNeighbors(query, nn_indices, nn_dists)) {
+        PCL_ERROR("[pcl::%s::computeTransformation] Unable to find a nearest neighbor "
+                  "in the target dataset for point %d in the source!\n",
+                  getClassName().c_str(),
+                  (*indices_)[i]);
+        return;
+      }
 
-        // Check if the distance to the nearest neighbor is smaller than the user imposed threshold
-        if (nn_dists[0] < dist_threshold)
-        {
-          Eigen::Matrix3d &C1 = (*input_covariances_)[i];
-          Eigen::Matrix3d &C2 = (*target_covariances_)[nn_indices[0]];
-          Eigen::Matrix3d &M = mahalanobis_[i];
-          // M = R*C1
-          M = R * C1;
-          // temp = M*R' + C2 = R*C1*R' + C2
-          Eigen::Matrix3d temp = M * R.transpose ();
-          temp += C2;
-          // M = temp^-1
-          M = temp.inverse ();
-          source_indices[cnt] = static_cast<int> (i);
-          target_indices[cnt] = nn_indices[0];
-          cnt++;
-        }
+      // Check if the distance to the nearest neighbor is smaller than the user imposed
+      // threshold
+      if (nn_dists[0] < dist_threshold) {
+        Eigen::Matrix3d& C1 = (*input_covariances_)[i];
+        Eigen::Matrix3d& C2 = (*target_covariances_)[nn_indices[0]];
+        Eigen::Matrix3d& M = mahalanobis_[i];
+        // M = R*C1
+        M = R * C1;
+        // temp = M*R' + C2 = R*C1*R' + C2
+        Eigen::Matrix3d temp = M * R.transpose();
+        temp += C2;
+        // M = temp^-1
+        M = temp.inverse();
+        source_indices[cnt] = static_cast<int>(i);
+        target_indices[cnt] = nn_indices[0];
+        cnt++;
       }
-      // Resize to the actual number of valid correspondences
-      source_indices.resize (cnt);
-      target_indices.resize (cnt);
-      /* optimize transformation using the current assignment and Mahalanobis metrics*/
-      previous_transformation_ = transformation_;
-      //optimization right here
-      try
-      {
-        rigid_transformation_estimation_ (output, source_indices, *target_,
-            target_indices, transformation_);
-        /* compute the delta from this iteration */
-        delta = 0.;
-        for (int k = 0; k < 4; k++)
-        {
-          for (int l = 0; l < 4; l++)
-          {
-            double ratio = 1;
-            if (k < 3 && l < 3)  // rotation part of the transform
-              ratio = 1. / rotation_epsilon_;
-            else
-              ratio = 1. / transformation_epsilon_;
-            double c_delta = ratio
-                * std::abs (
-                    previous_transformation_ (k, l) - transformation_ (k, l));
-            if (c_delta > delta)
-              delta = c_delta;
-          }
+    }
+    // Resize to the actual number of valid correspondences
+    source_indices.resize(cnt);
+    target_indices.resize(cnt);
+    /* optimize transformation using the current assignment and Mahalanobis metrics*/
+    previous_transformation_ = transformation_;
+    // optimization right here
+    try {
+      rigid_transformation_estimation_(
+          output, source_indices, *target_, target_indices, transformation_);
+      /* compute the delta from this iteration */
+      delta = 0.;
+      for (int k = 0; k < 4; k++) {
+        for (int l = 0; l < 4; l++) {
+          double ratio = 1;
+          if (k < 3 && l < 3) // rotation part of the transform
+            ratio = 1. / rotation_epsilon_;
+          else
+            ratio = 1. / transformation_epsilon_;
+          double c_delta =
+              ratio * std::abs(previous_transformation_(k, l) - transformation_(k, l));
+          if (c_delta > delta)
+            delta = c_delta;
         }
       }
-      catch (PCLException &e)
-      {
-        PCL_DEBUG("[pcl::%s::computeTransformation] Optimization issue %s\n",
-            getClassName ().c_str (), e.what ());
-        break;
-      }
-
-      nr_iterations_++;
-      // Check for convergence
-      if (nr_iterations_ >= max_iterations_ || delta < 1)
-      {
-        converged_ = true;
-        previous_transformation_ = transformation_;
-        PCL_DEBUG(
-            "[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n",
-            getClassName ().c_str (), nr_iterations_, max_iterations_,
-            (transformation_ - previous_transformation_).array ().abs ().sum ());
-      }
-      else
-        PCL_DEBUG("[pcl::%s::computeTransformation] Convergence failed\n",
-            getClassName ().c_str ());
+    } catch (PCLException& e) {
+      PCL_DEBUG("[pcl::%s::computeTransformation] Optimization issue %s\n",
+                getClassName().c_str(),
+                e.what());
+      break;
     }
-    //for some reason the static equivalent method raises an error
-    // final_transformation_.block<3,3> (0,0) = (transformation_.block<3,3> (0,0)) * (guess.block<3,3> (0,0));
-    // final_transformation_.block <3, 1> (0, 3) = transformation_.block <3, 1> (0, 3) + guess.rightCols<1>.block <3, 1> (0, 3);
-    final_transformation_.topLeftCorner (3, 3) =
-        previous_transformation_.topLeftCorner (3, 3)
-            * guess.topLeftCorner (3, 3);
-    final_transformation_ (0, 3) = previous_transformation_ (0, 3) + guess (0, 3);
-    final_transformation_ (1, 3) = previous_transformation_ (1, 3) + guess (1, 3);
-    final_transformation_ (2, 3) = previous_transformation_ (2, 3) + guess (2, 3);
 
-    // Transform the point cloud
-    pcl::transformPointCloud (*input_, output, final_transformation_);
+    nr_iterations_++;
+    // Check for convergence
+    if (nr_iterations_ >= max_iterations_ || delta < 1) {
+      converged_ = true;
+      previous_transformation_ = transformation_;
+      PCL_DEBUG("[pcl::%s::computeTransformation] Convergence reached. Number of "
+                "iterations: %d out of %d. Transformation difference: %f\n",
+                getClassName().c_str(),
+                nr_iterations_,
+                max_iterations_,
+                (transformation_ - previous_transformation_).array().abs().sum());
+    }
+    else
+      PCL_DEBUG("[pcl::%s::computeTransformation] Convergence failed\n",
+                getClassName().c_str());
   }
-
+  // for some reason the static equivalent method raises an error
+  // final_transformation_.block<3,3> (0,0) = (transformation_.block<3,3> (0,0)) *
+  // (guess.block<3,3> (0,0)); final_transformation_.block <3, 1> (0, 3) =
+  // transformation_.block <3, 1> (0, 3) + guess.rightCols<1>.block <3, 1> (0, 3);
+  final_transformation_.topLeftCorner(3, 3) =
+      previous_transformation_.topLeftCorner(3, 3) * guess.topLeftCorner(3, 3);
+  final_transformation_(0, 3) = previous_transformation_(0, 3) + guess(0, 3);
+  final_transformation_(1, 3) = previous_transformation_(1, 3) + guess(1, 3);
+  final_transformation_(2, 3) = previous_transformation_(2, 3) + guess(2, 3);
+
+  // Transform the point cloud
+  pcl::transformPointCloud(*input_, output, final_transformation_);
 }
+
+} // namespace pcl
diff --git a/registration/src/joint_icp.cpp b/registration/src/joint_icp.cpp
index b83032f6..6bf7845d 100644
--- a/registration/src/joint_icp.cpp
+++ b/registration/src/joint_icp.cpp
@@ -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
diff --git a/registration/src/lum.cpp b/registration/src/lum.cpp
index 4af14a54..b2c643ee 100644
--- a/registration/src/lum.cpp
+++ b/registration/src/lum.cpp
@@ -37,9 +37,10 @@
  *
  */
 
-#include <pcl/point_types.h>
 #include <pcl/impl/instantiate.hpp>
 #include <pcl/registration/lum.h>
+// Must come after its header
 #include <pcl/registration/impl/lum.hpp>
+#include <pcl/point_types.h>
 
 PCL_INSTANTIATE(LUM, PCL_XYZ_POINT_TYPES);
diff --git a/registration/src/ndt.cpp b/registration/src/ndt.cpp
index ad83d562..c78f316b 100644
--- a/registration/src/ndt.cpp
+++ b/registration/src/ndt.cpp
@@ -37,13 +37,19 @@
  *
  */
 
+#ifndef PCL_NO_PRECOMPILE
 
-#include <pcl/point_types.h>
 #include <pcl/impl/instantiate.hpp>
-
 #include <pcl/registration/ndt.h>
+// Must come after its header
 #include <pcl/registration/impl/ndt.hpp>
+#include <pcl/point_types.h>
+
+template class PCL_EXPORTS
+    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>;
+template class PCL_EXPORTS
+    pcl::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI>;
+template class PCL_EXPORTS
+    pcl::NormalDistributionsTransform<pcl::PointXYZRGB, pcl::PointXYZRGB>;
 
-template class PCL_EXPORTS pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>;
-template class PCL_EXPORTS pcl::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI>;
-template class PCL_EXPORTS pcl::NormalDistributionsTransform<pcl::PointXYZRGB, pcl::PointXYZRGB>;
+#endif
diff --git a/registration/src/ppf_registration.cpp b/registration/src/ppf_registration.cpp
index d39f9afa..30015e33 100644
--- a/registration/src/ppf_registration.cpp
+++ b/registration/src/ppf_registration.cpp
@@ -43,32 +43,39 @@
 //#include <pcl/impl/instantiate.hpp>
 //#include <pcl/registration/impl/ppf_registration.hpp>
 
-//PCL_INSTANTIATE_PRODUCT(PPFRegistration, (PCL_XYZ_POINT_TYPES)(PCL_NORMAL_POINT_TYPES));
-//#endif    // PCL_NO_PRECOMPILE
+// PCL_INSTANTIATE_PRODUCT(PPFRegistration,
+// (PCL_XYZ_POINT_TYPES)(PCL_NORMAL_POINT_TYPES)); #endif    // PCL_NO_PRECOMPILE
 
 void
-pcl::PPFHashMapSearch::setInputFeatureCloud (PointCloud<PPFSignature>::ConstPtr feature_cloud)
+pcl::PPFHashMapSearch::setInputFeatureCloud(
+    PointCloud<PPFSignature>::ConstPtr feature_cloud)
 {
   // Discretize the feature cloud and insert it in the hash map
-  feature_hash_map_->clear ();
-  unsigned int n = static_cast<unsigned int> (std::sqrt (static_cast<float> (feature_cloud->size ())));
+  feature_hash_map_->clear();
+  unsigned int n =
+      static_cast<unsigned int>(std::sqrt(static_cast<float>(feature_cloud->size())));
   int d1, d2, d3, d4;
   max_dist_ = -1.0;
-  alpha_m_.resize (n);
-  for (std::size_t i = 0; i < n; ++i)
-  {
-    std::vector <float> alpha_m_row (n);
-    for (std::size_t j = 0; j < n; ++j)
-    {
-      d1 = static_cast<int> (std::floor ((*feature_cloud)[i*n+j].f1 / angle_discretization_step_));
-      d2 = static_cast<int> (std::floor ((*feature_cloud)[i*n+j].f2 / angle_discretization_step_));
-      d3 = static_cast<int> (std::floor ((*feature_cloud)[i*n+j].f3 / angle_discretization_step_));
-      d4 = static_cast<int> (std::floor ((*feature_cloud)[i*n+j].f4 / distance_discretization_step_));
-      feature_hash_map_->insert (std::pair<HashKeyStruct, std::pair<std::size_t, std::size_t> > (HashKeyStruct (d1, d2, d3, d4), std::pair<std::size_t, std::size_t> (i, j)));
-      alpha_m_row [j] = (*feature_cloud)[i*n + j].alpha_m;
+  alpha_m_.resize(n);
+  for (std::size_t i = 0; i < n; ++i) {
+    std::vector<float> alpha_m_row(n);
+    for (std::size_t j = 0; j < n; ++j) {
+      d1 = static_cast<int>(
+          std::floor((*feature_cloud)[i * n + j].f1 / angle_discretization_step_));
+      d2 = static_cast<int>(
+          std::floor((*feature_cloud)[i * n + j].f2 / angle_discretization_step_));
+      d3 = static_cast<int>(
+          std::floor((*feature_cloud)[i * n + j].f3 / angle_discretization_step_));
+      d4 = static_cast<int>(
+          std::floor((*feature_cloud)[i * n + j].f4 / distance_discretization_step_));
+      feature_hash_map_->insert(
+          std::pair<HashKeyStruct, std::pair<std::size_t, std::size_t>>(
+              HashKeyStruct(d1, d2, d3, d4),
+              std::pair<std::size_t, std::size_t>(i, j)));
+      alpha_m_row[j] = (*feature_cloud)[i * n + j].alpha_m;
 
-      if (max_dist_ < (*feature_cloud)[i*n + j].f4)
-        max_dist_ = (*feature_cloud)[i*n + j].f4;
+      if (max_dist_ < (*feature_cloud)[i * n + j].f4)
+        max_dist_ = (*feature_cloud)[i * n + j].f4;
     }
     alpha_m_[i] = alpha_m_row;
   }
@@ -76,27 +83,30 @@ pcl::PPFHashMapSearch::setInputFeatureCloud (PointCloud<PPFSignature>::ConstPtr
   internals_initialized_ = true;
 }
 
-
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl::PPFHashMapSearch::nearestNeighborSearch (float &f1, float &f2, float &f3, float &f4,
-                                              std::vector<std::pair<std::size_t, std::size_t> > &indices)
+pcl::PPFHashMapSearch::nearestNeighborSearch(
+    float& f1,
+    float& f2,
+    float& f3,
+    float& f4,
+    std::vector<std::pair<std::size_t, std::size_t>>& indices)
 {
-  if (!internals_initialized_)
-  {
-    PCL_ERROR("[pcl::PPFRegistration::nearestNeighborSearch]: input feature cloud has not been set - skipping search!\n");
+  if (!internals_initialized_) {
+    PCL_ERROR("[pcl::PPFRegistration::nearestNeighborSearch]: input feature cloud has "
+              "not been set - skipping search!\n");
     return;
   }
 
-  int d1 = static_cast<int> (std::floor (f1 / angle_discretization_step_)),
-      d2 = static_cast<int> (std::floor (f2 / angle_discretization_step_)),
-      d3 = static_cast<int> (std::floor (f3 / angle_discretization_step_)),
-      d4 = static_cast<int> (std::floor (f4 / distance_discretization_step_));
+  int d1 = static_cast<int>(std::floor(f1 / angle_discretization_step_)),
+      d2 = static_cast<int>(std::floor(f2 / angle_discretization_step_)),
+      d3 = static_cast<int>(std::floor(f3 / angle_discretization_step_)),
+      d4 = static_cast<int>(std::floor(f4 / distance_discretization_step_));
 
-  indices.clear ();
-  HashKeyStruct key = HashKeyStruct (d1, d2, d3, d4);
-  auto map_iterator_pair = feature_hash_map_->equal_range (key);
-  for (; map_iterator_pair.first != map_iterator_pair.second; ++ map_iterator_pair.first)
+  indices.clear();
+  HashKeyStruct key = HashKeyStruct(d1, d2, d3, d4);
+  auto map_iterator_pair = feature_hash_map_->equal_range(key);
+  for (; map_iterator_pair.first != map_iterator_pair.second; ++map_iterator_pair.first)
     indices.emplace_back(map_iterator_pair.first->second.first,
-                                                  map_iterator_pair.first->second.second);
+                         map_iterator_pair.first->second.second);
 }
diff --git a/registration/src/pyramid_feature_matching.cpp b/registration/src/pyramid_feature_matching.cpp
index 09c78228..e6d505b2 100644
--- a/registration/src/pyramid_feature_matching.cpp
+++ b/registration/src/pyramid_feature_matching.cpp
@@ -36,10 +36,10 @@
  *
  */
 
-#include <pcl/point_types.h>
 #include <pcl/impl/instantiate.hpp>
 #include <pcl/registration/pyramid_feature_matching.h>
+// Must come after its header
 #include <pcl/registration/impl/pyramid_feature_matching.hpp>
-
+#include <pcl/point_types.h>
 
 PCL_INSTANTIATE_PRODUCT(PyramidFeatureHistogram, (PCL_FEATURE_POINT_TYPES))
diff --git a/registration/src/registration.cpp b/registration/src/registration.cpp
index 91e55bfd..9309e128 100644
--- a/registration/src/registration.cpp
+++ b/registration/src/registration.cpp
@@ -37,10 +37,10 @@
  *
  */
 
-#include <pcl/point_types.h>
-#include <pcl/registration/registration.h>
 #include <pcl/registration/icp.h>
 #include <pcl/registration/icp_nl.h>
+#include <pcl/registration/registration.h>
+#include <pcl/point_types.h>
 /*#include <pcl/registration/correspondence_estimation.h>
 #include <pcl/registration/correspondence_rejection.h>
 #include <pcl/registration/correspondence_rejection_distance.h>
@@ -50,8 +50,9 @@
 #include <pcl/registration/correspondence_sorting.h>
 */
 
-using IterativeClosestPoint = pcl::IterativeClosestPoint<pcl::PointXYZ,pcl::PointXYZ>;
-using IterativeClosestPointNonLinear = pcl::IterativeClosestPointNonLinear<pcl::PointXYZ,pcl::PointXYZ>;
-//PLUGINLIB_DECLARE_CLASS (pcl, IterativeClosestPoint, IterativeClosestPoint, nodelet::Nodelet);
-//PLUGINLIB_DECLARE_CLASS (pcl, IterativeClosestPointNonLinear, IterativeClosestPointNonLinear, nodelet::Nodelet);
-
+using IterativeClosestPoint = pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>;
+using IterativeClosestPointNonLinear =
+    pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ>;
+// PLUGINLIB_DECLARE_CLASS (pcl, IterativeClosestPoint, IterativeClosestPoint,
+// nodelet::Nodelet); PLUGINLIB_DECLARE_CLASS (pcl, IterativeClosestPointNonLinear,
+// IterativeClosestPointNonLinear, nodelet::Nodelet);
diff --git a/registration/src/transformation_estimation_2D.cpp b/registration/src/transformation_estimation_2D.cpp
index 1dd7ebb1..32a9be49 100644
--- a/registration/src/transformation_estimation_2D.cpp
+++ b/registration/src/transformation_estimation_2D.cpp
@@ -3,7 +3,7 @@
  *
  *  Point Cloud Library (PCL) - www.pointclouds.org
  *  Copyright (c) 2012-, Open Perception, Inc.
- * 
+ *
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
diff --git a/sample_consensus/include/pcl/sample_consensus/boost.h b/sample_consensus/include/pcl/sample_consensus/boost.h
index 4afdf893..ea42ca4c 100644
--- a/sample_consensus/include/pcl/sample_consensus/boost.h
+++ b/sample_consensus/include/pcl/sample_consensus/boost.h
@@ -42,5 +42,6 @@
 #if defined __GNUC__
 #  pragma GCC system_header 
 #endif
+PCL_DEPRECATED_HEADER(1, 15, "Please include the needed boost headers directly.")
 
 #include <boost/random.hpp>
diff --git a/sample_consensus/include/pcl/sample_consensus/eigen.h b/sample_consensus/include/pcl/sample_consensus/eigen.h
index 6d4f6490..63d73332 100644
--- a/sample_consensus/include/pcl/sample_consensus/eigen.h
+++ b/sample_consensus/include/pcl/sample_consensus/eigen.h
@@ -38,7 +38,7 @@
  */
 
 #pragma once
-
+PCL_DEPRECATED_HEADER(1, 15, "Please include the needed eigen headers directly.")
 #if defined __GNUC__
 #  pragma GCC system_header 
 #endif
diff --git a/sample_consensus/include/pcl/sample_consensus/impl/lmeds.hpp b/sample_consensus/include/pcl/sample_consensus/impl/lmeds.hpp
index 3816cdeb..e88b8f76 100644
--- a/sample_consensus/include/pcl/sample_consensus/impl/lmeds.hpp
+++ b/sample_consensus/include/pcl/sample_consensus/impl/lmeds.hpp
@@ -42,6 +42,7 @@
 #define PCL_SAMPLE_CONSENSUS_IMPL_LMEDS_H_
 
 #include <pcl/sample_consensus/lmeds.h>
+#include <pcl/common/common.h> // for computeMedian
 
 //////////////////////////////////////////////////////////////////////////
 template <typename PointT> bool
@@ -58,7 +59,7 @@ pcl::LeastMedianSquares<PointT>::computeModel (int debug_verbosity_level)
   double d_best_penalty = std::numeric_limits<double>::max();
 
   Indices selection;
-  Eigen::VectorXf model_coefficients;
+  Eigen::VectorXf model_coefficients (sac_model_->getModelSize ());
   std::vector<double> distances;
 
   unsigned skipped_count = 0;
@@ -73,6 +74,7 @@ pcl::LeastMedianSquares<PointT>::computeModel (int debug_verbosity_level)
 
     if (selection.empty ())
     {
+      PCL_ERROR ("[pcl::LeastMedianSquares::computeModel] No samples could be selected!\n");
       break;
     }
 
@@ -81,6 +83,7 @@ pcl::LeastMedianSquares<PointT>::computeModel (int debug_verbosity_level)
     {
       //iterations_++;
       ++skipped_count;
+      PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] The function computeModelCoefficients failed, so continue with next iteration.\n");
       continue;
     }
 
@@ -102,7 +105,6 @@ pcl::LeastMedianSquares<PointT>::computeModel (int debug_verbosity_level)
     const auto nr_valid_dists = std::distance (distances.begin (), new_end);
 
     // d_cur_penalty = median (distances)
-    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)
     {
@@ -110,23 +112,7 @@ pcl::LeastMedianSquares<PointT>::computeModel (int debug_verbosity_level)
       ++skipped_count;
       continue;
     }
-
-    // Do we have a "middle" point or should we "estimate" one ?
-    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]);
-    }
+    d_cur_penalty = pcl::computeMedian (distances.begin (), new_end, static_cast<double(*)(double)>(std::sqrt));
 
     // Better match ?
     if (d_cur_penalty < d_best_penalty)
diff --git a/sample_consensus/include/pcl/sample_consensus/impl/mlesac.hpp b/sample_consensus/include/pcl/sample_consensus/impl/mlesac.hpp
index 297156bb..43e5b0e2 100644
--- a/sample_consensus/include/pcl/sample_consensus/impl/mlesac.hpp
+++ b/sample_consensus/include/pcl/sample_consensus/impl/mlesac.hpp
@@ -42,7 +42,8 @@
 #define PCL_SAMPLE_CONSENSUS_IMPL_MLESAC_H_
 
 #include <pcl/sample_consensus/mlesac.h>
-#include <pcl/point_types.h>
+#include <cfloat> // for FLT_MAX
+#include <pcl/common/common.h> // for computeMedian
 
 //////////////////////////////////////////////////////////////////////////
 template <typename PointT> bool
@@ -60,11 +61,13 @@ pcl::MaximumLikelihoodSampleConsensus<PointT>::computeModel (int debug_verbosity
   double k = 1.0;
 
   Indices selection;
-  Eigen::VectorXf model_coefficients;
+  Eigen::VectorXf model_coefficients (sac_model_->getModelSize ());
   std::vector<double> distances;
 
   // Compute sigma - remember to set threshold_ correctly !
   sigma_ = computeMedianAbsoluteDeviation (sac_model_->getInputCloud (), sac_model_->getIndices (), threshold_);
+  const double dist_scaling_factor = -1.0 / (2.0 * sigma_ * sigma_); // Precompute since this does not change
+  const double normalization_factor = 1.0 / (sqrt (2 * M_PI) * sigma_);
   if (debug_verbosity_level > 1)
     PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Estimated sigma value: %f.\n", sigma_);
 
@@ -106,7 +109,7 @@ pcl::MaximumLikelihoodSampleConsensus<PointT>::computeModel (int debug_verbosity
       continue;
     }
     
-    // Use Expectiation-Maximization to find out the right value for d_cur_penalty
+    // Use Expectation-Maximization to find out the right value for d_cur_penalty
     // ---[ Initial estimate for the gamma mixing parameter = 1/2
     double gamma = 0.5;
     double p_outlier_prob = 0;
@@ -115,10 +118,10 @@ pcl::MaximumLikelihoodSampleConsensus<PointT>::computeModel (int debug_verbosity
     std::vector<double> p_inlier_prob (indices_size);
     for (int j = 0; j < iterations_EM_; ++j)
     {
+      const double weighted_normalization_factor = gamma * normalization_factor;
       // Likelihood of a datum given that it is an inlier
       for (std::size_t i = 0; i < indices_size; ++i)
-        p_inlier_prob[i] = gamma * std::exp (- (distances[i] * distances[i] ) / 2 * (sigma_ * sigma_) ) /
-                           (sqrt (2 * M_PI) * sigma_);
+        p_inlier_prob[i] = weighted_normalization_factor * std::exp ( dist_scaling_factor * distances[i] * distances[i] );
 
       // Likelihood of a datum given that it is an outlier
       p_outlier_prob = (1 - gamma) / v;
@@ -222,15 +225,7 @@ pcl::MaximumLikelihoodSampleConsensus<PointT>::computeMedianAbsoluteDeviation (
     distances[i] = ptdiff.dot (ptdiff);
   }
 
-  std::sort (distances.begin (), distances.end ());
-
-  double result;
-  std::size_t mid = indices->size () / 2;
-  // Do we have a "middle" point or should we "estimate" one ?
-  if (indices->size () % 2 == 0)
-    result = (sqrt (distances[mid-1]) + sqrt (distances[mid])) / 2;
-  else
-    result = sqrt (distances[mid]);
+  const double result = pcl::computeMedian (distances.begin (), distances.end (), static_cast<double(*)(double)>(std::sqrt));
   return (sigma * result);
 }
 
@@ -275,23 +270,10 @@ pcl::MaximumLikelihoodSampleConsensus<PointT>::computeMedian (
     y[i] = (*cloud)[(*indices)[i]].y;
     z[i] = (*cloud)[(*indices)[i]].z;
   }
-  std::sort (x.begin (), x.end ());
-  std::sort (y.begin (), y.end ());
-  std::sort (z.begin (), z.end ());
 
-  std::size_t mid = indices->size () / 2;
-  if (indices->size () % 2 == 0)
-  {
-    median[0] = (x[mid-1] + x[mid]) / 2;
-    median[1] = (y[mid-1] + y[mid]) / 2;
-    median[2] = (z[mid-1] + z[mid]) / 2;
-  }
-  else
-  {
-    median[0] = x[mid];
-    median[1] = y[mid];
-    median[2] = z[mid];
-  }
+  median[0] = pcl::computeMedian (x.begin(), x.end());
+  median[1] = pcl::computeMedian (y.begin(), y.end());
+  median[2] = pcl::computeMedian (z.begin(), z.end());
   median[3] = 0;
 }
 
diff --git a/sample_consensus/include/pcl/sample_consensus/impl/msac.hpp b/sample_consensus/include/pcl/sample_consensus/impl/msac.hpp
index 4dcac5b8..34276ee0 100644
--- a/sample_consensus/include/pcl/sample_consensus/impl/msac.hpp
+++ b/sample_consensus/include/pcl/sample_consensus/impl/msac.hpp
@@ -59,7 +59,7 @@ pcl::MEstimatorSampleConsensus<PointT>::computeModel (int debug_verbosity_level)
   double k = 1.0;
 
   Indices selection;
-  Eigen::VectorXf model_coefficients;
+  Eigen::VectorXf model_coefficients (sac_model_->getModelSize ());
   std::vector<double> distances;
 
   int n_inliers_count = 0;
diff --git a/sample_consensus/include/pcl/sample_consensus/impl/prosac.hpp b/sample_consensus/include/pcl/sample_consensus/impl/prosac.hpp
index 9ab7540b..8733de2e 100644
--- a/sample_consensus/include/pcl/sample_consensus/impl/prosac.hpp
+++ b/sample_consensus/include/pcl/sample_consensus/impl/prosac.hpp
@@ -84,7 +84,7 @@ pcl::ProgressiveSampleConsensus<PointT>::computeModel (int debug_verbosity_level
 
   Indices inliers;
   Indices selection;
-  Eigen::VectorXf model_coefficients;
+  Eigen::VectorXf model_coefficients (sac_model_->getModelSize ());
 
   // We will increase the pool so the indices_ vector can only contain m elements at first
   Indices index_pool;
diff --git a/sample_consensus/include/pcl/sample_consensus/impl/ransac.hpp b/sample_consensus/include/pcl/sample_consensus/impl/ransac.hpp
index 3c74b540..2245c28e 100644
--- a/sample_consensus/include/pcl/sample_consensus/impl/ransac.hpp
+++ b/sample_consensus/include/pcl/sample_consensus/impl/ransac.hpp
@@ -68,7 +68,7 @@ pcl::RandomSampleConsensus<PointT>::computeModel (int)
   double k = std::numeric_limits<double>::max();
 
   Indices selection;
-  Eigen::VectorXf model_coefficients;
+  Eigen::VectorXf model_coefficients (sac_model_->getModelSize ());
 
   const double log_probability  = std::log (1.0 - probability_);
   const double one_over_indices = 1.0 / static_cast<double> (sac_model_->getIndices ()->size ());
diff --git a/sample_consensus/include/pcl/sample_consensus/impl/rmsac.hpp b/sample_consensus/include/pcl/sample_consensus/impl/rmsac.hpp
index ff25956c..0c0ebab8 100644
--- a/sample_consensus/include/pcl/sample_consensus/impl/rmsac.hpp
+++ b/sample_consensus/include/pcl/sample_consensus/impl/rmsac.hpp
@@ -59,7 +59,7 @@ pcl::RandomizedMEstimatorSampleConsensus<PointT>::computeModel (int debug_verbos
   double k = 1.0;
 
   Indices selection;
-  Eigen::VectorXf model_coefficients;
+  Eigen::VectorXf model_coefficients (sac_model_->getModelSize ());
   std::vector<double> distances;
   std::set<index_t> indices_subset;
 
diff --git a/sample_consensus/include/pcl/sample_consensus/impl/rransac.hpp b/sample_consensus/include/pcl/sample_consensus/impl/rransac.hpp
index 0e952f08..21474b1d 100644
--- a/sample_consensus/include/pcl/sample_consensus/impl/rransac.hpp
+++ b/sample_consensus/include/pcl/sample_consensus/impl/rransac.hpp
@@ -59,7 +59,7 @@ pcl::RandomizedRandomSampleConsensus<PointT>::computeModel (int debug_verbosity_
   double k = std::numeric_limits<double>::max();
 
   Indices selection;
-  Eigen::VectorXf model_coefficients;
+  Eigen::VectorXf model_coefficients (sac_model_->getModelSize ());
   std::set<index_t> indices_subset;
 
   const double log_probability  = std::log (1.0 - probability_);
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 8e3840b9..b2607bde 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
@@ -41,7 +41,7 @@
 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_H_
 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_H_
 
-#include <pcl/sample_consensus/eigen.h>
+#include <unsupported/Eigen/NonLinearOptimization> // for LevenbergMarquardt
 #include <pcl/sample_consensus/sac_model_circle.h>
 #include <pcl/common/concatenate.h>
 
@@ -102,9 +102,35 @@ pcl::SampleConsensusModelCircle2D<PointT>::computeModelCoefficients (const Indic
   // Radius
   model_coefficients[2] = static_cast<float> (sqrt ((model_coefficients[0] - p0[0]) * (model_coefficients[0] - p0[0]) +
                                                     (model_coefficients[1] - p0[1]) * (model_coefficients[1] - p0[1])));
+  PCL_DEBUG ("[pcl::SampleConsensusModelCircle2D::computeModelCoefficients] Model is (%g,%g,%g).\n",
+             model_coefficients[0], model_coefficients[1], model_coefficients[2]);
   return (true);
 }
 
+#define AT(POS) ((*input_)[(*indices_)[(POS)]])
+
+#ifdef __AVX__
+// This function computes the squared distances (i.e. the distances without the square root) of 8 points to the center of the circle
+template <typename PointT> inline __m256 pcl::SampleConsensusModelCircle2D<PointT>::sqr_dist8 (const std::size_t i, const __m256 a_vec, const __m256 b_vec) const
+{
+  const __m256 tmp1 = _mm256_sub_ps (_mm256_set_ps (AT(i  ).x, AT(i+1).x, AT(i+2).x, AT(i+3).x, AT(i+4).x, AT(i+5).x, AT(i+6).x, AT(i+7).x), a_vec);
+  const __m256 tmp2 = _mm256_sub_ps (_mm256_set_ps (AT(i  ).y, AT(i+1).y, AT(i+2).y, AT(i+3).y, AT(i+4).y, AT(i+5).y, AT(i+6).y, AT(i+7).y), b_vec);
+  return _mm256_add_ps (_mm256_mul_ps (tmp1, tmp1), _mm256_mul_ps (tmp2, tmp2));
+}
+#endif // ifdef __AVX__
+
+#ifdef __SSE__
+// This function computes the squared distances (i.e. the distances without the square root) of 4 points to the center of the circle
+template <typename PointT> inline __m128 pcl::SampleConsensusModelCircle2D<PointT>::sqr_dist4 (const std::size_t i, const __m128 a_vec, const __m128 b_vec) const
+{
+  const __m128 tmp1 = _mm_sub_ps (_mm_set_ps (AT(i  ).x, AT(i+1).x, AT(i+2).x, AT(i+3).x), a_vec);
+  const __m128 tmp2 = _mm_sub_ps (_mm_set_ps (AT(i  ).y, AT(i+1).y, AT(i+2).y, AT(i+3).y), b_vec);
+  return _mm_add_ps (_mm_mul_ps (tmp1, tmp1), _mm_mul_ps (tmp2, tmp2));
+}
+#endif // ifdef __SSE__
+
+#undef AT
+
 //////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
 pcl::SampleConsensusModelCircle2D<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) const
@@ -147,23 +173,23 @@ pcl::SampleConsensusModelCircle2D<PointT>::selectWithinDistance (
   inliers.reserve (indices_->size ());
   error_sqr_dists_.reserve (indices_->size ());
 
+  const float sqr_inner_radius = (model_coefficients[2] <= threshold ? 0.0f : (model_coefficients[2] - threshold) * (model_coefficients[2] - threshold));
+  const float sqr_outer_radius = (model_coefficients[2] + threshold) * (model_coefficients[2] + threshold);
   // Iterate through the 3d points and calculate the distances from them to the circle
   for (std::size_t i = 0; i < indices_->size (); ++i)
   {
-    // Calculate the distance from the point to the circle as the difference between
-    // dist(point,circle_origin) and circle_radius
-    float distance = std::abs (std::sqrt (
-                                      ( (*input_)[(*indices_)[i]].x - model_coefficients[0] ) *
-                                      ( (*input_)[(*indices_)[i]].x - model_coefficients[0] ) +
-
-                                      ( (*input_)[(*indices_)[i]].y - model_coefficients[1] ) *
-                                      ( (*input_)[(*indices_)[i]].y - model_coefficients[1] )
-                                      ) - model_coefficients[2]);
-    if (distance < threshold)
+    // To avoid sqrt computation: consider one larger circle (radius + threshold) and one smaller circle (radius - threshold).
+    // Valid if point is in larger circle, but not in smaller circle.
+    const float sqr_dist = ( (*input_)[(*indices_)[i]].x - model_coefficients[0] ) *
+                           ( (*input_)[(*indices_)[i]].x - model_coefficients[0] ) +
+                           ( (*input_)[(*indices_)[i]].y - model_coefficients[1] ) *
+                           ( (*input_)[(*indices_)[i]].y - model_coefficients[1] );
+    if ((sqr_dist <= sqr_outer_radius) && (sqr_dist >= sqr_inner_radius))
     {
       // Returns the indices of the points whose distances are smaller than the threshold
       inliers.push_back ((*indices_)[i]);
-      error_sqr_dists_.push_back (static_cast<double> (distance));
+      // Only compute exact distance if necessary (if point is inlier)
+      error_sqr_dists_.push_back (static_cast<double> (std::abs (std::sqrt (sqr_dist) - model_coefficients[2])));
     }
   }
 }
@@ -176,26 +202,117 @@ pcl::SampleConsensusModelCircle2D<PointT>::countWithinDistance (
   // Check if the model is valid given the user constraints
   if (!isModelValid (model_coefficients))
     return (0);
-  std::size_t nr_p = 0;
 
+#if defined (__AVX__) && defined (__AVX2__)
+  return countWithinDistanceAVX (model_coefficients, threshold);
+#elif defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
+  return countWithinDistanceSSE (model_coefficients, threshold);
+#else
+  return countWithinDistanceStandard (model_coefficients, threshold);
+#endif
+}
+
+//////////////////////////////////////////////////////////////////////////
+template <typename PointT> std::size_t
+pcl::SampleConsensusModelCircle2D<PointT>::countWithinDistanceStandard (
+      const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i) const
+{
+  std::size_t nr_p = 0;
+  const float sqr_inner_radius = (model_coefficients[2] <= threshold ? 0.0f : (model_coefficients[2] - threshold) * (model_coefficients[2] - threshold));
+  const float sqr_outer_radius = (model_coefficients[2] + threshold) * (model_coefficients[2] + threshold);
   // Iterate through the 3d points and calculate the distances from them to the circle
-  for (std::size_t i = 0; i < indices_->size (); ++i)
+  for (; i < indices_->size (); ++i)
   {
-    // Calculate the distance from the point to the circle as the difference between
-    // dist(point,circle_origin) and circle_radius
-    float distance = std::abs (std::sqrt (
-                                      ( (*input_)[(*indices_)[i]].x - model_coefficients[0] ) *
-                                      ( (*input_)[(*indices_)[i]].x - model_coefficients[0] ) +
-
-                                      ( (*input_)[(*indices_)[i]].y - model_coefficients[1] ) *
-                                      ( (*input_)[(*indices_)[i]].y - model_coefficients[1] )
-                                      ) - model_coefficients[2]);
-    if (distance < threshold)
+    // To avoid sqrt computation: consider one larger circle (radius + threshold) and one smaller circle (radius - threshold).
+    // Valid if point is in larger circle, but not in smaller circle.
+    const float sqr_dist = ( (*input_)[(*indices_)[i]].x - model_coefficients[0] ) *
+                           ( (*input_)[(*indices_)[i]].x - model_coefficients[0] ) +
+                           ( (*input_)[(*indices_)[i]].y - model_coefficients[1] ) *
+                           ( (*input_)[(*indices_)[i]].y - model_coefficients[1] );
+    if ((sqr_dist <= sqr_outer_radius) && (sqr_dist >= sqr_inner_radius))
       nr_p++;
   }
   return (nr_p);
 }
 
+//////////////////////////////////////////////////////////////////////////
+#if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
+template <typename PointT> std::size_t
+pcl::SampleConsensusModelCircle2D<PointT>::countWithinDistanceSSE (
+      const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i) const
+{
+  std::size_t nr_p = 0;
+  const __m128 a_vec = _mm_set1_ps (model_coefficients[0]);
+  const __m128 b_vec = _mm_set1_ps (model_coefficients[1]);
+  // To avoid sqrt computation: consider one larger circle (radius + threshold) and one smaller circle (radius - threshold). Valid if point is in larger circle, but not in smaller circle.
+  const __m128 sqr_inner_radius = _mm_set1_ps ((model_coefficients[2] <= threshold ? 0.0 : (model_coefficients[2]-threshold)*(model_coefficients[2]-threshold)));
+  const __m128 sqr_outer_radius = _mm_set1_ps ((model_coefficients[2]+threshold)*(model_coefficients[2]+threshold));
+  __m128i res = _mm_set1_epi32(0); // This corresponds to nr_p: 4 32bit integers that, summed together, hold the number of inliers
+  for (; (i + 4) <= indices_->size (); i += 4)
+  {
+    const __m128 sqr_dist = sqr_dist4 (i, a_vec, b_vec);
+    const __m128 mask = _mm_and_ps (_mm_cmplt_ps (sqr_inner_radius, sqr_dist), _mm_cmplt_ps (sqr_dist, sqr_outer_radius)); // The mask contains 1 bits if the corresponding points are inliers, else 0 bits
+    res = _mm_add_epi32 (res, _mm_and_si128 (_mm_set1_epi32 (1), _mm_castps_si128 (mask))); // The latter part creates a vector with ones (as 32bit integers) where the points are inliers
+    //const int res = _mm_movemask_ps (mask);
+    //if (res & 1) nr_p++;
+    //if (res & 2) nr_p++;
+    //if (res & 4) nr_p++;
+    //if (res & 8) nr_p++;
+  }
+  nr_p += _mm_extract_epi32 (res, 0);
+  nr_p += _mm_extract_epi32 (res, 1);
+  nr_p += _mm_extract_epi32 (res, 2);
+  nr_p += _mm_extract_epi32 (res, 3);
+
+  // Process the remaining points (at most 3)
+  nr_p += countWithinDistanceStandard (model_coefficients, threshold, i);
+  return (nr_p);
+}
+#endif
+
+//////////////////////////////////////////////////////////////////////////
+#if defined (__AVX__) && defined (__AVX2__)
+template <typename PointT> std::size_t
+pcl::SampleConsensusModelCircle2D<PointT>::countWithinDistanceAVX (
+      const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i) const
+{
+  std::size_t nr_p = 0;
+  const __m256 a_vec = _mm256_set1_ps (model_coefficients[0]);
+  const __m256 b_vec = _mm256_set1_ps (model_coefficients[1]);
+  // To avoid sqrt computation: consider one larger circle (radius + threshold) and one smaller circle (radius - threshold). Valid if point is in larger circle, but not in smaller circle.
+  const __m256 sqr_inner_radius = _mm256_set1_ps ((model_coefficients[2] <= threshold ? 0.0 : (model_coefficients[2]-threshold)*(model_coefficients[2]-threshold)));
+  const __m256 sqr_outer_radius = _mm256_set1_ps ((model_coefficients[2]+threshold)*(model_coefficients[2]+threshold));
+  __m256i res = _mm256_set1_epi32(0); // This corresponds to nr_p: 8 32bit integers that, summed together, hold the number of inliers
+  for (; (i + 8) <= indices_->size (); i += 8)
+  {
+    const __m256 sqr_dist = sqr_dist8 (i, a_vec, b_vec);
+    const __m256 mask = _mm256_and_ps (_mm256_cmp_ps (sqr_inner_radius, sqr_dist, _CMP_LT_OQ), _mm256_cmp_ps (sqr_dist, sqr_outer_radius, _CMP_LT_OQ)); // The mask contains 1 bits if the corresponding points are inliers, else 0 bits
+    res = _mm256_add_epi32 (res, _mm256_and_si256 (_mm256_set1_epi32 (1), _mm256_castps_si256 (mask))); // The latter part creates a vector with ones (as 32bit integers) where the points are inliers
+    //const int res = _mm256_movemask_ps (mask);
+    //if (res &   1) nr_p++;
+    //if (res &   2) nr_p++;
+    //if (res &   4) nr_p++;
+    //if (res &   8) nr_p++;
+    //if (res &  16) nr_p++;
+    //if (res &  32) nr_p++;
+    //if (res &  64) nr_p++;
+    //if (res & 128) nr_p++;
+  }
+  nr_p += _mm256_extract_epi32 (res, 0);
+  nr_p += _mm256_extract_epi32 (res, 1);
+  nr_p += _mm256_extract_epi32 (res, 2);
+  nr_p += _mm256_extract_epi32 (res, 3);
+  nr_p += _mm256_extract_epi32 (res, 4);
+  nr_p += _mm256_extract_epi32 (res, 5);
+  nr_p += _mm256_extract_epi32 (res, 6);
+  nr_p += _mm256_extract_epi32 (res, 7);
+
+  // Process the remaining points (at most 7)
+  nr_p += countWithinDistanceStandard (model_coefficients, threshold, i);
+  return (nr_p);
+}
+#endif
+
 //////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
 pcl::SampleConsensusModelCircle2D<PointT>::optimizeModelCoefficients (
@@ -247,7 +364,7 @@ pcl::SampleConsensusModelCircle2D<PointT>::projectPoints (
   if (copy_data_fields)
   {
     // Allocate enough space and copy the basics
-    projected_points.points.resize (input_->size ());
+    projected_points.resize (input_->size ());
     projected_points.width    = input_->width;
     projected_points.height   = input_->height;
 
@@ -271,7 +388,7 @@ pcl::SampleConsensusModelCircle2D<PointT>::projectPoints (
   else
   {
     // Allocate enough space and copy the basics
-    projected_points.points.resize (inliers.size ());
+    projected_points.resize (inliers.size ());
     projected_points.width    = inliers.size ();
     projected_points.height   = 1;
 
@@ -306,17 +423,19 @@ pcl::SampleConsensusModelCircle2D<PointT>::doSamplesVerifyModel (
     return (false);
   }
 
+  const float sqr_inner_radius = (model_coefficients[2] <= threshold ? 0.0f : (model_coefficients[2] - threshold) * (model_coefficients[2] - threshold));
+  const float sqr_outer_radius = (model_coefficients[2] + threshold) * (model_coefficients[2] + threshold);
   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 (
-                         ( (*input_)[index].x - model_coefficients[0] ) *
-                         ( (*input_)[index].x - model_coefficients[0] ) +
-                         ( (*input_)[index].y - model_coefficients[1] ) *
-                         ( (*input_)[index].y - model_coefficients[1] )
-                         ) - model_coefficients[2]) > threshold)
+  {
+    // To avoid sqrt computation: consider one larger circle (radius + threshold) and one smaller circle (radius - threshold).
+    // Valid if point is in larger circle, but not in smaller circle.
+    const float sqr_dist = ( (*input_)[index].x - model_coefficients[0] ) *
+                           ( (*input_)[index].x - model_coefficients[0] ) +
+                           ( (*input_)[index].y - model_coefficients[1] ) *
+                           ( (*input_)[index].y - model_coefficients[1] );
+    if ((sqr_dist > sqr_outer_radius) || (sqr_dist < sqr_inner_radius))
       return (false);
-
+  }
   return (true);
 }
 
@@ -328,9 +447,17 @@ pcl::SampleConsensusModelCircle2D<PointT>::isModelValid (const Eigen::VectorXf &
     return (false);
 
   if (radius_min_ != -std::numeric_limits<double>::max() && model_coefficients[2] < radius_min_)
+  {
+    PCL_DEBUG ("[pcl::SampleConsensusModelCircle2D::isModelValid] Radius of circle is too small: should be larger than %g, but is %g.\n",
+               radius_min_, model_coefficients[2]);
     return (false);
+  }
   if (radius_max_ != std::numeric_limits<double>::max() && model_coefficients[2] > radius_max_)
+  {
+    PCL_DEBUG ("[pcl::SampleConsensusModelCircle2D::isModelValid] Radius of circle is too big: should be smaller than %g, but is %g.\n",
+               radius_max_, model_coefficients[2]);
     return (false);
+  }
 
   return (true);
 }
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 67fd03c4..32ccbda7 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
@@ -39,7 +39,9 @@
 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_3D_HPP_
 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_3D_HPP_
 
-#include <pcl/sample_consensus/eigen.h>
+#include <cfloat> // for DBL_MAX
+
+#include <unsupported/Eigen/NonLinearOptimization> // for LevenbergMarquardt
 #include <pcl/sample_consensus/sac_model_circle3d.h>
 #include <pcl/common/concatenate.h>
 
@@ -111,8 +113,11 @@ pcl::SampleConsensusModelCircle3D<PointT>::computeModelCoefficients (const Indic
   model_coefficients[4] = static_cast<float> (circle_normal[0]);
   model_coefficients[5] = static_cast<float> (circle_normal[1]);
   model_coefficients[6] = static_cast<float> (circle_normal[2]);
-   
- return (true);
+
+  PCL_DEBUG ("[pcl::SampleConsensusModelCircle3D::computeModelCoefficients] Model is (%g,%g,%g,%g,%g,%g,%g).\n",
+             model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3],
+             model_coefficients[4], model_coefficients[5], model_coefficients[6]);
+  return (true);
 }
 
 //////////////////////////////////////////////////////////////////////////
@@ -177,6 +182,7 @@ pcl::SampleConsensusModelCircle3D<PointT>::selectWithinDistance (
   inliers.clear ();
   inliers.reserve (indices_->size ());
 
+  const auto squared_threshold = threshold * threshold;
   // Iterate through the 3d points and calculate the distances from them to the sphere
   for (std::size_t i = 0; i < indices_->size (); ++i)
   {
@@ -201,7 +207,7 @@ pcl::SampleConsensusModelCircle3D<PointT>::selectWithinDistance (
     Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();
     Eigen::Vector3d distanceVector =  P - K;
 
-    if (distanceVector.norm () < threshold)
+    if (distanceVector.squaredNorm () < squared_threshold)
     {
       // Returns the indices of the points whose distances are smaller than the threshold
       inliers.push_back ((*indices_)[i]);
@@ -219,6 +225,7 @@ pcl::SampleConsensusModelCircle3D<PointT>::countWithinDistance (
     return (0);
   std::size_t nr_p = 0;
 
+  const auto squared_threshold = threshold * threshold;
   // Iterate through the 3d points and calculate the distances from them to the sphere
   for (std::size_t i = 0; i < indices_->size (); ++i)
   {
@@ -244,7 +251,7 @@ pcl::SampleConsensusModelCircle3D<PointT>::countWithinDistance (
     Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();
     Eigen::Vector3d distanceVector =  P - K;
 
-    if (distanceVector.norm () < threshold)
+    if (distanceVector.squaredNorm () < squared_threshold)
       nr_p++;
   }
   return (nr_p);
@@ -306,7 +313,7 @@ pcl::SampleConsensusModelCircle3D<PointT>::projectPoints (
   if (copy_data_fields)
   {
     // Allocate enough space and copy the basics
-    projected_points.points.resize (input_->size ());
+    projected_points.resize (input_->size ());
     projected_points.width    = input_->width;
     projected_points.height   = input_->height;
 
@@ -348,7 +355,7 @@ pcl::SampleConsensusModelCircle3D<PointT>::projectPoints (
   else
   {
     // Allocate enough space and copy the basics
-    projected_points.points.resize (inliers.size ());
+    projected_points.resize (inliers.size ());
     projected_points.width    = inliers.size ();
     projected_points.height   = 1;
 
@@ -402,6 +409,7 @@ pcl::SampleConsensusModelCircle3D<PointT>::doSamplesVerifyModel (
     return (false);
   }
 
+  const auto squared_threshold = threshold * threshold;
   for (const auto &index : indices)
   {
     // Calculate the distance from the point to the sphere as the difference between
@@ -427,7 +435,7 @@ pcl::SampleConsensusModelCircle3D<PointT>::doSamplesVerifyModel (
     Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();
     Eigen::Vector3d distanceVector =  P - K;
 
-    if (distanceVector.norm () > threshold)
+    if (distanceVector.squaredNorm () > squared_threshold)
       return (false);
   }
   return (true);
@@ -441,9 +449,17 @@ pcl::SampleConsensusModelCircle3D<PointT>::isModelValid (const Eigen::VectorXf &
     return (false);
 
   if (radius_min_ != -DBL_MAX && model_coefficients[3] < radius_min_)
+  {
+    PCL_DEBUG ("[pcl::SampleConsensusModelCircle3D::isModelValid] Radius of circle is too small: should be larger than %g, but is %g.\n",
+               radius_min_, model_coefficients[3]);
     return (false);
+  }
   if (radius_max_ != DBL_MAX && model_coefficients[3] > radius_max_)
+  {
+    PCL_DEBUG ("[pcl::SampleConsensusModelCircle3D::isModelValid] Radius of circle is too big: should be smaller than %g, but is %g.\n",
+               radius_max_, model_coefficients[3]);
     return (false);
+  }
 
   return (true);
 }
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 361bbd1a..65280ce7 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
@@ -39,8 +39,9 @@
 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CONE_H_
 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CONE_H_
 
-#include <pcl/sample_consensus/eigen.h>
+#include <unsupported/Eigen/NonLinearOptimization> // for LevenbergMarquardt
 #include <pcl/sample_consensus/sac_model_cone.h>
+#include <pcl/common/common.h> // for getAngle3D
 #include <pcl/common/concatenate.h>
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -134,6 +135,9 @@ pcl::SampleConsensusModelCone<PointT, PointNT>::computeModelCoefficients (
   if (model_coefficients[6] !=  std::numeric_limits<double>::max() && model_coefficients[6] > max_angle_)
     return (false);
 
+  PCL_DEBUG ("[pcl::SampleConsensusModelCone::computeModelCoefficients] Model is (%g,%g,%g,%g,%g,%g,%g).\n",
+             model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3],
+             model_coefficients[4], model_coefficients[5], model_coefficients[6]);
   return (true);
 }
 
@@ -161,31 +165,33 @@ pcl::SampleConsensusModelCone<PointT, PointNT>::getDistancesToModel (
   for (std::size_t i = 0; i  < indices_->size (); ++i)
   {
     Eigen::Vector4f pt ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z, 0.0f);
-    Eigen::Vector4f n  ((*normals_)[(*indices_)[i]].normal[0], (*normals_)[(*indices_)[i]].normal[1], (*normals_)[(*indices_)[i]].normal[2], 0.0f);
 
     // Calculate the point's projection on the cone axis
     float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
     Eigen::Vector4f pt_proj = apex + k * axis_dir;
-    Eigen::Vector4f dir = pt - pt_proj;
-    dir.normalize ();
 
     // Calculate the actual radius of the cone at the level of the projected point
     Eigen::Vector4f height = apex - pt_proj;
     float actual_cone_radius = tanf (opening_angle) * height.norm ();
-    height.normalize ();
-
-    // Calculate the cones perfect normals
-    Eigen::Vector4f cone_normal = sinf (opening_angle) * height + std::cos (opening_angle) * dir;
 
     // Approximate the distance from the point to the cone as the difference between
     // dist(point,cone_axis) and actual cone radius
-    double d_euclid = std::abs (pointToAxisDistance (pt, model_coefficients) - actual_cone_radius);
+    const double weighted_euclid_dist = (1.0 - normal_distance_weight_) * std::abs (pointToAxisDistance (pt, model_coefficients) - actual_cone_radius);
+
+    // Calculate the direction of the point from center
+    Eigen::Vector4f dir = pt - pt_proj;
+    dir.normalize ();
+
+    // Calculate the cones perfect normals
+    height.normalize ();
+    Eigen::Vector4f cone_normal = sinf (opening_angle) * height + std::cos (opening_angle) * dir;
 
     // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
+    Eigen::Vector4f n  ((*normals_)[(*indices_)[i]].normal[0], (*normals_)[(*indices_)[i]].normal[1], (*normals_)[(*indices_)[i]].normal[2], 0.0f);
     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.0 - normal_distance_weight_) * d_euclid);
+    distances[i] = std::abs (normal_distance_weight_ * d_normal + weighted_euclid_dist);
   }
 }
 
@@ -216,33 +222,35 @@ pcl::SampleConsensusModelCone<PointT, PointNT>::selectWithinDistance (
   for (std::size_t i = 0; i < indices_->size (); ++i)
   {
     Eigen::Vector4f pt ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z, 0.0f);
-    Eigen::Vector4f n  ((*normals_)[(*indices_)[i]].normal[0], (*normals_)[(*indices_)[i]].normal[1], (*normals_)[(*indices_)[i]].normal[2], 0.0f);
 
     // Calculate the point's projection on the cone axis
     float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
     Eigen::Vector4f pt_proj = apex + k * axis_dir;
 
-    // Calculate the direction of the point from center
-    Eigen::Vector4f pp_pt_dir = pt - pt_proj;
-    pp_pt_dir.normalize ();
-
     // Calculate the actual radius of the cone at the level of the projected point
     Eigen::Vector4f height = apex - pt_proj;
     double actual_cone_radius = tan(opening_angle) * height.norm ();
-    height.normalize ();
-
-    // Calculate the cones perfect normals
-    Eigen::Vector4f cone_normal = sinf (opening_angle) * height + std::cos (opening_angle) * pp_pt_dir;
 
     // Approximate the distance from the point to the cone as the difference between
     // dist(point,cone_axis) and actual cone radius
-    double d_euclid = std::abs (pointToAxisDistance (pt, model_coefficients) - actual_cone_radius);
+    const double weighted_euclid_dist = (1.0 - normal_distance_weight_) * std::abs (pointToAxisDistance (pt, model_coefficients) - actual_cone_radius);
+    if (weighted_euclid_dist > threshold) // Early termination: cannot be an inlier
+      continue;
+
+    // Calculate the direction of the point from center
+    Eigen::Vector4f pp_pt_dir = pt - pt_proj;
+    pp_pt_dir.normalize ();
+
+    // Calculate the cones perfect normals
+    height.normalize ();
+    Eigen::Vector4f cone_normal = sinf (opening_angle) * height + std::cos (opening_angle) * pp_pt_dir;
 
     // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
+    Eigen::Vector4f n  ((*normals_)[(*indices_)[i]].normal[0], (*normals_)[(*indices_)[i]].normal[1], (*normals_)[(*indices_)[i]].normal[2], 0.0f);
     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.0 - normal_distance_weight_) * d_euclid);
+    double distance = std::abs (normal_distance_weight_ * d_normal + weighted_euclid_dist);
     
     if (distance < threshold)
     {
@@ -275,33 +283,35 @@ pcl::SampleConsensusModelCone<PointT, PointNT>::countWithinDistance (
   for (std::size_t i = 0; i < indices_->size (); ++i)
   {
     Eigen::Vector4f pt ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z, 0.0f);
-    Eigen::Vector4f n  ((*normals_)[(*indices_)[i]].normal[0], (*normals_)[(*indices_)[i]].normal[1], (*normals_)[(*indices_)[i]].normal[2], 0.0f);
 
     // Calculate the point's projection on the cone axis
     float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
     Eigen::Vector4f pt_proj = apex + k * axis_dir;
 
-    // Calculate the direction of the point from center
-    Eigen::Vector4f pp_pt_dir = pt - pt_proj;
-    pp_pt_dir.normalize ();
-
     // Calculate the actual radius of the cone at the level of the projected point
     Eigen::Vector4f height = apex - pt_proj;
     double actual_cone_radius = tan(opening_angle) * height.norm ();
-    height.normalize ();
-
-    // Calculate the cones perfect normals
-    Eigen::Vector4f cone_normal = sinf (opening_angle) * height + std::cos (opening_angle) * pp_pt_dir;
 
     // Approximate the distance from the point to the cone as the difference between
     // dist(point,cone_axis) and actual cone radius
-    double d_euclid = std::abs (pointToAxisDistance (pt, model_coefficients) - actual_cone_radius);
+    const double weighted_euclid_dist = (1.0 - normal_distance_weight_) * std::abs (pointToAxisDistance (pt, model_coefficients) - actual_cone_radius);
+    if (weighted_euclid_dist > threshold) // Early termination: cannot be an inlier
+      continue;
+
+    // Calculate the direction of the point from center
+    Eigen::Vector4f pp_pt_dir = pt - pt_proj;
+    pp_pt_dir.normalize ();
+
+    // Calculate the cones perfect normals
+    height.normalize ();
+    Eigen::Vector4f cone_normal = sinf (opening_angle) * height + std::cos (opening_angle) * pp_pt_dir;
 
     // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
+    Eigen::Vector4f n  ((*normals_)[(*indices_)[i]].normal[0], (*normals_)[(*indices_)[i]].normal[1], (*normals_)[(*indices_)[i]].normal[2], 0.0f);
     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.0 - normal_distance_weight_) * d_euclid) < threshold)
+    if (std::abs (normal_distance_weight_ * d_normal + weighted_euclid_dist) < threshold)
       nr_p++;
   }
   return (nr_p);
@@ -371,7 +381,7 @@ pcl::SampleConsensusModelCone<PointT, PointNT>::projectPoints (
   if (copy_data_fields)
   {
     // Allocate enough space and copy the basics
-    projected_points.points.resize (input_->size ());
+    projected_points.resize (input_->size ());
     projected_points.width    = input_->width;
     projected_points.height   = input_->height;
 
@@ -408,7 +418,7 @@ pcl::SampleConsensusModelCone<PointT, PointNT>::projectPoints (
   else
   {
     // Allocate enough space and copy the basics
-    projected_points.points.resize (inliers.size ());
+    projected_points.resize (inliers.size ());
     projected_points.width    = inliers.size ();
     projected_points.height   = 1;
 
@@ -511,13 +521,24 @@ pcl::SampleConsensusModelCone<PointT, PointNT>::isModelValid (const Eigen::Vecto
     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_)
+    {
+      PCL_DEBUG ("[pcl::SampleConsensusModelCone::isModelValid] Angle between cone direction and given axis is too large.\n");
       return (false);
+    }
   }
 
   if (model_coefficients[6] != -std::numeric_limits<double>::max() && model_coefficients[6] < min_angle_)
+  {
+    PCL_DEBUG ("[pcl::SampleConsensusModelCone::isModelValid] The opening angle is too small: should be larger than %g, but is %g.\n",
+               min_angle_, model_coefficients[6]);
     return (false);
+  }
   if (model_coefficients[6] !=  std::numeric_limits<double>::max() && model_coefficients[6] > max_angle_)
+  {
+    PCL_DEBUG ("[pcl::SampleConsensusModelCone::isModelValid] The opening angle is too big: should be smaller than %g, but is %g.\n",
+               max_angle_, model_coefficients[6]);
     return (false);
+  }
 
   return (true);
 }
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 6b13db27..d21e1a69 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
@@ -41,8 +41,9 @@
 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CYLINDER_H_
 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CYLINDER_H_
 
-#include <pcl/sample_consensus/eigen.h>
+#include <unsupported/Eigen/NonLinearOptimization> // for LevenbergMarquardt
 #include <pcl/sample_consensus/sac_model_cylinder.h>
+#include <pcl/common/common.h> // for getAngle3D
 #include <pcl/common/concatenate.h>
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -128,6 +129,9 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::computeModelCoefficients (
   if (model_coefficients[6] > radius_max_ || model_coefficients[6] < radius_min_)
     return (false);
 
+  PCL_DEBUG ("[pcl::SampleConsensusModelCylinder::computeModelCoefficients] Model is (%g,%g,%g,%g,%g,%g,%g).\n",
+             model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3],
+             model_coefficients[4], model_coefficients[5], model_coefficients[6]);
   return (true);
 }
 
@@ -156,9 +160,8 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::getDistancesToModel (
     // dist(point,cylinder_axis) and cylinder radius
     // @note need to revise this.
     Eigen::Vector4f pt ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z, 0.0f);
-    Eigen::Vector4f n  ((*normals_)[(*indices_)[i]].normal[0], (*normals_)[(*indices_)[i]].normal[1], (*normals_)[(*indices_)[i]].normal[2], 0.0f);
 
-    double d_euclid = std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
+    const double weighted_euclid_dist = (1.0 - normal_distance_weight_) * std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
 
     // Calculate the point's projection on the cylinder axis
     float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir;
@@ -167,10 +170,11 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::getDistancesToModel (
     dir.normalize ();
 
     // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
+    Eigen::Vector4f n  ((*normals_)[(*indices_)[i]].normal[0], (*normals_)[(*indices_)[i]].normal[1], (*normals_)[(*indices_)[i]].normal[2], 0.0f);
     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.0 - normal_distance_weight_) * d_euclid);
+    distances[i] = std::abs (normal_distance_weight_ * d_normal + weighted_euclid_dist);
   }
 }
 
@@ -201,8 +205,9 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::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_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z, 0.0f);
-    Eigen::Vector4f n  ((*normals_)[(*indices_)[i]].normal[0], (*normals_)[(*indices_)[i]].normal[1], (*normals_)[(*indices_)[i]].normal[2], 0.0f);
-    double d_euclid = std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
+    const double weighted_euclid_dist = (1.0 - normal_distance_weight_) * std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
+    if (weighted_euclid_dist > threshold) // Early termination: cannot be an inlier
+      continue;
 
     // Calculate the point's projection on the cylinder axis
     float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir;
@@ -211,10 +216,11 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::selectWithinDistance (
     dir.normalize ();
 
     // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
+    Eigen::Vector4f n  ((*normals_)[(*indices_)[i]].normal[0], (*normals_)[(*indices_)[i]].normal[1], (*normals_)[(*indices_)[i]].normal[2], 0.0f);
     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.0 - normal_distance_weight_) * d_euclid);
+    double distance = std::abs (normal_distance_weight_ * d_normal + weighted_euclid_dist);
     if (distance < threshold)
     {
       // Returns the indices of the points whose distances are smaller than the threshold
@@ -245,8 +251,9 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::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_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z, 0.0f);
-    Eigen::Vector4f n  ((*normals_)[(*indices_)[i]].normal[0], (*normals_)[(*indices_)[i]].normal[1], (*normals_)[(*indices_)[i]].normal[2], 0.0f);
-    double d_euclid = std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
+    const double weighted_euclid_dist = (1.0 - normal_distance_weight_) * std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
+    if (weighted_euclid_dist > threshold) // Early termination: cannot be an inlier
+      continue;
 
     // Calculate the point's projection on the cylinder axis
     float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir;
@@ -255,10 +262,11 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::countWithinDistance (
     dir.normalize ();
 
     // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
+    Eigen::Vector4f n  ((*normals_)[(*indices_)[i]].normal[0], (*normals_)[(*indices_)[i]].normal[1], (*normals_)[(*indices_)[i]].normal[2], 0.0f);
     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.0 - normal_distance_weight_) * d_euclid) < threshold)
+    if (std::abs (normal_distance_weight_ * d_normal + weighted_euclid_dist) < threshold)
       nr_p++;
   }
   return (nr_p);
@@ -326,7 +334,7 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::projectPoints (
   if (copy_data_fields)
   {
     // Allocate enough space and copy the basics
-    projected_points.points.resize (input_->size ());
+    projected_points.resize (input_->size ());
     projected_points.width    = input_->width;
     projected_points.height   = input_->height;
 
@@ -359,7 +367,7 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::projectPoints (
   else
   {
     // Allocate enough space and copy the basics
-    projected_points.points.resize (inliers.size ());
+    projected_points.resize (inliers.size ());
     projected_points.width    = inliers.size ();
     projected_points.height   = 1;
 
@@ -458,13 +466,24 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::isModelValid (const Eigen::V
     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_)
+    {
+      PCL_DEBUG ("[pcl::SampleConsensusModelCylinder::isModelValid] Angle between cylinder direction and given axis is too large.\n");
       return (false);
+    }
   }
 
   if (radius_min_ != -std::numeric_limits<double>::max() && model_coefficients[6] < radius_min_)
+  {
+    PCL_DEBUG ("[pcl::SampleConsensusModelCylinder::isModelValid] Radius is too small: should be larger than %g, but is %g.\n",
+               radius_min_, model_coefficients[6]);
     return (false);
+  }
   if (radius_max_ != std::numeric_limits<double>::max() && model_coefficients[6] > radius_max_)
+  {
+    PCL_DEBUG ("[pcl::SampleConsensusModelCylinder::isModelValid] Radius is too big: should be smaller than %g, but is %g.\n",
+               radius_max_, model_coefficients[6]);
     return (false);
+  }
 
   return (true);
 }
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 4829a2a3..faaa1ac2 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
@@ -44,6 +44,7 @@
 #include <pcl/sample_consensus/sac_model_line.h>
 #include <pcl/common/centroid.h>
 #include <pcl/common/concatenate.h>
+#include <pcl/common/eigen.h> // for eigen33
 
 //////////////////////////////////////////////////////////////////////////
 template <typename PointT> bool
@@ -97,6 +98,9 @@ pcl::SampleConsensusModelLine<PointT>::computeModelCoefficients (
   model_coefficients[5] = (*input_)[samples[1]].z - model_coefficients[2];
 
   model_coefficients.template tail<3> ().normalize ();
+  PCL_DEBUG ("[pcl::SampleConsensusModelLine::computeModelCoefficients] Model is (%g,%g,%g,%g,%g,%g).\n",
+             model_coefficients[0], model_coefficients[1], model_coefficients[2],
+             model_coefficients[3], model_coefficients[4], model_coefficients[5]);
   return (true);
 }
 
@@ -220,7 +224,12 @@ pcl::SampleConsensusModelLine<PointT>::optimizeModelCoefficients (
 
   // Compute the 3x3 covariance matrix
   Eigen::Vector4f centroid;
-  compute3DCentroid (*input_, inliers, centroid);
+  if (0 == compute3DCentroid (*input_, inliers, centroid))
+  {
+    PCL_WARN ("[pcl::SampleConsensusModelLine::optimizeModelCoefficients] compute3DCentroid failed (returned 0) because there are no valid inliers.\n");
+    optimized_coefficients = model_coefficients;
+    return;
+  }
   Eigen::Matrix3f covariance_matrix;
   computeCovarianceMatrix (*input_, inliers, centroid, covariance_matrix);
   optimized_coefficients[0] = centroid[0];
@@ -257,7 +266,7 @@ pcl::SampleConsensusModelLine<PointT>::projectPoints (
   if (copy_data_fields)
   {
     // Allocate enough space and copy the basics
-    projected_points.points.resize (input_->size ());
+    projected_points.resize (input_->size ());
     projected_points.width    = input_->width;
     projected_points.height   = input_->height;
 
@@ -284,7 +293,7 @@ pcl::SampleConsensusModelLine<PointT>::projectPoints (
   else
   {
     // Allocate enough space and copy the basics
-    projected_points.points.resize (inliers.size ());
+    projected_points.resize (inliers.size ());
     projected_points.width    = inliers.size ();
     projected_points.height   = 1;
 
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 f9066229..a3640d56 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
@@ -59,13 +59,20 @@ pcl::SampleConsensusModelNormalParallelPlane<PointT, PointNT>::isModelValid (con
     coeff.normalize ();
 
     if (std::abs (axis_.dot (coeff)) < cos_angle_)
+    {
+      PCL_DEBUG ("[pcl::SampleConsensusModelNormalParallelPlane::isModelValid] Angle between plane normal and given axis is too large.\n");
       return  (false);
+    }
   }
 
   if (eps_dist_ > 0.0)
   {
     if (std::abs (-model_coefficients[3] - distance_from_origin_) > eps_dist_)
+    {
+      PCL_DEBUG ("[pcl::SampleConsensusModelNormalParallelPlane::isModelValid] Distance of plane to origin is wrong: expected %g, but is %g, difference is larger than %g.\n",
+                 distance_from_origin_, -model_coefficients[3], eps_dist_);
       return (false);
+    }
   }
 
   return (true);
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 fc8536e2..0cd94ff8 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
@@ -42,6 +42,7 @@
 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PLANE_H_
 
 #include <pcl/sample_consensus/sac_model_normal_plane.h>
+#include <pcl/sample_consensus/impl/sac_model_plane.hpp> // for dist4, dist8
 #include <pcl/common/common.h> // for getAngle3D
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -115,36 +116,178 @@ pcl::SampleConsensusModelNormalPlane<PointT, PointNT>::countWithinDistance (
   if (!isModelValid (model_coefficients))
     return (0);
 
+#if defined (__AVX__) && defined (__AVX2__)
+  return countWithinDistanceAVX (model_coefficients, threshold);
+#elif defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
+  return countWithinDistanceSSE (model_coefficients, threshold);
+#else
+  return countWithinDistanceStandard (model_coefficients, threshold);
+#endif
+}
+
+//////////////////////////////////////////////////////////////////////////
+template <typename PointT, typename PointNT> std::size_t
+pcl::SampleConsensusModelNormalPlane<PointT, PointNT>::countWithinDistanceStandard (
+      const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i) const
+{
+  std::size_t nr_p = 0;
+
   // Obtain the plane normal
   Eigen::Vector4f coeff = model_coefficients;
   coeff[3] = 0.0f;
 
-  std::size_t nr_p = 0;
-
   // Iterate through the 3d points and calculate the distances from them to the plane
-  for (std::size_t i = 0; i < indices_->size (); ++i)
+  for (; i < indices_->size (); ++i)
   {
     const PointT  &pt = (*input_)[(*indices_)[i]];
     const PointNT &nt = (*normals_)[(*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.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]);
+    const Eigen::Vector4f p (pt.x, pt.y, pt.z, 0.0f);
+    const Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0.0f);
+    const double d_euclid = std::abs (coeff.dot (p) + model_coefficients[3]);
 
     // Calculate the angular distance between the point normal and the plane normal
     double d_normal = std::abs (getAngle3D (n, coeff));
     d_normal = (std::min) (d_normal, M_PI - d_normal);
 
     // Weight with the point curvature. On flat surfaces, curvature -> 0, which means the normal will have a higher influence
-    double weight = normal_distance_weight_ * (1.0 - nt.curvature);
+    const double weight = normal_distance_weight_ * (1.0 - nt.curvature);
 
     if (std::abs (weight * d_normal + (1.0 - weight) * d_euclid) < threshold)
+    {
       nr_p++;
+    }
   }
   return (nr_p);
 }
 
+//////////////////////////////////////////////////////////////////////////
+#if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
+template <typename PointT, typename PointNT> std::size_t
+pcl::SampleConsensusModelNormalPlane<PointT, PointNT>::countWithinDistanceSSE (
+      const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i) const
+{
+  std::size_t nr_p = 0;
+  const __m128 a_vec = _mm_set1_ps (model_coefficients[0]);
+  const __m128 b_vec = _mm_set1_ps (model_coefficients[1]);
+  const __m128 c_vec = _mm_set1_ps (model_coefficients[2]);
+  const __m128 d_vec = _mm_set1_ps (model_coefficients[3]);
+  const __m128 threshold_vec = _mm_set1_ps (threshold);
+  const __m128 normal_distance_weight_vec = _mm_set1_ps (normal_distance_weight_);
+  const __m128 abs_help = _mm_set1_ps (-0.0F); // -0.0F (negative zero) means that all bits are 0, only the sign bit is 1
+  __m128i res = _mm_set1_epi32(0); // This corresponds to nr_p: 4 32bit integers that, summed together, hold the number of inliers
+  for (; (i + 4) <= indices_->size (); i += 4)
+  {
+    const __m128 d_euclid_vec = pcl::SampleConsensusModelPlane<PointT>::dist4 (i, a_vec, b_vec, c_vec, d_vec, abs_help);
+
+    const __m128 d_normal_vec = getAcuteAngle3DSSE (
+                                  _mm_set_ps ((*normals_)[(*indices_)[i  ]].normal_x,
+                                              (*normals_)[(*indices_)[i+1]].normal_x,
+                                              (*normals_)[(*indices_)[i+2]].normal_x,
+                                              (*normals_)[(*indices_)[i+3]].normal_x),
+                                  _mm_set_ps ((*normals_)[(*indices_)[i  ]].normal_y,
+                                              (*normals_)[(*indices_)[i+1]].normal_y,
+                                              (*normals_)[(*indices_)[i+2]].normal_y,
+                                              (*normals_)[(*indices_)[i+3]].normal_y),
+                                  _mm_set_ps ((*normals_)[(*indices_)[i  ]].normal_z,
+                                              (*normals_)[(*indices_)[i+1]].normal_z,
+                                              (*normals_)[(*indices_)[i+2]].normal_z,
+                                              (*normals_)[(*indices_)[i+3]].normal_z),
+                                  a_vec, b_vec, c_vec);
+    const __m128 weight_vec = _mm_mul_ps (normal_distance_weight_vec, _mm_sub_ps (_mm_set1_ps (1.0f),
+                                  _mm_set_ps ((*normals_)[(*indices_)[i  ]].curvature,
+                                              (*normals_)[(*indices_)[i+1]].curvature,
+                                              (*normals_)[(*indices_)[i+2]].curvature,
+                                              (*normals_)[(*indices_)[i+3]].curvature)));
+    const __m128 dist = _mm_andnot_ps (abs_help, _mm_add_ps (_mm_mul_ps (weight_vec, d_normal_vec), _mm_mul_ps (_mm_sub_ps (_mm_set1_ps (1.0f), weight_vec), d_euclid_vec)));
+    const __m128 mask = _mm_cmplt_ps (dist, threshold_vec); // The mask contains 1 bits if the corresponding points are inliers, else 0 bits
+    res = _mm_add_epi32 (res, _mm_and_si128 (_mm_set1_epi32 (1), _mm_castps_si128 (mask))); // The latter part creates a vector with ones (as 32bit integers) where the points are inliers
+  }
+  nr_p += _mm_extract_epi32 (res, 0);
+  nr_p += _mm_extract_epi32 (res, 1);
+  nr_p += _mm_extract_epi32 (res, 2);
+  nr_p += _mm_extract_epi32 (res, 3);
+
+  // Process the remaining points (at most 3)
+  nr_p += countWithinDistanceStandard(model_coefficients, threshold, i);
+  return (nr_p);
+}
+#endif
+
+//////////////////////////////////////////////////////////////////////////
+#if defined (__AVX__) && defined (__AVX2__)
+template <typename PointT, typename PointNT> std::size_t
+pcl::SampleConsensusModelNormalPlane<PointT, PointNT>::countWithinDistanceAVX (
+      const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i) const
+{
+  std::size_t nr_p = 0;
+  const __m256 a_vec = _mm256_set1_ps (model_coefficients[0]);
+  const __m256 b_vec = _mm256_set1_ps (model_coefficients[1]);
+  const __m256 c_vec = _mm256_set1_ps (model_coefficients[2]);
+  const __m256 d_vec = _mm256_set1_ps (model_coefficients[3]);
+  const __m256 threshold_vec = _mm256_set1_ps (threshold);
+  const __m256 normal_distance_weight_vec = _mm256_set1_ps (normal_distance_weight_);
+  const __m256 abs_help = _mm256_set1_ps (-0.0F); // -0.0F (negative zero) means that all bits are 0, only the sign bit is 1
+  __m256i res = _mm256_set1_epi32(0); // This corresponds to nr_p: 8 32bit integers that, summed together, hold the number of inliers
+  for (; (i + 8) <= indices_->size (); i += 8)
+  {
+    const __m256 d_euclid_vec = pcl::SampleConsensusModelPlane<PointT>::dist8 (i, a_vec, b_vec, c_vec, d_vec, abs_help);
+
+    const __m256 d_normal_vec = getAcuteAngle3DAVX (
+                                  _mm256_set_ps ((*normals_)[(*indices_)[i  ]].normal_x,
+                                                 (*normals_)[(*indices_)[i+1]].normal_x,
+                                                 (*normals_)[(*indices_)[i+2]].normal_x,
+                                                 (*normals_)[(*indices_)[i+3]].normal_x,
+                                                 (*normals_)[(*indices_)[i+4]].normal_x,
+                                                 (*normals_)[(*indices_)[i+5]].normal_x,
+                                                 (*normals_)[(*indices_)[i+6]].normal_x,
+                                                 (*normals_)[(*indices_)[i+7]].normal_x),
+                                  _mm256_set_ps ((*normals_)[(*indices_)[i  ]].normal_y,
+                                                 (*normals_)[(*indices_)[i+1]].normal_y,
+                                                 (*normals_)[(*indices_)[i+2]].normal_y,
+                                                 (*normals_)[(*indices_)[i+3]].normal_y,
+                                                 (*normals_)[(*indices_)[i+4]].normal_y,
+                                                 (*normals_)[(*indices_)[i+5]].normal_y,
+                                                 (*normals_)[(*indices_)[i+6]].normal_y,
+                                                 (*normals_)[(*indices_)[i+7]].normal_y),
+                                  _mm256_set_ps ((*normals_)[(*indices_)[i  ]].normal_z,
+                                                 (*normals_)[(*indices_)[i+1]].normal_z,
+                                                 (*normals_)[(*indices_)[i+2]].normal_z,
+                                                 (*normals_)[(*indices_)[i+3]].normal_z,
+                                                 (*normals_)[(*indices_)[i+4]].normal_z,
+                                                 (*normals_)[(*indices_)[i+5]].normal_z,
+                                                 (*normals_)[(*indices_)[i+6]].normal_z,
+                                                 (*normals_)[(*indices_)[i+7]].normal_z),
+                                  a_vec, b_vec, c_vec);
+    const __m256 weight_vec = _mm256_mul_ps (normal_distance_weight_vec, _mm256_sub_ps (_mm256_set1_ps (1.0f),
+                                  _mm256_set_ps ((*normals_)[(*indices_)[i  ]].curvature,
+                                                 (*normals_)[(*indices_)[i+1]].curvature,
+                                                 (*normals_)[(*indices_)[i+2]].curvature,
+                                                 (*normals_)[(*indices_)[i+3]].curvature,
+                                                 (*normals_)[(*indices_)[i+4]].curvature,
+                                                 (*normals_)[(*indices_)[i+5]].curvature,
+                                                 (*normals_)[(*indices_)[i+6]].curvature,
+                                                 (*normals_)[(*indices_)[i+7]].curvature)));
+    const __m256 dist = _mm256_andnot_ps (abs_help, _mm256_add_ps (_mm256_mul_ps (weight_vec, d_normal_vec), _mm256_mul_ps (_mm256_sub_ps (_mm256_set1_ps (1.0f), weight_vec), d_euclid_vec)));
+    const __m256 mask = _mm256_cmp_ps (dist, threshold_vec, _CMP_LT_OQ); // The mask contains 1 bits if the corresponding points are inliers, else 0 bits
+    res = _mm256_add_epi32 (res, _mm256_and_si256 (_mm256_set1_epi32 (1), _mm256_castps_si256 (mask))); // The latter part creates a vector with ones (as 32bit integers) where the points are inliers
+  }
+  nr_p += _mm256_extract_epi32 (res, 0);
+  nr_p += _mm256_extract_epi32 (res, 1);
+  nr_p += _mm256_extract_epi32 (res, 2);
+  nr_p += _mm256_extract_epi32 (res, 3);
+  nr_p += _mm256_extract_epi32 (res, 4);
+  nr_p += _mm256_extract_epi32 (res, 5);
+  nr_p += _mm256_extract_epi32 (res, 6);
+  nr_p += _mm256_extract_epi32 (res, 7);
+
+  // Process the remaining points (at most 7)
+  nr_p += countWithinDistanceStandard(model_coefficients, threshold, i);
+  return (nr_p);
+}
+#endif
+
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT, typename PointNT> void
 pcl::SampleConsensusModelNormalPlane<PointT, PointNT>::getDistancesToModel (
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 86756020..b83d0f70 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
@@ -42,6 +42,7 @@
 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_SPHERE_H_
 
 #include <pcl/sample_consensus/sac_model_normal_sphere.h>
+#include <pcl/common/common.h> // for getAngle3D
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT, typename PointNT> void
@@ -81,19 +82,20 @@ pcl::SampleConsensusModelNormalSphere<PointT, PointNT>::selectWithinDistance (
                        (*input_)[(*indices_)[i]].z, 
                        0.0f);
 
+    Eigen::Vector4f n_dir = p - center;
+    const double weighted_euclid_dist = (1.0 - normal_distance_weight_) * std::abs (n_dir.norm () - model_coefficients[3]);
+    if (weighted_euclid_dist > threshold) // Early termination: cannot be an inlier
+      continue;
+
+    // Calculate the angular distance between the point normal and the sphere normal
     Eigen::Vector4f n ((*normals_)[(*indices_)[i]].normal[0], 
                        (*normals_)[(*indices_)[i]].normal[1], 
                        (*normals_)[(*indices_)[i]].normal[2], 
                        0.0f);
-
-    Eigen::Vector4f n_dir = p - center;
-    double d_euclid = std::abs (n_dir.norm () - model_coefficients[3]);
-
-    // Calculate the angular distance between the point normal and the sphere normal
     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.0 - normal_distance_weight_) * d_euclid);
+    double distance = std::abs (normal_distance_weight_ * d_normal + weighted_euclid_dist);
     if (distance < threshold)
     {
       // Returns the indices of the points whose distances are smaller than the threshold
@@ -135,19 +137,20 @@ pcl::SampleConsensusModelNormalSphere<PointT, PointNT>::countWithinDistance (
                        (*input_)[(*indices_)[i]].z, 
                        0.0f);
 
+    Eigen::Vector4f n_dir = (p-center);
+    const double weighted_euclid_dist = (1.0 - normal_distance_weight_) * std::abs (n_dir.norm () - model_coefficients[3]);
+    if (weighted_euclid_dist > threshold) // Early termination: cannot be an inlier
+      continue;
+
+    // Calculate the angular distance between the point normal and the sphere normal
     Eigen::Vector4f n ((*normals_)[(*indices_)[i]].normal[0], 
                        (*normals_)[(*indices_)[i]].normal[1], 
                        (*normals_)[(*indices_)[i]].normal[2], 
                        0.0f);
-
-    Eigen::Vector4f n_dir = (p-center);
-    double d_euclid = std::abs (n_dir.norm () - model_coefficients[3]);
-    //
-    // Calculate the angular distance between the point normal and the sphere normal
     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.0 - normal_distance_weight_) * d_euclid) < threshold)
+    if (std::abs (normal_distance_weight_ * d_normal + weighted_euclid_dist) < threshold)
       nr_p++;
   }
   return (nr_p);
@@ -187,19 +190,18 @@ pcl::SampleConsensusModelNormalSphere<PointT, PointNT>::getDistancesToModel (
                        (*input_)[(*indices_)[i]].z, 
                        0.0f);
 
+    Eigen::Vector4f n_dir = (p-center);
+    const double weighted_euclid_dist = (1.0 - normal_distance_weight_) * std::abs (n_dir.norm () - model_coefficients[3]);
+
+    // Calculate the angular distance between the point normal and the sphere normal
     Eigen::Vector4f n ((*normals_)[(*indices_)[i]].normal[0], 
                        (*normals_)[(*indices_)[i]].normal[1], 
                        (*normals_)[(*indices_)[i]].normal[2], 
                        0.0f);
-
-    Eigen::Vector4f n_dir = (p-center);
-    double d_euclid = std::abs (n_dir.norm () - model_coefficients[3]);
-    //
-    // Calculate the angular distance between the point normal and the sphere normal
     double d_normal = std::abs (getAngle3D (n, 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 + weighted_euclid_dist);
   }
 }
 
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 b6d74a46..ede7c2de 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
@@ -107,6 +107,7 @@ pcl::SampleConsensusModelParallelLine<PointT>::isModelValid (const Eigen::Vector
     // Check whether the current line model satisfies our angle threshold criterion with respect to the given axis
     if (angle_diff > eps_angle_)
     {
+      PCL_DEBUG ("[pcl::SampleConsensusModelParallelLine::isModelValid] Angle between line direction and given axis is too large.\n");
       return (false);
     }
   }
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 e2db1286..44b7765b 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
@@ -107,6 +107,7 @@ pcl::SampleConsensusModelParallelPlane<PointT>::isModelValid (const Eigen::Vecto
     Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0.0f);
     if (std::abs (axis.dot (coeff)) > sin_angle_)
     {
+      PCL_DEBUG ("[pcl::SampleConsensusModelParallelPlane::isModelValid] Angle between plane normal and given axis is too large.\n");
       return  (false);
     }
   }
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 9f04f431..b066b7a0 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
@@ -42,6 +42,7 @@
 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PERPENDICULAR_PLANE_H_
 
 #include <pcl/sample_consensus/sac_model_perpendicular_plane.h>
+#include <pcl/common/common.h> // for getAngle3D
 
 //////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
@@ -109,6 +110,8 @@ pcl::SampleConsensusModelPerpendicularPlane<PointT>::isModelValid (const Eigen::
     // Check whether the current plane model satisfies our angle threshold criterion with respect to the given axis
     if (angle_diff > eps_angle_)
     {
+      PCL_DEBUG ("[pcl::SampleConsensusModelPerpendicularPlane::isModelValid] Angle between plane normal and given axis should be smaller than %g, but is %g.\n",
+                 eps_angle_, angle_diff);
       return (false);
     }
   }
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 ea998574..28ab2026 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
@@ -107,9 +107,41 @@ pcl::SampleConsensusModelPlane<PointT>::computeModelCoefficients (
   // ... + d = 0
   model_coefficients[3] = -1.0f * (model_coefficients.template head<4>().dot (p0.matrix ()));
 
+  PCL_DEBUG ("[pcl::SampleConsensusModelPlane::computeModelCoefficients] Model is (%g,%g,%g,%g).\n",
+             model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3]);
   return (true);
 }
 
+#define AT(POS) ((*input_)[(*indices_)[(POS)]])
+
+#ifdef __AVX__
+// This function computes the distances of 8 points to the plane
+template <typename PointT> inline __m256 pcl::SampleConsensusModelPlane<PointT>::dist8 (const std::size_t i, const __m256 &a_vec, const __m256 &b_vec, const __m256 &c_vec, const __m256 &d_vec, const __m256 &abs_help) const
+{
+  // The andnot-function realizes an abs-operation: the sign bit is removed
+  return _mm256_andnot_ps (abs_help,
+        _mm256_add_ps (_mm256_add_ps (_mm256_mul_ps (a_vec, _mm256_set_ps (AT(i  ).x, AT(i+1).x, AT(i+2).x, AT(i+3).x, AT(i+4).x, AT(i+5).x, AT(i+6).x, AT(i+7).x)),
+                                      _mm256_mul_ps (b_vec, _mm256_set_ps (AT(i  ).y, AT(i+1).y, AT(i+2).y, AT(i+3).y, AT(i+4).y, AT(i+5).y, AT(i+6).y, AT(i+7).y))),
+                       _mm256_add_ps (_mm256_mul_ps (c_vec, _mm256_set_ps (AT(i  ).z, AT(i+1).z, AT(i+2).z, AT(i+3).z, AT(i+4).z, AT(i+5).z, AT(i+6).z, AT(i+7).z)),
+                                      d_vec))); // TODO this could be replaced by three fmadd-instructions (if available), but the speed gain would probably be minimal
+}
+#endif // ifdef __AVX__
+
+#ifdef __SSE__
+// This function computes the distances of 4 points to the plane
+template <typename PointT> inline __m128 pcl::SampleConsensusModelPlane<PointT>::dist4 (const std::size_t i, const __m128 &a_vec, const __m128 &b_vec, const __m128 &c_vec, const __m128 &d_vec, const __m128 &abs_help) const
+{
+  // The andnot-function realizes an abs-operation: the sign bit is removed
+  return _mm_andnot_ps (abs_help,
+        _mm_add_ps (_mm_add_ps (_mm_mul_ps (a_vec, _mm_set_ps (AT(i  ).x, AT(i+1).x, AT(i+2).x, AT(i+3).x)),
+                                _mm_mul_ps (b_vec, _mm_set_ps (AT(i  ).y, AT(i+1).y, AT(i+2).y, AT(i+3).y))),
+                    _mm_add_ps (_mm_mul_ps (c_vec, _mm_set_ps (AT(i  ).z, AT(i+1).z, AT(i+2).z, AT(i+3).z)),
+                                d_vec))); // TODO this could be replaced by three fmadd-instructions (if available), but the speed gain would probably be minimal
+}
+#endif // ifdef __SSE__
+
+#undef AT
+
 //////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
 pcl::SampleConsensusModelPlane<PointT>::getDistancesToModel (
@@ -190,11 +222,23 @@ pcl::SampleConsensusModelPlane<PointT>::countWithinDistance (
     PCL_ERROR ("[pcl::SampleConsensusModelPlane::countWithinDistance] Given model is invalid!\n");
     return (0);
   }
+#if defined (__AVX__) && defined (__AVX2__)
+  return countWithinDistanceAVX (model_coefficients, threshold);
+#elif defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
+  return countWithinDistanceSSE (model_coefficients, threshold);
+#else
+  return countWithinDistanceStandard (model_coefficients, threshold);
+#endif
+}
 
+//////////////////////////////////////////////////////////////////////////
+template <typename PointT> std::size_t
+pcl::SampleConsensusModelPlane<PointT>::countWithinDistanceStandard (
+      const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i) const
+{
   std::size_t nr_p = 0;
-
   // Iterate through the 3d points and calculate the distances from them to the plane
-  for (std::size_t i = 0; i < indices_->size (); ++i)
+  for (; i < indices_->size (); ++i)
   {
     // Calculate the distance from the point to the plane normal as the dot product
     // D = (P-A).N/|N|
@@ -210,6 +254,84 @@ pcl::SampleConsensusModelPlane<PointT>::countWithinDistance (
   return (nr_p);
 }
 
+//////////////////////////////////////////////////////////////////////////
+#if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
+template <typename PointT> std::size_t
+pcl::SampleConsensusModelPlane<PointT>::countWithinDistanceSSE (
+      const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i) const
+{
+  std::size_t nr_p = 0;
+  const __m128 a_vec = _mm_set1_ps (model_coefficients[0]);
+  const __m128 b_vec = _mm_set1_ps (model_coefficients[1]);
+  const __m128 c_vec = _mm_set1_ps (model_coefficients[2]);
+  const __m128 d_vec = _mm_set1_ps (model_coefficients[3]);
+  const __m128 threshold_vec = _mm_set1_ps (threshold);
+  const __m128 abs_help = _mm_set1_ps (-0.0F); // -0.0F (negative zero) means that all bits are 0, only the sign bit is 1
+  __m128i res = _mm_set1_epi32(0); // This corresponds to nr_p: 4 32bit integers that, summed together, hold the number of inliers
+  for (; (i + 4) <= indices_->size (); i += 4)
+  {
+    const __m128 mask = _mm_cmplt_ps (dist4 (i, a_vec, b_vec, c_vec, d_vec, abs_help), threshold_vec); // The mask contains 1 bits if the corresponding points are inliers, else 0 bits
+    res = _mm_add_epi32 (res, _mm_and_si128 (_mm_set1_epi32 (1), _mm_castps_si128 (mask))); // The latter part creates a vector with ones (as 32bit integers) where the points are inliers
+    //const int res = _mm_movemask_ps (mask);
+    //if (res & 1) nr_p++;
+    //if (res & 2) nr_p++;
+    //if (res & 4) nr_p++;
+    //if (res & 8) nr_p++;
+  }
+  nr_p += _mm_extract_epi32 (res, 0);
+  nr_p += _mm_extract_epi32 (res, 1);
+  nr_p += _mm_extract_epi32 (res, 2);
+  nr_p += _mm_extract_epi32 (res, 3);
+
+  // Process the remaining points (at most 3)
+  nr_p += countWithinDistanceStandard(model_coefficients, threshold, i);
+  return (nr_p);
+}
+#endif
+
+//////////////////////////////////////////////////////////////////////////
+#if defined (__AVX__) && defined (__AVX2__)
+template <typename PointT> std::size_t
+pcl::SampleConsensusModelPlane<PointT>::countWithinDistanceAVX (
+      const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i) const
+{
+  std::size_t nr_p = 0;
+  const __m256 a_vec = _mm256_set1_ps (model_coefficients[0]);
+  const __m256 b_vec = _mm256_set1_ps (model_coefficients[1]);
+  const __m256 c_vec = _mm256_set1_ps (model_coefficients[2]);
+  const __m256 d_vec = _mm256_set1_ps (model_coefficients[3]);
+  const __m256 threshold_vec = _mm256_set1_ps (threshold);
+  const __m256 abs_help = _mm256_set1_ps (-0.0F); // -0.0F (negative zero) means that all bits are 0, only the sign bit is 1
+  __m256i res = _mm256_set1_epi32(0); // This corresponds to nr_p: 8 32bit integers that, summed together, hold the number of inliers
+  for (; (i + 8) <= indices_->size (); i += 8)
+  {
+    const __m256 mask = _mm256_cmp_ps (dist8 (i, a_vec, b_vec, c_vec, d_vec, abs_help), threshold_vec, _CMP_LT_OQ); // The mask contains 1 bits if the corresponding points are inliers, else 0 bits
+    res = _mm256_add_epi32 (res, _mm256_and_si256 (_mm256_set1_epi32 (1), _mm256_castps_si256 (mask))); // The latter part creates a vector with ones (as 32bit integers) where the points are inliers
+    //const int res = _mm256_movemask_ps (mask);
+    //if (res &   1) nr_p++;
+    //if (res &   2) nr_p++;
+    //if (res &   4) nr_p++;
+    //if (res &   8) nr_p++;
+    //if (res &  16) nr_p++;
+    //if (res &  32) nr_p++;
+    //if (res &  64) nr_p++;
+    //if (res & 128) nr_p++;
+  }
+  nr_p += _mm256_extract_epi32 (res, 0);
+  nr_p += _mm256_extract_epi32 (res, 1);
+  nr_p += _mm256_extract_epi32 (res, 2);
+  nr_p += _mm256_extract_epi32 (res, 3);
+  nr_p += _mm256_extract_epi32 (res, 4);
+  nr_p += _mm256_extract_epi32 (res, 5);
+  nr_p += _mm256_extract_epi32 (res, 6);
+  nr_p += _mm256_extract_epi32 (res, 7);
+
+  // Process the remaining points (at most 7)
+  nr_p += countWithinDistanceStandard(model_coefficients, threshold, i);
+  return (nr_p);
+}
+#endif
+
 //////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
 pcl::SampleConsensusModelPlane<PointT>::optimizeModelCoefficients (
@@ -288,7 +410,7 @@ pcl::SampleConsensusModelPlane<PointT>::projectPoints (
   if (copy_data_fields)
   {
     // Allocate enough space and copy the basics
-    projected_points.points.resize (input_->size ());
+    projected_points.resize (input_->size ());
     projected_points.width    = input_->width;
     projected_points.height   = input_->height;
 
@@ -316,7 +438,7 @@ pcl::SampleConsensusModelPlane<PointT>::projectPoints (
   else
   {
     // Allocate enough space and copy the basics
-    projected_points.points.resize (inliers.size ());
+    projected_points.resize (inliers.size ());
     projected_points.width    = inliers.size ();
     projected_points.height   = 1;
 
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 2c53579d..b81617d8 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,7 +42,6 @@
 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_H_
 
 #include <pcl/sample_consensus/sac_model_registration.h>
-#include <pcl/common/eigen.h>
 
 //////////////////////////////////////////////////////////////////////////
 template <typename PointT> bool
@@ -83,7 +82,14 @@ pcl::SampleConsensusModelRegistration<PointT>::computeModelCoefficients (const I
   Indices indices_tgt (3);
   for (int i = 0; i < 3; ++i)
   {
-    indices_tgt[i] = correspondences_.at (samples[i]);
+    const auto it = correspondences_.find (samples[i]);
+    if (it == correspondences_.cend ())
+    {
+      PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeModelCoefficients] Element with key %i is not in map (map contains %lu elements).\n",
+                 samples[i], correspondences_.size ());
+      return (false);
+    }
+    indices_tgt[i] = it->second;
   }
 
   estimateRigidTransformationSVD (*input_, samples, *target_, indices_tgt, model_coefficients);
@@ -264,7 +270,15 @@ pcl::SampleConsensusModelRegistration<PointT>::optimizeModelCoefficients (const
   for (std::size_t i = 0; i < inliers.size (); ++i)
   {
     indices_src[i] = inliers[i];
-    indices_tgt[i] = correspondences_.at (indices_src[i]);
+    const auto it = correspondences_.find (indices_src[i]);
+    if (it == correspondences_.cend ())
+    {
+      PCL_ERROR ("[pcl::SampleConsensusModelRegistration::optimizeModelCoefficients] Element with key %i is not in map (map contains %lu elements).\n",
+                 indices_src[i], correspondences_.size ());
+      optimized_coefficients = model_coefficients;
+      return;
+    }
+    indices_tgt[i] = it->second;
   }
 
   estimateRigidTransformationSVD (*input_, indices_src, *target_, indices_tgt, optimized_coefficients);
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 9bda670d..eac63585 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,7 +39,6 @@
 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_2D_HPP_
 
 #include <pcl/sample_consensus/sac_model_registration_2d.h>
-#include <pcl/common/eigen.h>
 
 //////////////////////////////////////////////////////////////////////////
 template <typename PointT> bool
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 518f58cb..fdb7bb92 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
@@ -41,7 +41,7 @@
 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_SPHERE_H_
 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_SPHERE_H_
 
-#include <pcl/sample_consensus/eigen.h>
+#include <unsupported/Eigen/NonLinearOptimization> // for LevenbergMarquardt
 #include <pcl/sample_consensus/sac_model_sphere.h>
 
 //////////////////////////////////////////////////////////////////////////
@@ -123,9 +123,37 @@ pcl::SampleConsensusModelSphere<PointT>::computeModelCoefficients (
                                      model_coefficients[1] * model_coefficients[1] +
                                      model_coefficients[2] * model_coefficients[2] - m15 / m11);
 
+  PCL_DEBUG ("[pcl::SampleConsensusModelSphere::computeModelCoefficients] Model is (%g,%g,%g,%g)\n",
+             model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3]);
   return (true);
 }
 
+#define AT(POS) ((*input_)[(*indices_)[(POS)]])
+
+#ifdef __AVX__
+// This function computes the squared distances (i.e. the distances without the square root) of 8 points to the center of the sphere
+template <typename PointT> inline __m256 pcl::SampleConsensusModelSphere<PointT>::sqr_dist8 (const std::size_t i, const __m256 a_vec, const __m256 b_vec, const __m256 c_vec) const
+{
+  const __m256 tmp1 = _mm256_sub_ps (_mm256_set_ps (AT(i  ).x, AT(i+1).x, AT(i+2).x, AT(i+3).x, AT(i+4).x, AT(i+5).x, AT(i+6).x, AT(i+7).x), a_vec);
+  const __m256 tmp2 = _mm256_sub_ps (_mm256_set_ps (AT(i  ).y, AT(i+1).y, AT(i+2).y, AT(i+3).y, AT(i+4).y, AT(i+5).y, AT(i+6).y, AT(i+7).y), b_vec);
+  const __m256 tmp3 = _mm256_sub_ps (_mm256_set_ps (AT(i  ).z, AT(i+1).z, AT(i+2).z, AT(i+3).z, AT(i+4).z, AT(i+5).z, AT(i+6).z, AT(i+7).z), c_vec);
+  return _mm256_add_ps (_mm256_add_ps (_mm256_mul_ps (tmp1, tmp1), _mm256_mul_ps (tmp2, tmp2)), _mm256_mul_ps(tmp3, tmp3));
+}
+#endif // ifdef __AVX__
+
+#ifdef __SSE__
+// This function computes the squared distances (i.e. the distances without the square root) of 4 points to the center of the sphere
+template <typename PointT> inline __m128 pcl::SampleConsensusModelSphere<PointT>::sqr_dist4 (const std::size_t i, const __m128 a_vec, const __m128 b_vec, const __m128 c_vec) const
+{
+  const __m128 tmp1 = _mm_sub_ps (_mm_set_ps (AT(i  ).x, AT(i+1).x, AT(i+2).x, AT(i+3).x), a_vec);
+  const __m128 tmp2 = _mm_sub_ps (_mm_set_ps (AT(i  ).y, AT(i+1).y, AT(i+2).y, AT(i+3).y), b_vec);
+  const __m128 tmp3 = _mm_sub_ps (_mm_set_ps (AT(i  ).z, AT(i+1).z, AT(i+2).z, AT(i+3).z), c_vec);
+  return _mm_add_ps (_mm_add_ps (_mm_mul_ps (tmp1, tmp1), _mm_mul_ps (tmp2, tmp2)), _mm_mul_ps(tmp3, tmp3));
+}
+#endif // ifdef __SSE__
+
+#undef AT
+
 //////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
 pcl::SampleConsensusModelSphere<PointT>::getDistancesToModel (
@@ -139,21 +167,13 @@ pcl::SampleConsensusModelSphere<PointT>::getDistancesToModel (
   }
   distances.resize (indices_->size ());
 
+  const Eigen::Vector3f center (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
   // 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 (
-                               ( (*input_)[(*indices_)[i]].x - model_coefficients[0] ) *
-                               ( (*input_)[(*indices_)[i]].x - model_coefficients[0] ) +
-
-                               ( (*input_)[(*indices_)[i]].y - model_coefficients[1] ) *
-                               ( (*input_)[(*indices_)[i]].y - model_coefficients[1] ) +
-
-                               ( (*input_)[(*indices_)[i]].z - model_coefficients[2] ) *
-                               ( (*input_)[(*indices_)[i]].z - model_coefficients[2] )
-                               ) - model_coefficients[3]);
+    distances[i] = std::abs (((*input_)[(*indices_)[i]].getVector3fMap () - center).norm () - model_coefficients[3]);
   }
 }
 
@@ -174,26 +194,21 @@ pcl::SampleConsensusModelSphere<PointT>::selectWithinDistance (
   inliers.reserve (indices_->size ());
   error_sqr_dists_.reserve (indices_->size ());
 
+  const float sqr_inner_radius = (model_coefficients[3] <= threshold ? 0.0f : (model_coefficients[3] - threshold) * (model_coefficients[3] - threshold));
+  const float sqr_outer_radius = (model_coefficients[3] + threshold) * (model_coefficients[3] + threshold);
+  const Eigen::Vector3f center (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
   // Iterate through the 3d points and calculate the distances from them to the sphere
   for (std::size_t i = 0; i < indices_->size (); ++i)
   {
-    double distance = std::abs (std::sqrt (
-                          ( (*input_)[(*indices_)[i]].x - model_coefficients[0] ) *
-                          ( (*input_)[(*indices_)[i]].x - model_coefficients[0] ) +
-
-                          ( (*input_)[(*indices_)[i]].y - model_coefficients[1] ) *
-                          ( (*input_)[(*indices_)[i]].y - model_coefficients[1] ) +
-
-                          ( (*input_)[(*indices_)[i]].z - model_coefficients[2] ) *
-                          ( (*input_)[(*indices_)[i]].z - model_coefficients[2] )
-                          ) - model_coefficients[3]);
-    // Calculate the distance from the point to the sphere as the difference between
-    // dist(point,sphere_origin) and sphere_radius
-    if (distance < threshold)
+    // To avoid sqrt computation: consider one larger sphere (radius + threshold) and one smaller sphere (radius - threshold).
+    // Valid if point is in larger sphere, but not in smaller sphere.
+    const float sqr_dist = ((*input_)[(*indices_)[i]].getVector3fMap () - center).squaredNorm ();
+    if ((sqr_dist <= sqr_outer_radius) && (sqr_dist >= sqr_inner_radius))
     {
       // Returns the indices of the points whose distances are smaller than the threshold
       inliers.push_back ((*indices_)[i]);
-      error_sqr_dists_.push_back (static_cast<double> (distance));
+      // Only compute exact distance if necessary (if point is inlier)
+      error_sqr_dists_.push_back (static_cast<double> (std::abs (std::sqrt (sqr_dist) - model_coefficients[3])));
     }
   }
 }
@@ -207,27 +222,115 @@ pcl::SampleConsensusModelSphere<PointT>::countWithinDistance (
   if (!isModelValid (model_coefficients))
     return (0);
 
-  std::size_t nr_p = 0;
+#if defined (__AVX__) && defined (__AVX2__)
+  return countWithinDistanceAVX (model_coefficients, threshold);
+#elif defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
+  return countWithinDistanceSSE (model_coefficients, threshold);
+#else
+  return countWithinDistanceStandard (model_coefficients, threshold);
+#endif
+}
 
+//////////////////////////////////////////////////////////////////////////
+template <typename PointT> std::size_t
+pcl::SampleConsensusModelSphere<PointT>::countWithinDistanceStandard (
+      const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i) const
+{
+  std::size_t nr_p = 0;
+  const float sqr_inner_radius = (model_coefficients[3] <= threshold ? 0.0f : (model_coefficients[3] - threshold) * (model_coefficients[3] - threshold));
+  const float sqr_outer_radius = (model_coefficients[3] + threshold) * (model_coefficients[3] + threshold);
+  const Eigen::Vector3f center (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
   // Iterate through the 3d points and calculate the distances from them to the sphere
-  for (std::size_t i = 0; i < indices_->size (); ++i)
+  for (; i < indices_->size (); ++i)
   {
-    // Calculate the distance from the point to the sphere as the difference between
-    // dist(point,sphere_origin) and sphere_radius
-    if (std::abs (std::sqrt (
-                        ( (*input_)[(*indices_)[i]].x - model_coefficients[0] ) *
-                        ( (*input_)[(*indices_)[i]].x - model_coefficients[0] ) +
+    // To avoid sqrt computation: consider one larger sphere (radius + threshold) and one smaller sphere (radius - threshold).
+    // Valid if point is in larger sphere, but not in smaller sphere.
+    const float sqr_dist = ((*input_)[(*indices_)[i]].getVector3fMap () - center).squaredNorm ();
+    if ((sqr_dist <= sqr_outer_radius) && (sqr_dist >= sqr_inner_radius))
+      nr_p++;
+  }
+  return (nr_p);
+}
 
-                        ( (*input_)[(*indices_)[i]].y - model_coefficients[1] ) *
-                        ( (*input_)[(*indices_)[i]].y - model_coefficients[1] ) +
+//////////////////////////////////////////////////////////////////////////
+#if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
+template <typename PointT> std::size_t
+pcl::SampleConsensusModelSphere<PointT>::countWithinDistanceSSE (
+      const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i) const
+{
+  std::size_t nr_p = 0;
+  const __m128 a_vec = _mm_set1_ps (model_coefficients[0]);
+  const __m128 b_vec = _mm_set1_ps (model_coefficients[1]);
+  const __m128 c_vec = _mm_set1_ps (model_coefficients[2]);
+  // To avoid sqrt computation: consider one larger sphere (radius + threshold) and one smaller sphere (radius - threshold). Valid if point is in larger sphere, but not in smaller sphere.
+  const __m128 sqr_inner_radius = _mm_set1_ps ((model_coefficients[3] <= threshold ? 0.0 : (model_coefficients[3]-threshold)*(model_coefficients[3]-threshold)));
+  const __m128 sqr_outer_radius = _mm_set1_ps ((model_coefficients[3]+threshold)*(model_coefficients[3]+threshold));
+  __m128i res = _mm_set1_epi32(0); // This corresponds to nr_p: 4 32bit integers that, summed together, hold the number of inliers
+  for (; (i + 4) <= indices_->size (); i += 4)
+  {
+    const __m128 sqr_dist = sqr_dist4 (i, a_vec, b_vec, c_vec);
+    const __m128 mask = _mm_and_ps (_mm_cmplt_ps (sqr_inner_radius, sqr_dist), _mm_cmplt_ps (sqr_dist, sqr_outer_radius)); // The mask contains 1 bits if the corresponding points are inliers, else 0 bits
+    res = _mm_add_epi32 (res, _mm_and_si128 (_mm_set1_epi32 (1), _mm_castps_si128 (mask))); // The latter part creates a vector with ones (as 32bit integers) where the points are inliers
+    //const int res = _mm_movemask_ps (mask);
+    //if (res & 1) nr_p++;
+    //if (res & 2) nr_p++;
+    //if (res & 4) nr_p++;
+    //if (res & 8) nr_p++;
+  }
+  nr_p += _mm_extract_epi32 (res, 0);
+  nr_p += _mm_extract_epi32 (res, 1);
+  nr_p += _mm_extract_epi32 (res, 2);
+  nr_p += _mm_extract_epi32 (res, 3);
 
-                        ( (*input_)[(*indices_)[i]].z - model_coefficients[2] ) *
-                        ( (*input_)[(*indices_)[i]].z - model_coefficients[2] )
-                        ) - model_coefficients[3]) < threshold)
-      nr_p++;
+  // Process the remaining points (at most 3)
+  nr_p += countWithinDistanceStandard (model_coefficients, threshold, i);
+  return (nr_p);
+}
+#endif
+
+//////////////////////////////////////////////////////////////////////////
+#if defined (__AVX__) && defined (__AVX2__)
+template <typename PointT> std::size_t
+pcl::SampleConsensusModelSphere<PointT>::countWithinDistanceAVX (
+      const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i) const
+{
+  std::size_t nr_p = 0;
+  const __m256 a_vec = _mm256_set1_ps (model_coefficients[0]);
+  const __m256 b_vec = _mm256_set1_ps (model_coefficients[1]);
+  const __m256 c_vec = _mm256_set1_ps (model_coefficients[2]);
+  // To avoid sqrt computation: consider one larger sphere (radius + threshold) and one smaller sphere (radius - threshold). Valid if point is in larger sphere, but not in smaller sphere.
+  const __m256 sqr_inner_radius = _mm256_set1_ps ((model_coefficients[3] <= threshold ? 0.0 : (model_coefficients[3]-threshold)*(model_coefficients[3]-threshold)));
+  const __m256 sqr_outer_radius = _mm256_set1_ps ((model_coefficients[3]+threshold)*(model_coefficients[3]+threshold));
+  __m256i res = _mm256_set1_epi32(0); // This corresponds to nr_p: 8 32bit integers that, summed together, hold the number of inliers
+  for (; (i + 8) <= indices_->size (); i += 8)
+  {
+    const __m256 sqr_dist = sqr_dist8 (i, a_vec, b_vec, c_vec);
+    const __m256 mask = _mm256_and_ps (_mm256_cmp_ps (sqr_inner_radius, sqr_dist, _CMP_LT_OQ), _mm256_cmp_ps (sqr_dist, sqr_outer_radius, _CMP_LT_OQ)); // The mask contains 1 bits if the corresponding points are inliers, else 0 bits
+    res = _mm256_add_epi32 (res, _mm256_and_si256 (_mm256_set1_epi32 (1), _mm256_castps_si256 (mask))); // The latter part creates a vector with ones (as 32bit integers) where the points are inliers
+    //const int res = _mm256_movemask_ps (mask);
+    //if (res &   1) nr_p++;
+    //if (res &   2) nr_p++;
+    //if (res &   4) nr_p++;
+    //if (res &   8) nr_p++;
+    //if (res &  16) nr_p++;
+    //if (res &  32) nr_p++;
+    //if (res &  64) nr_p++;
+    //if (res & 128) nr_p++;
   }
+  nr_p += _mm256_extract_epi32 (res, 0);
+  nr_p += _mm256_extract_epi32 (res, 1);
+  nr_p += _mm256_extract_epi32 (res, 2);
+  nr_p += _mm256_extract_epi32 (res, 3);
+  nr_p += _mm256_extract_epi32 (res, 4);
+  nr_p += _mm256_extract_epi32 (res, 5);
+  nr_p += _mm256_extract_epi32 (res, 6);
+  nr_p += _mm256_extract_epi32 (res, 7);
+
+  // Process the remaining points (at most 7)
+  nr_p += countWithinDistanceStandard (model_coefficients, threshold, i);
   return (nr_p);
 }
+#endif
 
 //////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
@@ -273,7 +376,7 @@ pcl::SampleConsensusModelSphere<PointT>::projectPoints (
   }
 
   // Allocate enough space and copy the basics
-  projected_points.points.resize (input_->size ());
+  projected_points.resize (input_->size ());
   projected_points.header   = input_->header;
   projected_points.width    = input_->width;
   projected_points.height   = input_->height;
@@ -295,18 +398,15 @@ pcl::SampleConsensusModelSphere<PointT>::doSamplesVerifyModel (
     return (false);
   }
 
+  const float sqr_inner_radius = (model_coefficients[3] <= threshold ? 0.0f : (model_coefficients[3] - threshold) * (model_coefficients[3] - threshold));
+  const float sqr_outer_radius = (model_coefficients[3] + threshold) * (model_coefficients[3] + threshold);
+  const Eigen::Vector3f center (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
   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 (
-                    ( (*input_)[index].x - model_coefficients[0] ) *
-                    ( (*input_)[index].x - model_coefficients[0] ) +
-                    ( (*input_)[index].y - model_coefficients[1] ) *
-                    ( (*input_)[index].y - model_coefficients[1] ) +
-                    ( (*input_)[index].z - model_coefficients[2] ) *
-                    ( (*input_)[index].z - model_coefficients[2] )
-                   ) - model_coefficients[3]) > threshold)
+    // To avoid sqrt computation: consider one larger sphere (radius + threshold) and one smaller sphere (radius - threshold).
+    // Valid if point is in larger sphere, but not in smaller sphere.
+    const float sqr_dist = ((*input_)[index].getVector3fMap () - center).squaredNorm ();
+    if ((sqr_dist > sqr_outer_radius) || (sqr_dist < sqr_inner_radius))
     {
       return (false);
     }
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 e03cbe2f..fdcf11b7 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
@@ -44,6 +44,7 @@
 #include <pcl/sample_consensus/sac_model_stick.h>
 #include <pcl/common/centroid.h>
 #include <pcl/common/concatenate.h>
+#include <pcl/common/eigen.h> // for eigen33
 
 //////////////////////////////////////////////////////////////////////////
 template <typename PointT> bool
@@ -95,6 +96,9 @@ pcl::SampleConsensusModelStick<PointT>::computeModelCoefficients (
 //  model_coefficients.template segment<3> (3).normalize ();
   // We don't care about model_coefficients[6] which is the width (radius) of the stick
 
+  PCL_DEBUG ("[pcl::SampleConsensusModelStick::computeModelCoefficients] Model is (%g,%g,%g,%g,%g,%g).\n",
+             model_coefficients[0], model_coefficients[1], model_coefficients[2],
+             model_coefficients[3], model_coefficients[4], model_coefficients[5]);
   return (true);
 }
 
@@ -304,7 +308,7 @@ pcl::SampleConsensusModelStick<PointT>::projectPoints (
   if (copy_data_fields)
   {
     // Allocate enough space and copy the basics
-    projected_points.points.resize (input_->size ());
+    projected_points.resize (input_->size ());
     projected_points.width    = input_->width;
     projected_points.height   = input_->height;
 
@@ -333,7 +337,7 @@ pcl::SampleConsensusModelStick<PointT>::projectPoints (
   else
   {
     // Allocate enough space and copy the basics
-    projected_points.points.resize (inliers.size ());
+    projected_points.resize (inliers.size ());
     projected_points.width    = inliers.size ();
     projected_points.height   = 1;
 
diff --git a/sample_consensus/include/pcl/sample_consensus/lmeds.h b/sample_consensus/include/pcl/sample_consensus/lmeds.h
index b83a0262..ec4704de 100644
--- a/sample_consensus/include/pcl/sample_consensus/lmeds.h
+++ b/sample_consensus/include/pcl/sample_consensus/lmeds.h
@@ -49,6 +49,9 @@ namespace pcl
     * is a RANSAC-like model-fitting algorithm that can tolerate up to 50% outliers without requiring thresholds to be 
     * set. See Andrea Fusiello's "Elements of Geometric Computer Vision"
     * (http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/FUSIELLO4/tutorial.html#x1-520007) for more details.
+    * In contrast to RANSAC, LMedS does not divide the points into inliers and outliers when finding the model. Instead,
+    * it uses the median of all point-model distances as the measure of how good a model is. A threshold is only needed
+    * at the end, when it is determined which points belong to the found model.
     * \author Radu B. Rusu
     * \ingroup sample_consensus
     */
diff --git a/sample_consensus/include/pcl/sample_consensus/msac.h b/sample_consensus/include/pcl/sample_consensus/msac.h
index 0fa427c8..9f2f8491 100644
--- a/sample_consensus/include/pcl/sample_consensus/msac.h
+++ b/sample_consensus/include/pcl/sample_consensus/msac.h
@@ -40,7 +40,6 @@
 
 #pragma once
 
-#include <algorithm>
 #include <pcl/sample_consensus/sac.h>
 #include <pcl/sample_consensus/sac_model.h>
 
@@ -49,6 +48,11 @@ namespace pcl
   /** \brief @b MEstimatorSampleConsensus represents an implementation of the MSAC (M-estimator SAmple Consensus) 
     * algorithm, as described in: "MLESAC: A new robust estimator with application to estimating image geometry", P.H.S. 
     * Torr and A. Zisserman, Computer Vision and Image Understanding, vol 78, 2000.
+    * The difference to RANSAC is how the quality of a model is computed: RANSAC counts the number of inliers, given a
+    * threshold. The more inliers, the better the model is - it does not matter how close the inliers actually are to
+    * the model, as long as they are within the threshold. MSAC changes this by using the sum of all point-model distances
+    * as the quality measure, however outliers only add the threshold instead of their true distance. This method can lead
+    * to better results compared to RANSAC.
     * \author Radu B. Rusu
     * \ingroup sample_consensus
     */
diff --git a/sample_consensus/include/pcl/sample_consensus/prosac.h b/sample_consensus/include/pcl/sample_consensus/prosac.h
index 2929d988..b8cf6d0e 100644
--- a/sample_consensus/include/pcl/sample_consensus/prosac.h
+++ b/sample_consensus/include/pcl/sample_consensus/prosac.h
@@ -45,7 +45,7 @@
 
 namespace pcl
 {
-  /** \brief @b RandomSampleConsensus represents an implementation of the RANSAC (RAndom SAmple Consensus) algorithm, as
+  /** \brief @b ProgressiveSampleConsensus represents an implementation of the PROSAC (PROgressive SAmple Consensus) algorithm, as
     * described in: "Matching with PROSAC – Progressive Sample Consensus", Chum, O. and Matas, J.G., CVPR, I: 220-226
     * 2005.
     * \author Vincent Rabaud
diff --git a/sample_consensus/include/pcl/sample_consensus/ransac.h b/sample_consensus/include/pcl/sample_consensus/ransac.h
index 23c0302d..2e2c09ee 100644
--- a/sample_consensus/include/pcl/sample_consensus/ransac.h
+++ b/sample_consensus/include/pcl/sample_consensus/ransac.h
@@ -49,6 +49,15 @@ namespace pcl
     * described in: "Random Sample Consensus: A Paradigm for Model Fitting with Applications to Image Analysis and 
     * Automated Cartography", Martin A. Fischler and Robert C. Bolles, Comm. Of the ACM 24: 381–395, June 1981.
     * A parallel variant is available, enable with setNumberOfThreads. Default is non-parallel.
+    * 
+    * The algorithm works as follows:
+    * <ol>
+    *   <li> randomly select samples from the cloud, just as many as needed to determine a model
+    *   <li> compute the coefficients of the model from the samples
+    *   <li> count how many points of the cloud belong to the model, given a threshold. These are called inliers
+    *   <li> repeat until a good model has been found or a max number of iterations has been reached
+    *   <li> return the model with the most inliers 
+    * </ol>
     * \author Radu B. Rusu
     * \ingroup sample_consensus
     */
@@ -71,7 +80,7 @@ namespace pcl
       using SampleConsensus<PointT>::probability_;
       using SampleConsensus<PointT>::threads_;
 
-      /** \brief RANSAC (RAndom SAmple Consensus) main constructor
+      /** \brief RANSAC (RANdom SAmple Consensus) main constructor
         * \param[in] model a Sample Consensus model
         */
       RandomSampleConsensus (const SampleConsensusModelPtr &model) 
@@ -81,7 +90,7 @@ namespace pcl
         max_iterations_ = 10000;
       }
 
-      /** \brief RANSAC (RAndom SAmple Consensus) main constructor
+      /** \brief RANSAC (RANdom SAmple Consensus) main constructor
         * \param[in] model a Sample Consensus model
         * \param[in] threshold distance to model threshold
         */
diff --git a/sample_consensus/include/pcl/sample_consensus/rmsac.h b/sample_consensus/include/pcl/sample_consensus/rmsac.h
index 5f81e7ec..4e690ef3 100644
--- a/sample_consensus/include/pcl/sample_consensus/rmsac.h
+++ b/sample_consensus/include/pcl/sample_consensus/rmsac.h
@@ -40,7 +40,6 @@
 
 #pragma once
 
-#include <algorithm>
 #include <pcl/sample_consensus/sac.h>
 #include <pcl/sample_consensus/sac_model.h>
 
diff --git a/sample_consensus/include/pcl/sample_consensus/rransac.h b/sample_consensus/include/pcl/sample_consensus/rransac.h
index d2fadbca..2f07f4f2 100644
--- a/sample_consensus/include/pcl/sample_consensus/rransac.h
+++ b/sample_consensus/include/pcl/sample_consensus/rransac.h
@@ -45,9 +45,13 @@
 
 namespace pcl
 {
-  /** \brief @b RandomizedRandomSampleConsensus represents an implementation of the RRANSAC (Randomized RAndom SAmple 
+  /** \brief @b RandomizedRandomSampleConsensus represents an implementation of the RRANSAC (Randomized RANdom SAmple 
     * Consensus), as described in "Randomized RANSAC with Td,d test", O. Chum and J. Matas, Proc. British Machine Vision 
     * Conf. (BMVC '02), vol. 2, BMVA, pp. 448-457, 2002.
+    * 
+    * The algorithm works similar to RANSAC, with one addition: after computing the model coefficients, randomly select a fraction
+    * of points. If any of these points do not belong to the model (given a threshold), continue with the next iteration instead
+    * of checking all points. This may speed up the finding of the model if the fraction of points to pre-test is chosen well.
     * \note RRANSAC is useful in situations where most of the data samples belong to the model, and a fast outlier rejection algorithm is needed.
     * \author Radu B. Rusu
     * \ingroup sample_consensus
@@ -70,7 +74,7 @@ namespace pcl
       using SampleConsensus<PointT>::inliers_;
       using SampleConsensus<PointT>::probability_;
 
-      /** \brief RANSAC (Randomized RAndom SAmple Consensus) main constructor
+      /** \brief RRANSAC (Randomized RANdom SAmple Consensus) main constructor
         * \param[in] model a Sample Consensus model
         */
       RandomizedRandomSampleConsensus (const SampleConsensusModelPtr &model) 
@@ -81,7 +85,7 @@ namespace pcl
         max_iterations_ = 10000;
       }
 
-      /** \brief RRANSAC (RAndom SAmple Consensus) main constructor
+      /** \brief RRANSAC (Randomized RANdom SAmple Consensus) main constructor
         * \param[in] model a Sample Consensus model
         * \param[in] threshold distance to model threshold
         */
diff --git a/sample_consensus/include/pcl/sample_consensus/sac.h b/sample_consensus/include/pcl/sample_consensus/sac.h
index c42d3696..0c28f5a1 100644
--- a/sample_consensus/include/pcl/sample_consensus/sac.h
+++ b/sample_consensus/include/pcl/sample_consensus/sac.h
@@ -40,10 +40,12 @@
 
 #pragma once
 
-#include <pcl/sample_consensus/boost.h>
 #include <pcl/sample_consensus/sac_model.h>
 #include <pcl/pcl_base.h>
 
+#include <boost/random/mersenne_twister.hpp> // for mt19937
+#include <boost/random/uniform_01.hpp> // for uniform_01
+
 #include <ctime>
 #include <memory>
 #include <set>
diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model.h b/sample_consensus/include/pcl/sample_consensus/sac_model.h
index db448815..2fc9ef72 100644
--- a/sample_consensus/include/pcl/sample_consensus/sac_model.h
+++ b/sample_consensus/include/pcl/sample_consensus/sac_model.h
@@ -40,19 +40,18 @@
 
 #pragma once
 
-#include <cfloat>
 #include <ctime>
 #include <climits>
 #include <memory>
 #include <set>
+#include <boost/random/mersenne_twister.hpp> // for mt19937
+#include <boost/random/uniform_int.hpp> // for uniform_int
+#include <boost/random/variate_generator.hpp> // for variate_generator
 
 #include <pcl/memory.h>
-#include <pcl/pcl_macros.h>
-#include <pcl/pcl_base.h>
 #include <pcl/console/print.h>
 #include <pcl/point_cloud.h>
 #include <pcl/types.h> // for index_t, Indices
-#include <pcl/sample_consensus/boost.h>
 #include <pcl/sample_consensus/model_types.h>
 
 #include <pcl/search/search.h>
@@ -89,6 +88,7 @@ namespace pcl
         , samples_radius_ (0.)
         , samples_radius_search_ ()
         , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
+        , custom_model_constraints_ ([](auto){return true;})
       {
         // Create a random number generator object
         if (random)
@@ -111,6 +111,7 @@ namespace pcl
         , samples_radius_ (0.)
         , samples_radius_search_ ()
         , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
+        , custom_model_constraints_ ([](auto){return true;})
       {
         if (random)
           rng_alg_.seed (static_cast<unsigned> (std::time (nullptr)));
@@ -139,6 +140,7 @@ namespace pcl
         , samples_radius_ (0.)
         , samples_radius_search_ ()
         , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
+        , custom_model_constraints_ ([](auto){return true;})
       {
         if (random)
           rng_alg_.seed (static_cast<unsigned> (std::time(nullptr)));
@@ -389,6 +391,21 @@ namespace pcl
         max_radius = radius_max_;
       }
 
+      /** \brief This can be used to impose any kind of constraint on the model,
+        * e.g. that it has a specific direction, size, or anything else.
+        * \param[in] function A function that gets model coefficients and returns whether the model is acceptable or not.
+        */
+      inline void
+      setModelConstraints (std::function<bool(const Eigen::VectorXf &)> function)
+      {
+        if (!function)
+        {
+          PCL_ERROR ("[pcl::SampleConsensusModel::setModelConstraints] The given function is empty (i.e. does not contain a callable target)!\n");
+          return;
+        }
+        custom_model_constraints_ = std::move (function);
+      }
+
       /** \brief Set the maximum distance allowed when drawing random samples
         * \param[in] radius the maximum distance (L2 norm)
         * \param search
@@ -514,6 +531,11 @@ namespace pcl
           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);
         }
+        if (!custom_model_constraints_(model_coefficients))
+        {
+          PCL_DEBUG ("[pcl::%s::isModelValid] The user defined isModelValid function returned false.\n", getClassName ().c_str ());
+          return (false);
+        }
         return (true);
       }
 
@@ -574,12 +596,16 @@ namespace pcl
       {
         return ((*rng_gen_) ());
       }
+
+      /** \brief A user defined function that takes model coefficients and returns whether the model is acceptable or not. */
+      std::function<bool(const Eigen::VectorXf &)> custom_model_constraints_;
     public:
       PCL_MAKE_ALIGNED_OPERATOR_NEW
  };
 
   /** \brief @b SampleConsensusModelFromNormals represents the base model class
     * for models that require the use of surface normals for estimation.
+    * \ingroup sample_consensus
     */
   template <typename PointT, typename PointNT>
   class SampleConsensusModelFromNormals //: public SampleConsensusModel<PointT>
@@ -605,6 +631,11 @@ namespace pcl
       inline void 
       setNormalDistanceWeight (const double w) 
       { 
+        if (w < 0.0 || w > 1.0)
+        {
+          PCL_ERROR ("[pcl::SampleConsensusModel::setNormalDistanceWeight] w is %g, but should be in [0; 1]. Weight will not be set.", w);
+          return;
+        }
         normal_distance_weight_ = w; 
       }
 
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 838b5d93..6f35d9e0 100644
--- a/sample_consensus/include/pcl/sample_consensus/sac_model_circle.h
+++ b/sample_consensus/include/pcl/sample_consensus/sac_model_circle.h
@@ -40,6 +40,13 @@
 
 #pragma once
 
+#ifdef __SSE__
+#include <xmmintrin.h> // for __m128
+#endif // ifdef __SSE__
+#ifdef __AVX__
+#include <immintrin.h> // for __m256
+#endif // ifdef __AVX__
+
 #include <pcl/sample_consensus/sac_model.h>
 #include <pcl/sample_consensus/model_types.h>
 
@@ -213,6 +220,34 @@ namespace pcl
       bool
       isSampleGood(const Indices &samples) const override;
 
+      /** This implementation uses no SIMD instructions. It is not intended for normal use.
+        * See countWithinDistance which automatically uses the fastest implementation.
+        */
+      std::size_t
+      countWithinDistanceStandard (const Eigen::VectorXf &model_coefficients,
+                                   const double threshold,
+                                   std::size_t i = 0) const;
+
+#if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
+      /** This implementation uses SSE, SSE2, and SSE4.1 instructions. It is not intended for normal use.
+        * See countWithinDistance which automatically uses the fastest implementation.
+        */
+      std::size_t
+      countWithinDistanceSSE (const Eigen::VectorXf &model_coefficients,
+                              const double threshold,
+                              std::size_t i = 0) const;
+#endif
+
+#if defined (__AVX__) && defined (__AVX2__)
+      /** This implementation uses AVX and AVX2 instructions. It is not intended for normal use.
+        * See countWithinDistance which automatically uses the fastest implementation.
+        */
+      std::size_t
+      countWithinDistanceAVX (const Eigen::VectorXf &model_coefficients,
+                              const double threshold,
+                              std::size_t i = 0) const;
+#endif
+
     private:
       /** \brief Functor for the optimization function */
       struct OptimizationFunctor : pcl::Functor<float>
@@ -247,6 +282,14 @@ namespace pcl
         const pcl::SampleConsensusModelCircle2D<PointT> *model_;
         const Indices &indices_;
       };
+
+#ifdef __AVX__
+      inline __m256 sqr_dist8 (const std::size_t i, const __m256 a_vec, const __m256 b_vec) const;
+#endif
+
+#ifdef __SSE__
+      inline __m128 sqr_dist4 (const std::size_t i, const __m128 a_vec, const __m128 b_vec) const;
+#endif
   };
 }
 
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 f95e8443..9fe7dcbf 100644
--- a/sample_consensus/include/pcl/sample_consensus/sac_model_cone.h
+++ b/sample_consensus/include/pcl/sample_consensus/sac_model_cone.h
@@ -40,9 +40,7 @@
 
 #include <pcl/sample_consensus/sac_model.h>
 #include <pcl/sample_consensus/model_types.h>
-#include <pcl/common/common.h>
 #include <pcl/common/distances.h>
-#include <climits>
 
 namespace pcl
 {
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 b01dfbbf..889f436e 100644
--- a/sample_consensus/include/pcl/sample_consensus/sac_model_cylinder.h
+++ b/sample_consensus/include/pcl/sample_consensus/sac_model_cylinder.h
@@ -42,7 +42,6 @@
 
 #include <pcl/sample_consensus/sac_model.h>
 #include <pcl/sample_consensus/model_types.h>
-#include <pcl/common/common.h>
 #include <pcl/common/distances.h>
 
 namespace pcl
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 d948276a..385bdc41 100644
--- a/sample_consensus/include/pcl/sample_consensus/sac_model_line.h
+++ b/sample_consensus/include/pcl/sample_consensus/sac_model_line.h
@@ -42,7 +42,6 @@
 
 #include <pcl/sample_consensus/sac_model.h>
 #include <pcl/sample_consensus/model_types.h>
-#include <pcl/common/eigen.h>
 
 namespace pcl
 {
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 670275bf..b8164cbb 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
@@ -164,6 +164,34 @@ namespace pcl
     protected:
       using SampleConsensusModel<PointT>::sample_size_;
       using SampleConsensusModel<PointT>::model_size_;
+
+      /** This implementation uses no SIMD instructions. It is not intended for normal use.
+        * See countWithinDistance which automatically uses the fastest implementation.
+        */
+      std::size_t
+      countWithinDistanceStandard (const Eigen::VectorXf &model_coefficients,
+                                   const double threshold,
+                                   std::size_t i = 0) const;
+
+#if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
+      /** This implementation uses SSE, SSE2, and SSE4.1 instructions. It is not intended for normal use.
+        * See countWithinDistance which automatically uses the fastest implementation.
+        */
+      std::size_t
+      countWithinDistanceSSE (const Eigen::VectorXf &model_coefficients,
+                              const double threshold,
+                              std::size_t i = 0) const;
+#endif
+
+#if defined (__AVX__) && defined (__AVX2__)
+      /** This implementation uses AVX and AVX2 instructions. It is not intended for normal use.
+        * See countWithinDistance which automatically uses the fastest implementation.
+        */
+      std::size_t
+      countWithinDistanceAVX (const Eigen::VectorXf &model_coefficients,
+                              const double threshold,
+                              std::size_t i = 0) const;
+#endif
   };
 }
 
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 7b149b82..2bb45c0b 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
@@ -43,7 +43,6 @@
 #include <pcl/sample_consensus/sac_model.h>
 #include <pcl/sample_consensus/sac_model_sphere.h>
 #include <pcl/sample_consensus/model_types.h>
-#include <pcl/common/common.h>
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
 
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 8d17ad20..a3c82db8 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
@@ -41,7 +41,6 @@
 #pragma once
 
 #include <pcl/sample_consensus/sac_model_plane.h>
-#include <pcl/common/common.h>
 
 namespace pcl
 {
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 ebf061b9..2f649460 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
@@ -41,7 +41,6 @@
 #pragma once
 
 #include <pcl/sample_consensus/sac_model_plane.h>
-#include <pcl/common/common.h>
 
 namespace pcl
 {
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 6bcfedd2..5a71c5d4 100644
--- a/sample_consensus/include/pcl/sample_consensus/sac_model_plane.h
+++ b/sample_consensus/include/pcl/sample_consensus/sac_model_plane.h
@@ -40,6 +40,13 @@
 
 #pragma once
 
+#ifdef __SSE__
+#include <xmmintrin.h> // for __m128
+#endif // ifdef __SSE__
+#ifdef __AVX__
+#include <immintrin.h> // for __m256
+#endif // ifdef __AVX__
+
 #include <pcl/sample_consensus/sac_model.h>
 #include <pcl/sample_consensus/model_types.h>
 
@@ -257,6 +264,42 @@ namespace pcl
       using SampleConsensusModel<PointT>::sample_size_;
       using SampleConsensusModel<PointT>::model_size_;
 
+      /** This implementation uses no SIMD instructions. It is not intended for normal use.
+        * See countWithinDistance which automatically uses the fastest implementation.
+        */
+      std::size_t
+      countWithinDistanceStandard (const Eigen::VectorXf &model_coefficients,
+                                   const double threshold,
+                                   std::size_t i = 0) const;
+
+#if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
+      /** This implementation uses SSE, SSE2, and SSE4.1 instructions. It is not intended for normal use.
+        * See countWithinDistance which automatically uses the fastest implementation.
+        */
+      std::size_t
+      countWithinDistanceSSE (const Eigen::VectorXf &model_coefficients,
+                              const double threshold,
+                              std::size_t i = 0) const;
+#endif
+
+#if defined (__AVX__) && defined (__AVX2__)
+      /** This implementation uses AVX and AVX2 instructions. It is not intended for normal use.
+        * See countWithinDistance which automatically uses the fastest implementation.
+        */
+      std::size_t
+      countWithinDistanceAVX (const Eigen::VectorXf &model_coefficients,
+                              const double threshold,
+                              std::size_t i = 0) const;
+#endif
+
+#ifdef __AVX__
+      inline __m256 dist8 (const std::size_t i, const __m256 &a_vec, const __m256 &b_vec, const __m256 &c_vec, const __m256 &d_vec, const __m256 &abs_help) const;
+#endif
+
+#ifdef __SSE__
+      inline __m128 dist4 (const std::size_t i, const __m128 &a_vec, const __m128 &b_vec, const __m128 &c_vec, const __m128 &d_vec, const __m128 &abs_help) const;
+#endif
+
     private:
       /** \brief Check if a sample of indices results in a good sample of points
         * indices.
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 f649a726..43c15c91 100644
--- a/sample_consensus/include/pcl/sample_consensus/sac_model_registration.h
+++ b/sample_consensus/include/pcl/sample_consensus/sac_model_registration.h
@@ -43,12 +43,12 @@
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
 #include <pcl/pcl_base.h>
-#include <pcl/sample_consensus/eigen.h>
 #include <pcl/sample_consensus/sac_model.h>
 #include <pcl/sample_consensus/model_types.h>
 #include <pcl/common/eigen.h>
 #include <pcl/common/centroid.h>
 #include <map>
+#include <numeric> // for std::iota
 
 namespace pcl
 {
@@ -130,13 +130,10 @@ namespace pcl
       setInputTarget (const PointCloudConstPtr &target)
       {
         target_ = target;
-        indices_tgt_.reset (new Indices);
         // Cache the size and fill the target indices
-        int target_size = static_cast<int> (target->size ());
-        indices_tgt_->resize (target_size);
-
-        for (int i = 0; i < target_size; ++i)
-          (*indices_tgt_)[i] = i;
+        const index_t target_size = static_cast<index_t> (target->size ());
+        indices_tgt_.reset (new Indices (target_size));
+        std::iota (indices_tgt_->begin (), indices_tgt_->end (), 0);
         computeOriginalIndexMapping ();
       }
 
@@ -307,10 +304,30 @@ namespace pcl
       void
       computeOriginalIndexMapping ()
       {
-        if (!indices_tgt_ || !indices_ || indices_->empty () || indices_->size () != indices_tgt_->size ())
+        if (!indices_tgt_)
+        {
+          PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Cannot compute mapping: indices_tgt_ is null.\n");
+          return;
+        }
+        if (!indices_)
+        {
+          PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Cannot compute mapping: indices_ is null.\n");
+          return;
+        }
+        if (indices_->empty ())
+        {
+          PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Cannot compute mapping: indices_ is empty.\n");
+          return;
+        }
+        if (indices_->size () != indices_tgt_->size ())
+        {
+          PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Cannot compute mapping: indices_ and indices_tgt_ are not the same size (%zu vs %zu).\n",
+                     indices_->size (), indices_tgt_->size ());
           return;
+        }
         for (std::size_t i = 0; i < indices_->size (); ++i)
           correspondences_[(*indices_)[i]] = (*indices_tgt_)[i];
+        PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Successfully computed mapping.\n");
       }
 
       /** \brief A boost shared pointer to the target point cloud data array. */
@@ -320,7 +337,7 @@ namespace pcl
       IndicesPtr indices_tgt_;
 
       /** \brief Given the index in the original point cloud, give the matching original index in the target cloud */
-      std::map<int, int> correspondences_;
+      std::map<index_t, index_t> correspondences_;
 
       /** \brief Internal distance threshold used for the sample selection step. */
       double sample_dist_thresh_;
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 0906ca29..6840e0be 100644
--- a/sample_consensus/include/pcl/sample_consensus/sac_model_sphere.h
+++ b/sample_consensus/include/pcl/sample_consensus/sac_model_sphere.h
@@ -40,6 +40,13 @@
 
 #pragma once
 
+#ifdef __SSE__
+#include <xmmintrin.h> // for __m128
+#endif // ifdef __SSE__
+#ifdef __AVX__
+#include <immintrin.h> // for __m256
+#endif // ifdef __AVX__
+
 #include <pcl/sample_consensus/sac_model.h>
 #include <pcl/sample_consensus/model_types.h>
 
@@ -227,6 +234,34 @@ namespace pcl
       bool
       isSampleGood(const Indices &samples) const override;
 
+      /** This implementation uses no SIMD instructions. It is not intended for normal use.
+        * See countWithinDistance which automatically uses the fastest implementation.
+        */
+      std::size_t
+      countWithinDistanceStandard (const Eigen::VectorXf &model_coefficients,
+                                   const double threshold,
+                                   std::size_t i = 0) const;
+
+#if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
+      /** This implementation uses SSE, SSE2, and SSE4.1 instructions. It is not intended for normal use.
+        * See countWithinDistance which automatically uses the fastest implementation.
+        */
+      std::size_t
+      countWithinDistanceSSE (const Eigen::VectorXf &model_coefficients,
+                              const double threshold,
+                              std::size_t i = 0) const;
+#endif
+
+#if defined (__AVX__) && defined (__AVX2__)
+      /** This implementation uses AVX and AVX2 instructions. It is not intended for normal use.
+        * See countWithinDistance which automatically uses the fastest implementation.
+        */
+      std::size_t
+      countWithinDistanceAVX (const Eigen::VectorXf &model_coefficients,
+                              const double threshold,
+                              std::size_t i = 0) const;
+#endif
+
     private:
       struct OptimizationFunctor : pcl::Functor<float>
       {
@@ -261,6 +296,14 @@ namespace pcl
         const pcl::SampleConsensusModelSphere<PointT> *model_;
         const Indices &indices_;
       };
+
+#ifdef __AVX__
+      inline __m256 sqr_dist8 (const std::size_t i, const __m256 a_vec, const __m256 b_vec, const __m256 c_vec) const;
+#endif
+
+#ifdef __SSE__
+      inline __m128 sqr_dist4 (const std::size_t i, const __m128 a_vec, const __m128 b_vec, const __m128 c_vec) const;
+#endif
    };
 }
 
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 e3821a25..9f7f4f3b 100644
--- a/sample_consensus/include/pcl/sample_consensus/sac_model_stick.h
+++ b/sample_consensus/include/pcl/sample_consensus/sac_model_stick.h
@@ -42,7 +42,6 @@
 
 #include <pcl/sample_consensus/sac_model.h>
 #include <pcl/sample_consensus/model_types.h>
-#include <pcl/common/eigen.h>
 
 namespace pcl
 {
diff --git a/sample_consensus/sample_consensus.doxy b/sample_consensus/sample_consensus.doxy
index 1cbf55d8..a73462a9 100644
--- a/sample_consensus/sample_consensus.doxy
+++ b/sample_consensus/sample_consensus.doxy
@@ -4,7 +4,7 @@
   \section secSampleConsensusPresentation Overview
 
   The <b>pcl_sample_consensus</b> library holds SAmple Consensus (SAC) methods like 
-  <a href="http://en.wikipedia.org/wiki/RANSAC">RANSAC</a> and models like planes and cylinders.  These can
+  <a href="http://en.wikipedia.org/wiki/RANSAC">RANSAC</a> and models like planes and cylinders.  These can be
   combined freely in order to detect specific models and their parameters in point clouds.
 
   Some of the models implemented in this library include: lines, planes, cylinders, and spheres.  Plane fitting
@@ -37,11 +37,11 @@
   The following list describes the robust sample consensus estimators implemented:
   <ul>
     <li><a href="http://en.wikipedia.org/wiki/RANSAC">SAC_RANSAC</a> - RANdom SAmple Consensus</li>
-    <li><a href="http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node25.html">SAC_LMEDS</a> - Least Median of Squares</li> 
-    <li><a href="http://www.robots.ox.ac.uk/~vgg/publications-new/Public/2000/Torr00/torr00.pdf">SAC_MSAC</a> - M-Estimator SAmple Consensus</li>
-    <li><a href="http://cmp.felk.cvut.cz/~matas/papers/presentations/rransac-cvww02_pres.pdf">SAC_RRANSAC</a> - Randomized RANSAC</li>
-    <li><a href="http://cmp.felk.cvut.cz/~matas/papers/presentations/rransac-cvww02_pres.pdf">SAC_RMSAC</a> - Randomized MSAC</li>
-    <li><a href="http://www.robots.ox.ac.uk/~vgg/publications-new/Public/2000/Torr00/torr00.pdf">SAC_MLESAC</a> - Maximum LikeLihood Estimation SAmple Consensus</li>
+    <li><a href="https://www-sop.inria.fr/odyssee/software/old_robotvis/Tutorial-Estim/node25.html">SAC_LMEDS</a> - Least Median of Squares</li>
+    <li><a href="https://www.robots.ox.ac.uk/~vgg/publications/2000/Torr00/torr00.pdf">SAC_MSAC</a> - M-Estimator SAmple Consensus</li>
+    <li><a href="https://web.archive.org/web/20170811194151/http://www.bmva.org/bmvc/2002/papers/50/full_50.pdf">SAC_RRANSAC</a> - Randomized RANSAC</li>
+    <li><a href="https://web.archive.org/web/20170811194151/http://www.bmva.org/bmvc/2002/papers/50/full_50.pdf">SAC_RMSAC</a> - Randomized MSAC</li>
+    <li><a href="https://www.robots.ox.ac.uk/~vgg/publications/2000/Torr00/torr00.pdf">SAC_MLESAC</a> - Maximum LikeLihood Estimation SAmple Consensus</li>
     <li><a href="http://cmp.felk.cvut.cz/~matas/papers/chum-prosac-cvpr05.pdf">SAC_PROSAC</a> - PROgressive SAmple Consensus</li>
   </ul>
 
diff --git a/search/include/pcl/search/impl/flann_search.hpp b/search/include/pcl/search/impl/flann_search.hpp
index 1b38f6cf..f54098ff 100644
--- a/search/include/pcl/search/impl/flann_search.hpp
+++ b/search/include/pcl/search/impl/flann_search.hpp
@@ -45,6 +45,9 @@
 #include <flann/algorithms/kmeans_index.h>
 
 #include <pcl/search/flann_search.h>
+#include <pcl/kdtree/kdtree_flann.h> // for radius_search, knn_search
+// @TODO: remove once constexpr makes it easy to have the function in the header only
+#include <pcl/kdtree/impl/kdtree_flann.hpp>
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT, typename FlannDistance>
@@ -123,9 +126,8 @@ pcl::search::FlannSearch<PointT, FlannDistance>::nearestKSearch (const PointT &p
     indices.resize (k,-1);
   if (dists.size() != static_cast<unsigned int> (k))
     dists.resize (k);
-  flann::Matrix<index_t> i (&indices[0],1,k);
   flann::Matrix<float> d (&dists[0],1,k);
-  int result = index_->knnSearch (m,i,d,k, p);
+  int result = knn_search(*index_, m, indices, d, k, p);
 
   delete [] data;
 
@@ -182,7 +184,7 @@ pcl::search::FlannSearch<PointT, FlannDistance>::nearestKSearch (
     p.sorted = sorted_results_;
     p.eps = eps_;
     p.checks = checks_;
-    index_->knnSearch (m,k_indices,k_sqr_distances,k, p);
+    knn_search(*index_, m, k_indices, k_sqr_distances, k, p);
 
     delete [] data;
   }
@@ -211,7 +213,7 @@ pcl::search::FlannSearch<PointT, FlannDistance>::nearestKSearch (
     p.sorted = sorted_results_;
     p.eps = eps_;
     p.checks = checks_;
-    index_->knnSearch (m,k_indices,k_sqr_distances,k, p);
+    knn_search(*index_, m, k_indices, k_sqr_distances, k, p);
 
     delete[] data;
   }
@@ -253,7 +255,7 @@ pcl::search::FlannSearch<PointT, FlannDistance>::radiusSearch (const PointT& poi
   p.checks = checks_;
   std::vector<Indices> i (1);
   std::vector<std::vector<float> > d (1);
-  int result = index_->radiusSearch (m,i,d,static_cast<float> (radius * radius), p);
+  int result = radius_search(*index_, m, i, d, static_cast<float>(radius * radius), p);
 
   delete [] data;
   indices = i [0];
@@ -310,7 +312,8 @@ pcl::search::FlannSearch<PointT, FlannDistance>::radiusSearch (
     p.checks = checks_;
     // here: max_nn==0: take all neighbors. flann: max_nn==0: return no neighbors, only count them. max_nn==-1: return all neighbors
     p.max_neighbors = max_nn != 0 ? max_nn : -1;
-    index_->radiusSearch (m,k_indices,k_sqr_distances,static_cast<float> (radius * radius), p);
+    radius_search(
+        *index_, m, k_indices, k_sqr_distances, static_cast<float>(radius * radius), p);
 
     delete [] data;
   }
@@ -341,7 +344,8 @@ pcl::search::FlannSearch<PointT, FlannDistance>::radiusSearch (
     p.checks = checks_;
     // here: max_nn==0: take all neighbors. flann: max_nn==0: return no neighbors, only count them. max_nn==-1: return all neighbors
     p.max_neighbors = max_nn != 0 ? max_nn : -1;
-    index_->radiusSearch (m, k_indices, k_sqr_distances, static_cast<float> (radius * radius), p);
+    radius_search(
+        *index_, m, k_indices, k_sqr_distances, static_cast<float>(radius * radius), p);
 
     delete[] data;
   }
diff --git a/search/include/pcl/search/impl/organized.hpp b/search/include/pcl/search/impl/organized.hpp
index 5205ee07..45b493a3 100644
--- a/search/include/pcl/search/impl/organized.hpp
+++ b/search/include/pcl/search/impl/organized.hpp
@@ -40,9 +40,8 @@
 #pragma once
 
 #include <pcl/search/organized.h>
-#include <pcl/common/eigen.h>
 #include <pcl/common/point_tests.h> // for pcl::isFinite
-#include <pcl/common/time.h>
+#include <pcl/common/projection_matrix.h> // for getCameraMatrixFromProjectionMatrix, ...
 #include <Eigen/Eigenvalues>
 
 //////////////////////////////////////////////////////////////////////////////////////////////
@@ -60,14 +59,12 @@ pcl::search::OrganizedNeighbor<PointT>::radiusSearch (const               PointT
   unsigned left, right, top, bottom;
   //unsigned x, y, idx;
   float squared_distance;
-  double squared_radius;
+  const float squared_radius = radius * radius;
 
   k_indices.clear ();
   k_sqr_distances.clear ();
 
-  squared_radius = radius * radius;
-
-  this->getProjectedRadiusSearchBox (query, static_cast<float> (squared_radius), left, right, top, bottom);
+  this->getProjectedRadiusSearchBox (query, squared_radius, left, right, top, bottom);
 
   // iterate over search box
   if (max_nn == 0 || max_nn >= static_cast<unsigned int> (input_->size ()))
@@ -142,9 +139,8 @@ pcl::search::OrganizedNeighbor<PointT>::nearestKSearch (const PointT &query,
   unsigned top = 0;
   unsigned bottom = input_->height - 1;
 
-  std::priority_queue <Entry> results;
-  //std::vector<Entry> k_results;
-  //k_results.reserve (k);
+  std::vector <Entry> results; // sorted from smallest to largest distance
+  results.reserve (k);
   // add point laying on the projection of the query point.
   if (xBegin >= 0 && 
       xBegin < static_cast<int> (input_->width) && 
@@ -242,7 +238,7 @@ pcl::search::OrganizedNeighbor<PointT>::nearestKSearch (const PointT &query,
       }
       // stop here means that the k-nearest neighbor changed -> recalculate bounding box of ellipse.
       if (stop)
-        getProjectedRadiusSearchBox (query, results.top ().distance, left, right, top, bottom);
+        getProjectedRadiusSearchBox (query, results.back ().distance, left, right, top, bottom);
       
     }
     // now we use it as stop flag -> if bounding box is completely within the already examined search box were done!
@@ -254,18 +250,18 @@ pcl::search::OrganizedNeighbor<PointT>::nearestKSearch (const PointT &query,
   } while (!stop);
 
   
-  k_indices.resize (results.size ());
-  k_sqr_distances.resize (results.size ());
-  std::size_t idx = results.size () - 1;
-  while (!results.empty ())
+  const auto results_size = results.size ();
+  k_indices.resize (results_size);
+  k_sqr_distances.resize (results_size);
+  std::size_t idx = 0;
+  for(const auto& result : results)
   {
-    k_indices [idx] = results.top ().index;
-    k_sqr_distances [idx] = results.top ().distance;
-    results.pop ();
-    --idx;
+    k_indices [idx] = result.index;
+    k_sqr_distances [idx] = result.distance;
+    ++idx;
   }
   
-  return (static_cast<int> (k_indices.size ()));
+  return (static_cast<int> (results_size));
 }
 
 ////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/search/include/pcl/search/octree.h b/search/include/pcl/search/octree.h
index 0df81f3d..d98e605e 100644
--- a/search/include/pcl/search/octree.h
+++ b/search/include/pcl/search/octree.h
@@ -275,7 +275,17 @@ namespace pcl
         {
           return (tree_->approxNearestSearch (query_index, result_index, sqr_distance));
         }
-
+        /** \brief Search for points within rectangular search area
+	        * \param[in] min_pt lower corner of search area
+	        * \param[in] max_pt upper corner of search area
+	        * \param[out] k_indices the resultant point indices
+	        * \return number of points found within search area
+	        */
+        inline uindex_t
+        boxSearch(const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, Indices &k_indices) const
+        {
+          return (tree_->boxSearch(min_pt, max_pt, k_indices));
+        }
     };
   }
 }
diff --git a/search/include/pcl/search/organized.h b/search/include/pcl/search/organized.h
index a6d40bbf..c1101327 100644
--- a/search/include/pcl/search/organized.h
+++ b/search/include/pcl/search/organized.h
@@ -42,14 +42,11 @@
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
 #include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
 #include <pcl/search/search.h>
 #include <pcl/common/eigen.h>
 
 #include <algorithm>
-#include <queue>
 #include <vector>
-#include <pcl/common/projection_matrix.h>
 
 namespace pcl
 {
@@ -212,7 +209,7 @@ namespace pcl
           * \return whether the top element changed or not.
           */
         inline bool 
-        testPoint (const PointT& query, unsigned k, std::priority_queue<Entry>& queue, index_t index) const
+        testPoint (const PointT& query, unsigned k, std::vector<Entry>& queue, index_t index) const
         {
           const PointT& point = input_->points [index];
           if (mask_ [index] && std::isfinite (point.x))
@@ -222,15 +219,20 @@ namespace pcl
             float dist_y = point.y - query.y;
             float dist_z = point.z - query.z;
             float squared_distance = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z;
-            if (queue.size () < k)
+            const auto queue_size = queue.size ();
+            const auto insert_into_queue = [&]{ queue.emplace (
+                                                std::upper_bound (queue.begin(), queue.end(), squared_distance,
+                                                [](float dist, const Entry& ent){ return dist<ent.distance; }),
+                                                               index, squared_distance); };
+            if (queue_size < k)
             {
-              queue.push (Entry (index, squared_distance));
-              return queue.size () == k;
+              insert_into_queue ();
+              return (queue_size + 1) == k;
             }
-            if (queue.top ().distance > squared_distance)
+            if (queue.back ().distance > squared_distance)
             {
-              queue.pop ();
-              queue.push (Entry (index, squared_distance));
+              queue.pop_back ();
+              insert_into_queue ();
               return true; // top element has changed!
             }
           }
diff --git a/segmentation/include/pcl/segmentation/approximate_progressive_morphological_filter.h b/segmentation/include/pcl/segmentation/approximate_progressive_morphological_filter.h
index 401175ef..f98b3a2b 100644
--- a/segmentation/include/pcl/segmentation/approximate_progressive_morphological_filter.h
+++ b/segmentation/include/pcl/segmentation/approximate_progressive_morphological_filter.h
@@ -139,7 +139,7 @@ namespace pcl
         * \param[out] ground indices of points determined to be ground returns.
         */
       virtual void
-      extract (std::vector<int>& ground);
+      extract (Indices& ground);
 
     protected:
 
diff --git a/segmentation/include/pcl/segmentation/boost.h b/segmentation/include/pcl/segmentation/boost.h
index 40c3aac5..6ef50c38 100644
--- a/segmentation/include/pcl/segmentation/boost.h
+++ b/segmentation/include/pcl/segmentation/boost.h
@@ -39,7 +39,7 @@
  */
 
 #pragma once
-
+PCL_DEPRECATED_HEADER(1, 15, "Please include the needed boost headers directly.")
 #ifdef __GNUC__
 #pragma GCC system_header
 #endif
diff --git a/segmentation/include/pcl/segmentation/conditional_euclidean_clustering.h b/segmentation/include/pcl/segmentation/conditional_euclidean_clustering.h
index 55d96b2c..dd715786 100644
--- a/segmentation/include/pcl/segmentation/conditional_euclidean_clustering.h
+++ b/segmentation/include/pcl/segmentation/conditional_euclidean_clustering.h
@@ -40,7 +40,8 @@
 #include <pcl/memory.h>
 #include <pcl/pcl_base.h>
 #include <pcl/pcl_macros.h>
-#include <pcl/search/pcl_search.h>
+#include <pcl/console/print.h> // for PCL_WARN
+#include <pcl/search/search.h> // for Search
 
 #include <functional>
 
diff --git a/segmentation/include/pcl/segmentation/cpc_segmentation.h b/segmentation/include/pcl/segmentation/cpc_segmentation.h
index de38bd26..cb5236bd 100644
--- a/segmentation/include/pcl/segmentation/cpc_segmentation.h
+++ b/segmentation/include/pcl/segmentation/cpc_segmentation.h
@@ -47,7 +47,6 @@
 #include <pcl/segmentation/lccp_segmentation.h>
 #include <pcl/sample_consensus/sac.h>
 
-#include <pcl/sample_consensus/sac_model_plane.h>
 #include <pcl/segmentation/extract_clusters.h>
 
 #define PCL_INSTANTIATE_CPCSegmentation(T) template class PCL_EXPORTS pcl::CPCSegmentation<T>;
@@ -249,8 +248,8 @@ namespace pcl
             // Maximum number of trials before we give up.
             max_iterations_ = 10000;
             use_directed_weights_ = false;
-            model_pt_indices_.reset (new std::vector<int>);
-            full_cloud_pt_indices_.reset (new std::vector<int> (* (sac_model_->getIndices ())));
+            model_pt_indices_.reset (new Indices);
+            full_cloud_pt_indices_.reset (new Indices (* (sac_model_->getIndices ())));
             point_cloud_ptr_ = sac_model_->getInputCloud ();
           }
 
diff --git a/segmentation/include/pcl/segmentation/crf_segmentation.h b/segmentation/include/pcl/segmentation/crf_segmentation.h
index c9701840..b99f72d8 100644
--- a/segmentation/include/pcl/segmentation/crf_segmentation.h
+++ b/segmentation/include/pcl/segmentation/crf_segmentation.h
@@ -46,7 +46,6 @@
 
 #include <pcl/ml/densecrf.h>
 #include <pcl/filters/voxel_grid.h>
-#include <pcl/filters/voxel_grid_label.h>
 
 //#include <pcl/ml/densecrfORI.h>
 
diff --git a/segmentation/include/pcl/segmentation/edge_aware_plane_comparator.h b/segmentation/include/pcl/segmentation/edge_aware_plane_comparator.h
index c2a4a5eb..848dd6e8 100644
--- a/segmentation/include/pcl/segmentation/edge_aware_plane_comparator.h
+++ b/segmentation/include/pcl/segmentation/edge_aware_plane_comparator.h
@@ -39,7 +39,6 @@
 
 #pragma once
 
-#include <pcl/segmentation/boost.h>
 #include <pcl/segmentation/plane_coefficient_comparator.h>
 
 namespace pcl
diff --git a/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h b/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h
index e85373a9..300fb20f 100644
--- a/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h
+++ b/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h
@@ -39,234 +39,166 @@
 
 #pragma once
 
+#include <set> // for std::set
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
 #include <pcl/point_types.h>
-#include <pcl/segmentation/boost.h>
 #include <pcl/segmentation/comparator.h>
 
 
 namespace pcl
 {
-  namespace experimental
+  /** \brief EuclideanClusterComparator is a comparator used for finding clusters based on euclidian distance.
+  *
+  * \author Alex Trevor
+  */
+  template<typename PointT, typename PointLT = pcl::Label>
+  class EuclideanClusterComparator : public ::pcl::Comparator<PointT>
   {
-    template<typename PointT, typename PointLT = pcl::Label>
-    class EuclideanClusterComparator : public ::pcl::Comparator<PointT>
-    {
-      protected:
+    protected:
 
-        using pcl::Comparator<PointT>::input_;
+      using pcl::Comparator<PointT>::input_;
 
-      public:
-        using typename Comparator<PointT>::PointCloud;
-        using typename Comparator<PointT>::PointCloudConstPtr;
+    public:
+      using typename Comparator<PointT>::PointCloud;
+      using typename Comparator<PointT>::PointCloudConstPtr;
 
-        using PointCloudL = pcl::PointCloud<PointLT>;
-        using PointCloudLPtr = typename PointCloudL::Ptr;
-        using PointCloudLConstPtr = typename PointCloudL::ConstPtr;
+      using PointCloudL = pcl::PointCloud<PointLT>;
+      using PointCloudLPtr = typename PointCloudL::Ptr;
+      using PointCloudLConstPtr = typename PointCloudL::ConstPtr;
 
-        using Ptr = shared_ptr<EuclideanClusterComparator<PointT, PointLT> >;
-        using ConstPtr = shared_ptr<const EuclideanClusterComparator<PointT, PointLT> >;
+      using Ptr = shared_ptr<EuclideanClusterComparator<PointT, PointLT> >;
+      using ConstPtr = shared_ptr<const EuclideanClusterComparator<PointT, PointLT> >;
 
-        using ExcludeLabelSet = std::set<std::uint32_t>;
-        using ExcludeLabelSetPtr = shared_ptr<ExcludeLabelSet>;
-        using ExcludeLabelSetConstPtr = shared_ptr<const ExcludeLabelSet>;
+      using ExcludeLabelSet = std::set<std::uint32_t>;
+      using ExcludeLabelSetPtr = shared_ptr<ExcludeLabelSet>;
+      using ExcludeLabelSetConstPtr = shared_ptr<const ExcludeLabelSet>;
 
-        /** \brief Default constructor for EuclideanClusterComparator. */
-        EuclideanClusterComparator ()
-          : distance_threshold_ (0.005f)
-          , depth_dependent_ ()
-        {}
+      /** \brief Default constructor for EuclideanClusterComparator. */
+      EuclideanClusterComparator() = default;
 
-        void
-        setInputCloud (const PointCloudConstPtr& cloud) override
-        {
-          input_ = cloud;
-          Eigen::Matrix3f rot = input_->sensor_orientation_.toRotationMatrix ();
-          z_axis_ = rot.col (2);
-        }
+      void
+      setInputCloud (const PointCloudConstPtr& cloud) override
+      {
+        input_ = cloud;
+        Eigen::Matrix3f rot = input_->sensor_orientation_.toRotationMatrix ();
+        z_axis_ = rot.col (2);
+      }
 
-        /** \brief Set the tolerance in meters for difference in perpendicular distance (d component of plane equation) to the plane between neighboring points, to be considered part of the same plane.
-          * \param[in] distance_threshold the tolerance in meters
-          * \param depth_dependent
-          */
-        inline void
-        setDistanceThreshold (float distance_threshold, bool depth_dependent)
-        {
-          distance_threshold_ = distance_threshold;
-          depth_dependent_ = depth_dependent;
-        }
+      /** \brief Set the tolerance in meters for difference in perpendicular distance (d component of plane equation) to the plane between neighboring points, to be considered part of the same plane.
+        * \param[in] distance_threshold the tolerance in meters
+        * \param depth_dependent
+        */
+      inline void
+      setDistanceThreshold (float distance_threshold, bool depth_dependent)
+      {
+        distance_threshold_ = distance_threshold;
+        depth_dependent_ = depth_dependent;
+      }
 
-        /** \brief Get the distance threshold in meters (d component of plane equation) between neighboring points, to be considered part of the same plane. */
-        inline float
-        getDistanceThreshold () const
-        {
-          return (distance_threshold_);
-        }
+      /** \brief Get the distance threshold in meters (d component of plane equation) between neighboring points, to be considered part of the same plane. */
+      inline float
+      getDistanceThreshold () const
+      {
+        return distance_threshold_;
+      }
 
-        /** \brief Set label cloud
-          * \param[in] labels The label cloud
-          */
-        void
-        setLabels (const PointCloudLPtr& labels)
-        {
-          labels_ = labels;
-        }
+      /** \brief Get if depth dependent */
+      inline bool
+      getDepthDependent() const
+      {
+        return depth_dependent_;
+      }
 
-        const ExcludeLabelSetConstPtr&
-        getExcludeLabels () const
-        {
-          return exclude_labels_;
-        }
+      /** \brief Set label cloud
+        * \param[in] labels The label cloud
+        */
+      void
+      setLabels (const PointCloudLPtr& labels)
+      {
+        labels_ = labels;
+      }
 
-        /** \brief Set labels in the label cloud to exclude.
-          * \param exclude_labels a vector of bools corresponding to whether or not a given label should be considered
-          */
-        void
-        setExcludeLabels (const ExcludeLabelSetConstPtr &exclude_labels)
-        {
-          exclude_labels_ = exclude_labels;
-        }
+      /** \brief Get labels */
+      const PointCloudLPtr&
+      getLabels() const
+      {
+        return labels_;
+      }
 
-        /** \brief Compare points at two indices by their euclidean distance
-          * \param idx1 The first index for the comparison
-          * \param idx2 The second index for the comparison
-          */
-        bool
-        compare (int idx1, int idx2) const override
-        {
-          if (labels_ && exclude_labels_)
-          {
-            assert (labels_->size () == input_->size ());
-            const std::uint32_t &label1 = (*labels_)[idx1].label;
-            const std::uint32_t &label2 = (*labels_)[idx2].label;
+      /** \brief Get exlude labels */
+      const ExcludeLabelSetConstPtr&
+      getExcludeLabels () const
+      {
+        return exclude_labels_;
+      }
 
-            const std::set<std::uint32_t>::const_iterator it1 = exclude_labels_->find (label1);
-            if (it1 == exclude_labels_->end ())
-              return false;
+      /** \brief Set labels in the label cloud to exclude.
+        * \param exclude_labels a vector of bools corresponding to whether or not a given label should be considered
+        */
+      void
+      setExcludeLabels (const ExcludeLabelSetConstPtr &exclude_labels)
+      {
+        exclude_labels_ = exclude_labels;
+      }
 
-            const std::set<std::uint32_t>::const_iterator it2 = exclude_labels_->find (label2);
-            if (it2 == exclude_labels_->end ())
-              return false;
-          }
+      /** \brief Compare points at two indices by their euclidean distance
+        * \param idx1 The first index for the comparison
+        * \param idx2 The second index for the comparison
+        */
+      bool
+      compare (int idx1, int idx2) const override
+      {
+        if (labels_ && exclude_labels_)
+        {
+          assert (labels_->size () == input_->size ());
+          const std::uint32_t &label1 = (*labels_)[idx1].label;
+          const std::uint32_t &label2 = (*labels_)[idx2].label;
 
-          float dist_threshold = distance_threshold_;
-          if (depth_dependent_)
-          {
-            Eigen::Vector3f vec = (*input_)[idx1].getVector3fMap ();
-            float z = vec.dot (z_axis_);
-            dist_threshold *= z * z;
-          }
+          const std::set<std::uint32_t>::const_iterator it1 = exclude_labels_->find (label1);
+          if (it1 == exclude_labels_->end ())
+            return false;
 
-          const float dist = ((*input_)[idx1].getVector3fMap ()
-                                - (*input_)[idx2].getVector3fMap ()).norm ();
-          return (dist < dist_threshold);
+          const std::set<std::uint32_t>::const_iterator it2 = exclude_labels_->find (label2);
+          if (it2 == exclude_labels_->end ())
+            return false;
         }
 
-      protected:
-
-
-        /** \brief Set of labels with similar size as the input point cloud,
-          * aggregating points into groups based on a similar label identifier.
-          *
-          * It needs to be set in conjunction with the \ref exclude_labels_
-          * member in order to provided a masking functionality.
-          */
-        PointCloudLPtr labels_;
-
-        /** \brief Specifies which labels should be excluded com being clustered.
-          *
-          * If a label is not specified, it's assumed by default that it's
-          * intended be excluded
-          */
-        ExcludeLabelSetConstPtr exclude_labels_;
-
-        float distance_threshold_;
-
-        bool depth_dependent_;
-
-        Eigen::Vector3f z_axis_;
-    };
-  } // namespace experimental
+        float dist_threshold = distance_threshold_;
+        if (depth_dependent_)
+        {
+          Eigen::Vector3f vec = (*input_)[idx1].getVector3fMap ();
+          float z = vec.dot (z_axis_);
+          dist_threshold *= z * z;
+        }
 
+        const float dist = ((*input_)[idx1].getVector3fMap ()
+                              - (*input_)[idx2].getVector3fMap ()).norm ();
+        return (dist < dist_threshold);
+      }
 
-  /** \brief EuclideanClusterComparator is a comparator used for finding clusters based on euclidian distance.
-    *
-    * \author Alex Trevor
-    */
-  template<typename PointT, typename PointNT, typename PointLT = deprecated::T>
-  class EuclideanClusterComparator : public experimental::EuclideanClusterComparator<PointT, PointLT>
-  {
     protected:
 
-      using experimental::EuclideanClusterComparator<PointT, PointLT>::exclude_labels_;
-
-    public:
-
-      using PointCloudN = pcl::PointCloud<PointNT>;
-      using PointCloudNPtr = typename PointCloudN::Ptr;
-      using PointCloudNConstPtr = typename PointCloudN::ConstPtr;
-
-      using Ptr = shared_ptr<EuclideanClusterComparator<PointT, PointNT, PointLT> >;
-      using ConstPtr = shared_ptr<const EuclideanClusterComparator<PointT, PointNT, PointLT> >;
-
-      using experimental::EuclideanClusterComparator<PointT, PointLT>::setExcludeLabels;
-
-      /** \brief Default constructor for EuclideanClusterComparator. */
-      PCL_DEPRECATED(1, 12, "remove PointNT from template parameters")
-      EuclideanClusterComparator ()
-        : normals_ ()
-        , angular_threshold_ (0.0f)
-      {}
-
-      /** \brief Provide a pointer to the input normals.
-       * \param[in] normals the input normal cloud
-       */
-      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(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
+      /** \brief Set of labels with similar size as the input point cloud,
+        * aggregating points into groups based on a similar label identifier.
+        *
+        * It needs to be set in conjunction with the \ref exclude_labels_
+        * member in order to provided a masking functionality.
         */
-      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)
-      {
-        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. */
-      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_) ); }
+      PointCloudLPtr labels_;
 
-      /** \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
+      /** \brief Specifies which labels should be excluded com being clustered.
+        *
+        * If a label is not specified, it's assumed by default that it's
+        * intended be excluded
         */
-      PCL_DEPRECATED(1, 12, "use setExcludeLabels(const ExcludeLabelSetConstPtr &) instead")
-      void
-      setExcludeLabels (const std::vector<bool>& exclude_labels)
-      {
-        exclude_labels_ = pcl::make_shared<std::set<std::uint32_t> > ();
-        for (std::size_t i = 0; i < exclude_labels.size (); ++i)
-          if (exclude_labels[i])
-            exclude_labels_->insert (i);
-      }
+      ExcludeLabelSetConstPtr exclude_labels_;
 
-    protected:
+      float distance_threshold_ = 0.005f;
 
-      PointCloudNConstPtr normals_;
+      bool depth_dependent_ = false;
 
-      float angular_threshold_;
+      Eigen::Vector3f z_axis_;
   };
-
-  template<typename PointT, typename PointLT>
-  class EuclideanClusterComparator<PointT, PointLT, deprecated::T>
-    : public experimental::EuclideanClusterComparator<PointT, PointLT> {};
 }
diff --git a/segmentation/include/pcl/segmentation/euclidean_plane_coefficient_comparator.h b/segmentation/include/pcl/segmentation/euclidean_plane_coefficient_comparator.h
index 29e486d2..b752ac0b 100644
--- a/segmentation/include/pcl/segmentation/euclidean_plane_coefficient_comparator.h
+++ b/segmentation/include/pcl/segmentation/euclidean_plane_coefficient_comparator.h
@@ -39,7 +39,6 @@
 
 #pragma once
 
-#include <pcl/segmentation/boost.h>
 #include <pcl/segmentation/plane_coefficient_comparator.h>
 
 namespace pcl
diff --git a/segmentation/include/pcl/segmentation/extract_clusters.h b/segmentation/include/pcl/segmentation/extract_clusters.h
index 233bcaf3..d7234ade 100644
--- a/segmentation/include/pcl/segmentation/extract_clusters.h
+++ b/segmentation/include/pcl/segmentation/extract_clusters.h
@@ -39,9 +39,11 @@
 
 #pragma once
 
+#include <pcl/console/print.h> // for PCL_ERROR
 #include <pcl/pcl_base.h>
 
-#include <pcl/search/pcl_search.h>
+#include <pcl/search/search.h> // for Search
+#include <pcl/search/kdtree.h> // for KdTree
 
 namespace pcl
 {
@@ -76,13 +78,16 @@ namespace pcl
     */
   template <typename PointT> void 
   extractEuclideanClusters (
-      const PointCloud<PointT> &cloud, const std::vector<int> &indices,
+      const PointCloud<PointT> &cloud, const Indices &indices,
       const typename search::Search<PointT>::Ptr &tree, float tolerance, std::vector<PointIndices> &clusters,
       unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());
 
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   /** \brief Decompose a region of space into clusters based on the euclidean distance between points, and the normal
-    * angular deviation
+    * angular deviation between points. Each point added to the cluster is origin to another radius search. Each point
+    * within radius range will be compared to the origin in respect to normal angle and euclidean distance. If both
+    * are under their respective threshold the point will be added to the cluster. Generally speaking the cluster 
+    * algorithm will not stop on smooth surfaces but on surfaces with sharp edges.
     * \param cloud the point cloud message
     * \param normals the point cloud message containing normal information
     * \param tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching
@@ -118,11 +123,12 @@ namespace pcl
                 static_cast<std::size_t>(normals.size()));
       return;
     }
+    const double cos_eps_angle = std::cos (eps_angle); // compute this once instead of acos many times (faster)
 
     // Create a bool vector of processed point indices, and initialize it to false
     std::vector<bool> processed (cloud.size (), false);
 
-    std::vector<int> nn_indices;
+    Indices nn_indices;
     std::vector<float> nn_distances;
     // Process all points in the indices vector
     for (std::size_t i = 0; i < cloud.size (); ++i)
@@ -130,9 +136,9 @@ namespace pcl
       if (processed[i])
         continue;
 
-      std::vector<unsigned int> seed_queue;
+      Indices seed_queue;
       int sq_idx = 0;
-      seed_queue.push_back (static_cast<int> (i));
+      seed_queue.push_back (static_cast<index_t> (i));
 
       processed[i] = true;
 
@@ -152,10 +158,10 @@ namespace pcl
 
           //processed[nn_indices[j]] = true;
           // [-1;1]
-          double dot_p = normals[i].normal[0] * normals[nn_indices[j]].normal[0] +
-                         normals[i].normal[1] * normals[nn_indices[j]].normal[1] +
-                         normals[i].normal[2] * normals[nn_indices[j]].normal[2];
-          if ( std::acos (std::abs (dot_p)) < eps_angle )
+          double dot_p = normals[seed_queue[sq_idx]].normal[0] * normals[nn_indices[j]].normal[0] +
+                         normals[seed_queue[sq_idx]].normal[1] * normals[nn_indices[j]].normal[1] +
+                         normals[seed_queue[sq_idx]].normal[2] * normals[nn_indices[j]].normal[2];
+          if ( std::abs (dot_p) > cos_eps_angle )
           {
             processed[nn_indices[j]] = true;
             seed_queue.push_back (nn_indices[j]);
@@ -180,13 +186,21 @@ namespace pcl
         r.header = cloud.header;
         clusters.push_back (r);   // We could avoid a copy by working directly in the vector
       }
+      else
+      {
+        PCL_DEBUG("[pcl::extractEuclideanClusters] This cluster has %zu points, which is not between %u and %u points, so it is not a final cluster\n",
+                  seed_queue.size (), min_pts_per_cluster, max_pts_per_cluster);
+      }
     }
   }
 
 
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   /** \brief Decompose a region of space into clusters based on the euclidean distance between points, and the normal
-    * angular deviation
+    * angular deviation between points. Each point added to the cluster is origin to another radius search. Each point
+    * within radius range will be compared to the origin in respect to normal angle and euclidean distance. If both
+    * are under their respective threshold the point will be added to the cluster. Generally speaking the cluster 
+    * algorithm will not stop on smooth surfaces but on surfaces with sharp edges.
     * \param cloud the point cloud message
     * \param normals the point cloud message containing normal information
     * \param indices a list of point indices to use from \a cloud
@@ -194,7 +208,7 @@ namespace pcl
     * \note the tree has to be created as a spatial locator on \a cloud
     * \param tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space
     * \param clusters the resultant clusters containing point indices (as PointIndices)
-    * \param eps_angle the maximum allowed difference between normals in degrees for cluster/region growing
+    * \param eps_angle the maximum allowed difference between normals in radians for cluster/region growing
     * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1)
     * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int)
     * \ingroup segmentation
@@ -202,7 +216,7 @@ namespace pcl
   template <typename PointT, typename Normal> 
   void extractEuclideanClusters (
       const PointCloud<PointT> &cloud, const PointCloud<Normal> &normals,
-      const std::vector<int> &indices, const typename KdTree<PointT>::Ptr &tree,
+      const Indices &indices, const typename KdTree<PointT>::Ptr &tree,
       float tolerance, std::vector<PointIndices> &clusters, double eps_angle,
       unsigned int min_pts_per_cluster = 1,
       unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
@@ -230,22 +244,23 @@ namespace pcl
                 static_cast<std::size_t>(normals.size()));
       return;
     }
+    const double cos_eps_angle = std::cos (eps_angle); // compute this once instead of acos many times (faster)
     // Create a bool vector of processed point indices, and initialize it to false
     std::vector<bool> processed (cloud.size (), false);
 
-    std::vector<int> nn_indices;
+    Indices nn_indices;
     std::vector<float> nn_distances;
     // Process all points in the indices vector
-    for (std::size_t i = 0; i < indices.size (); ++i)
+    for (const auto& point_idx : indices)
     {
-      if (processed[indices[i]])
+      if (processed[point_idx])
         continue;
 
-      std::vector<int> seed_queue;
+      Indices seed_queue;
       int sq_idx = 0;
-      seed_queue.push_back (indices[i]);
+      seed_queue.push_back (point_idx);
 
-      processed[indices[i]] = true;
+      processed[point_idx] = true;
 
       while (sq_idx < static_cast<int> (seed_queue.size ()))
       {
@@ -263,11 +278,10 @@ namespace pcl
 
           //processed[nn_indices[j]] = true;
           // [-1;1]
-          double dot_p =
-            normals[indices[i]].normal[0] * normals[indices[nn_indices[j]]].normal[0] +
-            normals[indices[i]].normal[1] * normals[indices[nn_indices[j]]].normal[1] +
-            normals[indices[i]].normal[2] * normals[indices[nn_indices[j]]].normal[2];
-          if ( std::acos (std::abs (dot_p)) < eps_angle )
+          double dot_p = normals[seed_queue[sq_idx]].normal[0] * normals[nn_indices[j]].normal[0] +
+                         normals[seed_queue[sq_idx]].normal[1] * normals[nn_indices[j]].normal[1] +
+                         normals[seed_queue[sq_idx]].normal[2] * normals[nn_indices[j]].normal[2];
+          if ( std::abs (dot_p) > cos_eps_angle )
           {
             processed[nn_indices[j]] = true;
             seed_queue.push_back (nn_indices[j]);
@@ -292,6 +306,11 @@ namespace pcl
         r.header = cloud.header;
         clusters.push_back (r);
       }
+      else
+      {
+        PCL_DEBUG("[pcl::extractEuclideanClusters] This cluster has %zu points, which is not between %u and %u points, so it is not a final cluster\n",
+                  seed_queue.size (), min_pts_per_cluster, max_pts_per_cluster);
+      }
     }
   }
 
@@ -323,7 +342,7 @@ namespace pcl
       EuclideanClusterExtraction () : tree_ (), 
                                       cluster_tolerance_ (0),
                                       min_pts_per_cluster_ (1), 
-                                      max_pts_per_cluster_ (std::numeric_limits<int>::max ())
+                                      max_pts_per_cluster_ (std::numeric_limits<pcl::uindex_t>::max ())
       {};
 
       /** \brief Provide a pointer to the search object.
@@ -364,13 +383,13 @@ namespace pcl
         * \param[in] min_cluster_size the minimum cluster size
         */
       inline void 
-      setMinClusterSize (int min_cluster_size) 
+      setMinClusterSize (pcl::uindex_t min_cluster_size)
       { 
         min_pts_per_cluster_ = min_cluster_size; 
       }
 
       /** \brief Get the minimum number of points that a cluster needs to contain in order to be considered valid. */
-      inline int 
+      inline pcl::uindex_t
       getMinClusterSize () const 
       { 
         return (min_pts_per_cluster_); 
@@ -380,13 +399,13 @@ namespace pcl
         * \param[in] max_cluster_size the maximum cluster size
         */
       inline void 
-      setMaxClusterSize (int max_cluster_size) 
+      setMaxClusterSize (pcl::uindex_t max_cluster_size)
       { 
         max_pts_per_cluster_ = max_cluster_size; 
       }
 
       /** \brief Get the maximum number of points that a cluster needs to contain in order to be considered valid. */
-      inline int 
+      inline pcl::uindex_t
       getMaxClusterSize () const 
       { 
         return (max_pts_per_cluster_); 
@@ -412,10 +431,10 @@ namespace pcl
       double cluster_tolerance_;
 
       /** \brief The minimum number of points that a cluster needs to contain in order to be considered valid (default = 1). */
-      int min_pts_per_cluster_;
+      pcl::uindex_t min_pts_per_cluster_;
 
       /** \brief The maximum number of points that a cluster needs to contain in order to be considered valid (default = MAXINT). */
-      int max_pts_per_cluster_;
+      pcl::uindex_t max_pts_per_cluster_;
 
       /** \brief Class getName method. */
       virtual std::string getClassName () const { return ("EuclideanClusterExtraction"); }
diff --git a/segmentation/include/pcl/segmentation/extract_labeled_clusters.h b/segmentation/include/pcl/segmentation/extract_labeled_clusters.h
index e1a9b522..d65beaca 100644
--- a/segmentation/include/pcl/segmentation/extract_labeled_clusters.h
+++ b/segmentation/include/pcl/segmentation/extract_labeled_clusters.h
@@ -35,155 +35,227 @@
 
 #pragma once
 
+#include <pcl/search/search.h>
 #include <pcl/pcl_base.h>
-#include <pcl/search/pcl_search.h>
 
-namespace pcl
-{
+namespace pcl {
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief Decompose a region of space into clusters based on the Euclidean distance
+ * between points
+ * \param[in] cloud the point cloud message
+ * \param[in] tree the spatial locator (e.g., kd-tree) used for nearest neighbors
+ * searching
+ * \note the tree has to be created as a spatial locator on \a cloud
+ * \param[in] tolerance the spatial cluster tolerance as a measure in L2 Euclidean space
+ * \param[out] labeled_clusters the resultant clusters containing point indices (as a
+ * vector of PointIndices)
+ * \param[in] min_pts_per_cluster minimum number of points that a cluster may contain
+ * (default: 1)
+ * \param[in] max_pts_per_cluster maximum number of points that a cluster may contain
+ * (default: max int)
+ * \param[in] max_label
+ * \ingroup segmentation
+ */
+template <typename PointT>
+PCL_DEPRECATED(1, 14, "Use of max_label is deprecated")
+void extractLabeledEuclideanClusters(
+    const PointCloud<PointT>& cloud,
+    const typename search::Search<PointT>::Ptr& tree,
+    float tolerance,
+    std::vector<std::vector<PointIndices>>& labeled_clusters,
+    unsigned int min_pts_per_cluster,
+    unsigned int max_pts_per_cluster,
+    unsigned int max_label);
+
+/** \brief Decompose a region of space into clusters based on the Euclidean distance
+ * between points
+ * \param[in] cloud the point cloud message
+ * \param[in] tree the spatial locator (e.g., kd-tree) used for nearest neighbors
+ * searching \note the tree has to be created as a spatial locator on \a cloud
+ * \param[in] tolerance the spatial cluster tolerance as a measure in L2 Euclidean space
+ * \param[out] labeled_clusters the resultant clusters containing point indices
+ * (as a vector of PointIndices)
+ * \param[in] min_pts_per_cluster minimum number of points that a cluster may contain
+ * (default: 1)
+ * \param[in] max_pts_per_cluster maximum number of points that a cluster may contain
+ * (default: max int)
+ * \ingroup segmentation
+ */
+template <typename PointT>
+void
+extractLabeledEuclideanClusters(
+    const PointCloud<PointT>& cloud,
+    const typename search::Search<PointT>::Ptr& tree,
+    float tolerance,
+    std::vector<std::vector<PointIndices>>& labeled_clusters,
+    unsigned int min_pts_per_cluster = 1,
+    unsigned int max_pts_per_cluster = std::numeric_limits<unsigned int>::max());
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b LabeledEuclideanClusterExtraction represents a segmentation class for
+ * cluster extraction in an Euclidean sense, with label info. \author Koen Buys
+ * \ingroup segmentation
+ */
+template <typename PointT>
+class LabeledEuclideanClusterExtraction : public PCLBase<PointT> {
+  using BasePCLBase = PCLBase<PointT>;
+
+public:
+  using PointCloud = pcl::PointCloud<PointT>;
+  using PointCloudPtr = typename PointCloud::Ptr;
+  using PointCloudConstPtr = typename PointCloud::ConstPtr;
+
+  using KdTree = pcl::search::Search<PointT>;
+  using KdTreePtr = typename KdTree::Ptr;
+
+  using PointIndicesPtr = PointIndices::Ptr;
+  using PointIndicesConstPtr = PointIndices::ConstPtr;
+
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-  /** \brief Decompose a region of space into clusters based on the Euclidean distance between points
-    * \param[in] cloud the point cloud message
-    * \param[in] tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching
-    * \note the tree has to be created as a spatial locator on \a cloud
-    * \param[in] tolerance the spatial cluster tolerance as a measure in L2 Euclidean space
-    * \param[out] labeled_clusters the resultant clusters containing point indices (as a vector of PointIndices)
-    * \param[in] min_pts_per_cluster minimum number of points that a cluster may contain (default: 1)
-    * \param[in] max_pts_per_cluster maximum number of points that a cluster may contain (default: max int)
-    * \param[in] max_label
-    * \ingroup segmentation
-    */
-  template <typename PointT> void 
-  extractLabeledEuclideanClusters (
-      const PointCloud<PointT> &cloud, const typename search::Search<PointT>::Ptr &tree,
-      float tolerance, std::vector<std::vector<PointIndices> > &labeled_clusters,
-      unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = std::numeric_limits<unsigned int>::max (),
-      unsigned int max_label = std::numeric_limits<unsigned int>::max ());
-
-  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-  /** \brief @b LabeledEuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense, with label info.
-    * \author Koen Buys
-    * \ingroup segmentation
-    */
-  template <typename PointT>
-  class LabeledEuclideanClusterExtraction: public PCLBase<PointT>
+  /** \brief Empty constructor. */
+  LabeledEuclideanClusterExtraction()
+  : tree_()
+  , cluster_tolerance_(0)
+  , min_pts_per_cluster_(1)
+  , max_pts_per_cluster_(std::numeric_limits<int>::max())
+  , max_label_(std::numeric_limits<int>::max()){};
+
+  /** \brief Provide a pointer to the search object.
+   * \param[in] tree a pointer to the spatial search object.
+   */
+  inline void
+  setSearchMethod(const KdTreePtr& tree)
+  {
+    tree_ = tree;
+  }
+
+  /** \brief Get a pointer to the search method used. */
+  inline KdTreePtr
+  getSearchMethod() const
+  {
+    return (tree_);
+  }
+
+  /** \brief Set the spatial cluster tolerance as a measure in the L2 Euclidean space
+   * \param[in] tolerance the spatial cluster tolerance as a measure in the L2 Euclidean
+   * space
+   */
+  inline void
+  setClusterTolerance(double tolerance)
+  {
+    cluster_tolerance_ = tolerance;
+  }
+
+  /** \brief Get the spatial cluster tolerance as a measure in the L2 Euclidean space.
+   */
+  inline double
+  getClusterTolerance() const
+  {
+    return (cluster_tolerance_);
+  }
+
+  /** \brief Set the minimum number of points that a cluster needs to contain in order
+   * to be considered valid. \param[in] min_cluster_size the minimum cluster size
+   */
+  inline void
+  setMinClusterSize(int min_cluster_size)
+  {
+    min_pts_per_cluster_ = min_cluster_size;
+  }
+
+  /** \brief Get the minimum number of points that a cluster needs to contain in order
+   * to be considered valid. */
+  inline int
+  getMinClusterSize() const
   {
-    using BasePCLBase = PCLBase<PointT>;
-
-    public:
-      using PointCloud = pcl::PointCloud<PointT>;
-      using PointCloudPtr = typename PointCloud::Ptr;
-      using PointCloudConstPtr = typename PointCloud::ConstPtr;
-
-      using KdTree = pcl::search::Search<PointT>;
-      using KdTreePtr = typename KdTree::Ptr;
-
-      using PointIndicesPtr = PointIndices::Ptr;
-      using PointIndicesConstPtr = PointIndices::ConstPtr;
-
-      //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-      /** \brief Empty constructor. */
-      LabeledEuclideanClusterExtraction () : 
-        tree_ (), 
-        cluster_tolerance_ (0),
-        min_pts_per_cluster_ (1), 
-        max_pts_per_cluster_ (std::numeric_limits<int>::max ()),
-        max_label_ (std::numeric_limits<int>::max ())
-      {};
-
-      /** \brief Provide a pointer to the search object.
-        * \param[in] tree a pointer to the spatial search object.
-        */
-      inline void 
-      setSearchMethod (const KdTreePtr &tree) { tree_ = tree; }
-
-      /** \brief Get a pointer to the search method used. */
-      inline KdTreePtr 
-      getSearchMethod () const { return (tree_); }
-
-      /** \brief Set the spatial cluster tolerance as a measure in the L2 Euclidean space
-        * \param[in] tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space
-        */
-      inline void 
-      setClusterTolerance (double tolerance) { cluster_tolerance_ = tolerance; }
-
-      /** \brief Get the spatial cluster tolerance as a measure in the L2 Euclidean space. */
-      inline double 
-      getClusterTolerance () const { return (cluster_tolerance_); }
-
-      /** \brief Set the minimum number of points that a cluster needs to contain in order to be considered valid.
-        * \param[in] min_cluster_size the minimum cluster size
-        */
-      inline void 
-      setMinClusterSize (int min_cluster_size) { min_pts_per_cluster_ = min_cluster_size; }
-
-      /** \brief Get the minimum number of points that a cluster needs to contain in order to be considered valid. */
-      inline int 
-      getMinClusterSize () const { return (min_pts_per_cluster_); }
-
-      /** \brief Set the maximum number of points that a cluster needs to contain in order to be considered valid.
-        * \param[in] max_cluster_size the maximum cluster size
-        */
-      inline void 
-      setMaxClusterSize (int max_cluster_size) { max_pts_per_cluster_ = max_cluster_size; }
-
-      /** \brief Get the maximum number of points that a cluster needs to contain in order to be considered valid. */
-      inline int 
-      getMaxClusterSize () const { return (max_pts_per_cluster_); }
-
-      /** \brief Set the maximum number of labels in the cloud.
-        * \param[in] max_label the maximum
-        */
-      inline void 
-      setMaxLabels (unsigned int max_label) { max_label_ = max_label; }
-
-      /** \brief Get the maximum number of labels */
-      inline unsigned int 
-      getMaxLabels () const { return (max_label_); }
-
-      /** \brief Cluster extraction in a PointCloud given by <setInputCloud (), setIndices ()>
-        * \param[out] labeled_clusters the resultant point clusters
-        */
-      void 
-      extract (std::vector<std::vector<PointIndices> > &labeled_clusters);
-
-    protected:
-      // Members derived from the base class
-      using BasePCLBase::input_;
-      using BasePCLBase::indices_;
-      using BasePCLBase::initCompute;
-      using BasePCLBase::deinitCompute;
-
-      /** \brief A pointer to the spatial search object. */
-      KdTreePtr tree_;
-
-      /** \brief The spatial cluster tolerance as a measure in the L2 Euclidean space. */
-      double cluster_tolerance_;
-
-      /** \brief The minimum number of points that a cluster needs to contain in order to be considered valid (default = 1). */
-      int min_pts_per_cluster_;
-
-      /** \brief The maximum number of points that a cluster needs to contain in order to be considered valid (default = MAXINT). */
-      int max_pts_per_cluster_;
-
-      /** \brief The maximum number of labels we can find in this pointcloud (default = MAXINT)*/
-      unsigned int max_label_;
-
-      /** \brief Class getName method. */
-      virtual std::string getClassName () const { return ("LabeledEuclideanClusterExtraction"); }
-
-  };
-
-  /** \brief Sort clusters method (for std::sort). 
-    * \ingroup segmentation
-    */
-  inline bool 
-    compareLabeledPointClusters (const pcl::PointIndices &a, const pcl::PointIndices &b)
+    return (min_pts_per_cluster_);
+  }
+
+  /** \brief Set the maximum number of points that a cluster needs to contain in order
+   * to be considered valid. \param[in] max_cluster_size the maximum cluster size
+   */
+  inline void
+  setMaxClusterSize(int max_cluster_size)
   {
-    return (a.indices.size () < b.indices.size ());
+    max_pts_per_cluster_ = max_cluster_size;
   }
+
+  /** \brief Get the maximum number of points that a cluster needs to contain in order
+   * to be considered valid. */
+  inline int
+  getMaxClusterSize() const
+  {
+    return (max_pts_per_cluster_);
+  }
+
+  /** \brief Set the maximum number of labels in the cloud.
+   * \param[in] max_label the maximum
+   */
+  PCL_DEPRECATED(1, 14, "Max label is being deprecated")
+  inline void
+  setMaxLabels(unsigned int max_label)
+  {
+    max_label_ = max_label;
+  }
+
+  /** \brief Get the maximum number of labels */
+  PCL_DEPRECATED(1, 14, "Max label is being deprecated")
+  inline unsigned int
+  getMaxLabels() const
+  {
+    return (max_label_);
+  }
+
+  /** \brief Cluster extraction in a PointCloud given by <setInputCloud (), setIndices
+   * ()> \param[out] labeled_clusters the resultant point clusters
+   */
+  void
+  extract(std::vector<std::vector<PointIndices>>& labeled_clusters);
+
+protected:
+  // Members derived from the base class
+  using BasePCLBase::deinitCompute;
+  using BasePCLBase::indices_;
+  using BasePCLBase::initCompute;
+  using BasePCLBase::input_;
+
+  /** \brief A pointer to the spatial search object. */
+  KdTreePtr tree_;
+
+  /** \brief The spatial cluster tolerance as a measure in the L2 Euclidean space. */
+  double cluster_tolerance_;
+
+  /** \brief The minimum number of points that a cluster needs to contain in order to be
+   * considered valid (default = 1). */
+  int min_pts_per_cluster_;
+
+  /** \brief The maximum number of points that a cluster needs to contain in order to be
+   * considered valid (default = MAXINT). */
+  int max_pts_per_cluster_;
+
+  /** \brief The maximum number of labels we can find in this pointcloud (default =
+   * MAXINT)*/
+  unsigned int max_label_;
+
+  /** \brief Class getName method. */
+  virtual std::string
+  getClassName() const
+  {
+    return ("LabeledEuclideanClusterExtraction");
+  }
+};
+
+/** \brief Sort clusters method (for std::sort).
+ * \ingroup segmentation
+ */
+inline bool
+compareLabeledPointClusters(const pcl::PointIndices& a, const pcl::PointIndices& b)
+{
+  return (a.indices.size() < b.indices.size());
 }
+} // namespace pcl
 
 #ifdef PCL_NO_PRECOMPILE
 #include <pcl/segmentation/impl/extract_labeled_clusters.hpp>
diff --git a/segmentation/include/pcl/segmentation/extract_polygonal_prism_data.h b/segmentation/include/pcl/segmentation/extract_polygonal_prism_data.h
index f2df3fd9..4289db01 100644
--- a/segmentation/include/pcl/segmentation/extract_polygonal_prism_data.h
+++ b/segmentation/include/pcl/segmentation/extract_polygonal_prism_data.h
@@ -37,8 +37,9 @@
 
 #pragma once
 
+#include <cfloat> // for FLT_MAX
+
 #include <pcl/pcl_base.h>
-#include <pcl/sample_consensus/sac_model_plane.h>
 
 namespace pcl
 {
diff --git a/segmentation/include/pcl/segmentation/grabcut_segmentation.h b/segmentation/include/pcl/segmentation/grabcut_segmentation.h
index 2e21ab66..0e1eed8e 100644
--- a/segmentation/include/pcl/segmentation/grabcut_segmentation.h
+++ b/segmentation/include/pcl/segmentation/grabcut_segmentation.h
@@ -294,14 +294,14 @@ namespace pcl
       /** Build the initial GMMs using the Orchard and Bouman color clustering algorithm */
       PCL_EXPORTS void
       buildGMMs (const Image &image,
-                 const std::vector<int>& indices,
+                 const Indices& indices,
                  const std::vector<SegmentationValue> &hardSegmentation,
                  std::vector<std::size_t> &components,
                  GMM &background_GMM, GMM &foreground_GMM);
       /** Iteratively learn GMMs using GrabCut updating algorithm */
       PCL_EXPORTS void
       learnGMMs (const Image& image,
-                 const std::vector<int>& indices,
+                 const Indices& indices,
                  const std::vector<SegmentationValue>& hard_segmentation,
                  std::vector<std::size_t>& components,
                  GMM& background_GMM, GMM& foreground_GMM);
@@ -402,7 +402,7 @@ namespace pcl
         NLinks () : nb_links (0), indices (0), dists (0), weights (0) {}
 
         int nb_links;
-        std::vector<int> indices;
+        Indices indices;
         std::vector<float> dists;
         std::vector<float> weights;
       };
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 e9e4bb25..f9bd7c30 100644
--- a/segmentation/include/pcl/segmentation/impl/approximate_progressive_morphological_filter.hpp
+++ b/segmentation/include/pcl/segmentation/impl/approximate_progressive_morphological_filter.hpp
@@ -69,7 +69,7 @@ pcl::ApproximateProgressiveMorphologicalFilter<PointT>::~ApproximateProgressiveM
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
-pcl::ApproximateProgressiveMorphologicalFilter<PointT>::extract (std::vector<int>& ground)
+pcl::ApproximateProgressiveMorphologicalFilter<PointT>::extract (Indices& ground)
 {
   bool segmentation_is_possible = initCompute ();
   if (!segmentation_is_possible)
@@ -229,7 +229,7 @@ pcl::ApproximateProgressiveMorphologicalFilter<PointT>::extract (std::vector<int
 
     // Find indices of the points whose difference between the source and
     // filtered point clouds is less than the current height threshold.
-    std::vector<int> pt_indices;
+    Indices pt_indices;
     for (std::size_t p_idx = 0; p_idx < ground.size (); ++p_idx)
     {
       PointT p = (*cloud)[p_idx];
diff --git a/segmentation/include/pcl/segmentation/impl/conditional_euclidean_clustering.hpp b/segmentation/include/pcl/segmentation/impl/conditional_euclidean_clustering.hpp
index 752098cc..93230268 100644
--- a/segmentation/include/pcl/segmentation/impl/conditional_euclidean_clustering.hpp
+++ b/segmentation/include/pcl/segmentation/impl/conditional_euclidean_clustering.hpp
@@ -38,6 +38,8 @@
 #define PCL_SEGMENTATION_IMPL_CONDITIONAL_EUCLIDEAN_CLUSTERING_HPP_
 
 #include <pcl/segmentation/conditional_euclidean_clustering.h>
+#include <pcl/search/organized.h> // for OrganizedNeighbor
+#include <pcl/search/kdtree.h> // for KdTree
 
 template<typename PointT> void
 pcl::ConditionalEuclideanClustering<PointT>::segment (pcl::IndicesClusters &clusters)
@@ -65,7 +67,7 @@ pcl::ConditionalEuclideanClustering<PointT>::segment (pcl::IndicesClusters &clus
   searcher_->setInputCloud (input_, indices_);
 
   // Temp variables used by search class
-  std::vector<int> nn_indices;
+  Indices nn_indices;
   std::vector<float> nn_distances;
 
   // Create a bool vector of processed point indices, and initialize it to false
@@ -73,19 +75,19 @@ pcl::ConditionalEuclideanClustering<PointT>::segment (pcl::IndicesClusters &clus
   std::vector<bool> processed (input_->size (), false);
 
   // Process all points indexed by indices_
-  for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)  // iii = input indices iterator
+  for (const auto& iindex : (*indices_)) // iindex = input index
   {
     // Has this point been processed before?
-    if ((*indices_)[iii] == -1 || processed[(*indices_)[iii]])
+    if (iindex == UNAVAILABLE || processed[iindex])
       continue;
 
     // Set up a new growing cluster
-    std::vector<int> current_cluster;
+    Indices current_cluster;
     int cii = 0;  // cii = cluster indices iterator
 
     // Add the point to the cluster
-    current_cluster.push_back ((*indices_)[iii]);
-    processed[(*indices_)[iii]] = true;
+    current_cluster.push_back (iindex);
+    processed[iindex] = true;
 
     // Process the current cluster (it can be growing in size as it is being processed)
     while (cii < static_cast<int> (current_cluster.size ()))
@@ -101,7 +103,7 @@ pcl::ConditionalEuclideanClustering<PointT>::segment (pcl::IndicesClusters &clus
       for (int nii = 1; nii < static_cast<int> (nn_indices.size ()); ++nii)  // nii = neighbor indices iterator
       {
         // Has this point been processed before?
-        if (nn_indices[nii] == -1 || processed[nn_indices[nii]])
+        if (nn_indices[nii] == UNAVAILABLE || processed[nn_indices[nii]])
           continue;
 
         // Validate if condition holds
diff --git a/segmentation/include/pcl/segmentation/impl/cpc_segmentation.hpp b/segmentation/include/pcl/segmentation/impl/cpc_segmentation.hpp
index 872bf29e..030d8594 100644
--- a/segmentation/include/pcl/segmentation/impl/cpc_segmentation.hpp
+++ b/segmentation/include/pcl/segmentation/impl/cpc_segmentation.hpp
@@ -38,6 +38,7 @@
 #ifndef PCL_SEGMENTATION_IMPL_CPC_SEGMENTATION_HPP_
 #define PCL_SEGMENTATION_IMPL_CPC_SEGMENTATION_HPP_
 
+#include <pcl/sample_consensus/sac_model_plane.h> // for SampleConsensusModelPlane
 #include <pcl/segmentation/cpc_segmentation.h>
 
 template <typename PointT>
@@ -203,7 +204,7 @@ pcl::CPCSegmentation<PointT>::applyCuttingPlane (std::uint32_t depth_levels_left
         int cluster_concave_pts = 0;
         float cluster_score = 0;
 //         std::cout << "Cluster has " << cluster_indices[cc].indices.size () << " points" << std::endl;
-        for (const int &current_index : cluster_index.indices)
+        for (const auto &current_index : cluster_index.indices)
         {
           double index_score = weights[current_index];
           if (use_directed_weights_)
@@ -239,7 +240,7 @@ pcl::CPCSegmentation<PointT>::applyCuttingPlane (std::uint32_t depth_levels_left
     }
 
     int number_connections_cut = 0;
-    for (const int &point_index : cut_support_indices)
+    for (const auto &point_index : cut_support_indices)
     {
       if (use_clean_cutting_)
       {
@@ -294,7 +295,7 @@ pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel (int)
   iterations_ = 0;
   best_score_ = -std::numeric_limits<double>::max ();
 
-  std::vector<int> selection;
+  pcl::Indices selection;
   Eigen::VectorXf model_coefficients;
 
   unsigned skipped_count = 0;
@@ -328,7 +329,7 @@ pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel (int)
     sac_model_->selectWithinDistance (model_coefficients, threshold_, *current_inliers);
     double current_score = 0;
     Eigen::Vector3f plane_normal (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
-    for (const int &current_index : *current_inliers)
+    for (const auto &current_index : *current_inliers)
     {
       double index_score = weights_[current_index];
       if (use_directed_weights_)
diff --git a/segmentation/include/pcl/segmentation/impl/crf_segmentation.hpp b/segmentation/include/pcl/segmentation/impl/crf_segmentation.hpp
index dd16bf24..14821ea6 100644
--- a/segmentation/include/pcl/segmentation/impl/crf_segmentation.hpp
+++ b/segmentation/include/pcl/segmentation/impl/crf_segmentation.hpp
@@ -40,11 +40,11 @@
 #ifndef PCL_CRF_SEGMENTATION_HPP_
 #define PCL_CRF_SEGMENTATION_HPP_
 
+#include <pcl/filters/voxel_grid_label.h> // for VoxelGridLabel
 #include <pcl/segmentation/crf_segmentation.h>
 
 #include <pcl/common/io.h>
 
-#include <cstdio>
 #include <cstdlib>
 #include <ctime>
 
diff --git a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
index e41dc3f0..f5ec59ad 100644
--- a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
+++ b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
@@ -39,6 +39,7 @@
 #define PCL_SEGMENTATION_IMPL_EXTRACT_CLUSTERS_H_
 
 #include <pcl/segmentation/extract_clusters.h>
+#include <pcl/search/organized.h> // for OrganizedNeighbor
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
@@ -61,7 +62,7 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
   // Create a bool vector of processed point indices, and initialize it to false
   std::vector<bool> processed (cloud.size (), false);
 
-  std::vector<int> nn_indices;
+  Indices nn_indices;
   std::vector<float> nn_distances;
   // Process all points in the indices vector
   for (int i = 0; i < static_cast<int> (cloud.size ()); ++i)
@@ -69,7 +70,7 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
     if (processed[i])
       continue;
 
-    std::vector<int> seed_queue;
+    Indices seed_queue;
     int sq_idx = 0;
     seed_queue.push_back (i);
 
@@ -86,7 +87,7 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
 
       for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)             // can't assume sorted (default isn't!)
       {
-        if (nn_indices[j] == -1 || processed[nn_indices[j]])        // Has this point been processed before ?
+        if (nn_indices[j] == UNAVAILABLE || processed[nn_indices[j]])        // Has this point been processed before ?
           continue;
 
         // Perform a simple Euclidean clustering
@@ -112,6 +113,11 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
       r.header = cloud.header;
       clusters.push_back (r);   // We could avoid a copy by working directly in the vector
     }
+    else
+    {
+      PCL_DEBUG("[pcl::extractEuclideanClusters] This cluster has %zu points, which is not between %u and %u points, so it is not a final cluster\n",
+                seed_queue.size (), min_pts_per_cluster, max_pts_per_cluster);
+    }
   }
 }
 
@@ -119,7 +125,7 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
 /** @todo: fix the return value, make sure the exit is not needed anymore*/
 template <typename PointT> void
 pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
-                               const std::vector<int> &indices,
+                               const Indices &indices,
                                const typename search::Search<PointT>::Ptr &tree,
                                float tolerance, std::vector<PointIndices> &clusters,
                                unsigned int min_pts_per_cluster,
@@ -147,15 +153,15 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
   // Create a bool vector of processed point indices, and initialize it to false
   std::vector<bool> processed (cloud.size (), false);
 
-  std::vector<int> nn_indices;
+  Indices nn_indices;
   std::vector<float> nn_distances;
   // Process all points in the indices vector
-  for (const int &index : indices)
+  for (const auto &index : indices)
   {
     if (processed[index])
       continue;
 
-    std::vector<int> seed_queue;
+    Indices seed_queue;
     int sq_idx = 0;
     seed_queue.push_back (index);
 
@@ -178,7 +184,7 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
 
       for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)             // can't assume sorted (default isn't!)
       {
-        if (nn_indices[j] == -1 || processed[nn_indices[j]])        // Has this point been processed before ?
+        if (nn_indices[j] == UNAVAILABLE || processed[nn_indices[j]])        // Has this point been processed before ?
           continue;
 
         // Perform a simple Euclidean clustering
@@ -206,6 +212,11 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
       r.header = cloud.header;
       clusters.push_back (r);   // We could avoid a copy by working directly in the vector
     }
+    else
+    {
+      PCL_DEBUG("[pcl::extractEuclideanClusters] This cluster has %zu points, which is not between %u and %u points, so it is not a final cluster\n",
+                seed_queue.size (), min_pts_per_cluster, max_pts_per_cluster);
+    }
   }
 }
 
@@ -248,6 +259,6 @@ pcl::EuclideanClusterExtraction<PointT>::extract (std::vector<PointIndices> &clu
 
 #define PCL_INSTANTIATE_EuclideanClusterExtraction(T) template class PCL_EXPORTS pcl::EuclideanClusterExtraction<T>;
 #define PCL_INSTANTIATE_extractEuclideanClusters(T) template void PCL_EXPORTS pcl::extractEuclideanClusters<T>(const pcl::PointCloud<T> &, const typename pcl::search::Search<T>::Ptr &, float , std::vector<pcl::PointIndices> &, unsigned int, unsigned int);
-#define PCL_INSTANTIATE_extractEuclideanClusters_indices(T) template void PCL_EXPORTS pcl::extractEuclideanClusters<T>(const pcl::PointCloud<T> &, const std::vector<int> &, const typename pcl::search::Search<T>::Ptr &, float , std::vector<pcl::PointIndices> &, unsigned int, unsigned int);
+#define PCL_INSTANTIATE_extractEuclideanClusters_indices(T) template void PCL_EXPORTS pcl::extractEuclideanClusters<T>(const pcl::PointCloud<T> &, const pcl::Indices &, const typename pcl::search::Search<T>::Ptr &, float , std::vector<pcl::PointIndices> &, unsigned int, unsigned int);
 
 #endif        // PCL_EXTRACT_CLUSTERS_IMPL_H_
diff --git a/segmentation/include/pcl/segmentation/impl/extract_labeled_clusters.hpp b/segmentation/include/pcl/segmentation/impl/extract_labeled_clusters.hpp
index 5ebea0b1..cbaa75aa 100644
--- a/segmentation/include/pcl/segmentation/impl/extract_labeled_clusters.hpp
+++ b/segmentation/include/pcl/segmentation/impl/extract_labeled_clusters.hpp
@@ -40,61 +40,81 @@
 #include <pcl/segmentation/extract_labeled_clusters.h>
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> void
-pcl::extractLabeledEuclideanClusters (const PointCloud<PointT> &cloud,
-                                      const typename search::Search<PointT>::Ptr &tree,
-                                      float tolerance,
-                                      std::vector<std::vector<PointIndices> > &labeled_clusters,
-                                      unsigned int min_pts_per_cluster,
-                                      unsigned int max_pts_per_cluster,
-                                      unsigned int)
+template <typename PointT>
+void
+pcl::extractLabeledEuclideanClusters(
+    const PointCloud<PointT>& cloud,
+    const typename search::Search<PointT>::Ptr& tree,
+    float tolerance,
+    std::vector<std::vector<PointIndices>>& labeled_clusters,
+    unsigned int min_pts_per_cluster,
+    unsigned int max_pts_per_cluster,
+    unsigned int)
 {
-  if (tree->getInputCloud ()->size () != cloud.size ())
-  {
+  pcl::extractLabeledEuclideanClusters<PointT>(cloud,
+                                               tree,
+                                               tolerance,
+                                               labeled_clusters,
+                                               min_pts_per_cluster,
+                                               max_pts_per_cluster);
+}
+
+template <typename PointT>
+void
+pcl::extractLabeledEuclideanClusters(
+    const PointCloud<PointT>& cloud,
+    const typename search::Search<PointT>::Ptr& tree,
+    float tolerance,
+    std::vector<std::vector<PointIndices>>& labeled_clusters,
+    unsigned int min_pts_per_cluster,
+    unsigned int max_pts_per_cluster)
+{
+  if (tree->getInputCloud()->size() != cloud.size()) {
     PCL_ERROR("[pcl::extractLabeledEuclideanClusters] Tree built for a different point "
-              "cloud dataset (%zu) than the input cloud (%zu)!\n",
-              static_cast<std::size_t>(tree->getInputCloud()->size()),
-              static_cast<std::size_t>(cloud.size()));
+              "cloud dataset (%lu) than the input cloud (%lu)!\n",
+              tree->getInputCloud()->size(),
+              cloud.size());
     return;
   }
   // Create a bool vector of processed point indices, and initialize it to false
-  std::vector<bool> processed (cloud.size (), false);
+  std::vector<bool> processed(cloud.size(), false);
 
-  std::vector<int> nn_indices;
+  Indices nn_indices;
   std::vector<float> nn_distances;
 
   // Process all points in the indices vector
-  for (int i = 0; i < static_cast<int> (cloud.size ()); ++i)
-  {
+  for (index_t i = 0; i < static_cast<index_t>(cloud.size()); ++i) {
     if (processed[i])
       continue;
 
-    std::vector<int> seed_queue;
+    Indices seed_queue;
     int sq_idx = 0;
-    seed_queue.push_back (i);
+    seed_queue.push_back(i);
 
     processed[i] = true;
 
-    while (sq_idx < static_cast<int> (seed_queue.size ()))
-    {
+    while (sq_idx < static_cast<int>(seed_queue.size())) {
       // Search for sq_idx
-      int ret = tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, std::numeric_limits<int>::max());
-      if(ret == -1)
+      int ret = tree->radiusSearch(seed_queue[sq_idx],
+                                   tolerance,
+                                   nn_indices,
+                                   nn_distances,
+                                   std::numeric_limits<int>::max());
+      if (ret == -1)
         PCL_ERROR("radiusSearch on tree came back with error -1");
-      if (!ret)
-      {
+      if (!ret) {
         sq_idx++;
         continue;
       }
 
-      for (std::size_t j = 1; j < nn_indices.size (); ++j)             // nn_indices[0] should be sq_idx
+      for (std::size_t j = 1; j < nn_indices.size();
+           ++j) // nn_indices[0] should be sq_idx
       {
-        if (processed[nn_indices[j]])                             // Has this point been processed before ?
+        if (processed[nn_indices[j]]) // Has this point been processed before ?
           continue;
-        if (cloud[i].label == cloud[nn_indices[j]].label)
-        {
+        if (cloud[i].label == cloud[nn_indices[j]].label) {
           // Perform a simple Euclidean clustering
-          seed_queue.push_back (nn_indices[j]);
+          seed_queue.push_back(nn_indices[j]);
           processed[nn_indices[j]] = true;
         }
       }
@@ -103,18 +123,19 @@ pcl::extractLabeledEuclideanClusters (const PointCloud<PointT> &cloud,
     }
 
     // If this queue is satisfactory, add to the clusters
-    if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
-    {
+    if (seed_queue.size() >= min_pts_per_cluster &&
+        seed_queue.size() <= max_pts_per_cluster) {
       pcl::PointIndices r;
-      r.indices.resize (seed_queue.size ());
-      for (std::size_t j = 0; j < seed_queue.size (); ++j)
+      r.indices.resize(seed_queue.size());
+      for (std::size_t j = 0; j < seed_queue.size(); ++j)
         r.indices[j] = seed_queue[j];
 
-      std::sort (r.indices.begin (), r.indices.end ());
-      r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
+      std::sort(r.indices.begin(), r.indices.end());
+      r.indices.erase(std::unique(r.indices.begin(), r.indices.end()), r.indices.end());
 
       r.header = cloud.header;
-      labeled_clusters[cloud[i].label].push_back (r);   // We could avoid a copy by working directly in the vector
+      labeled_clusters[cloud[i].label].push_back(
+          r); // We could avoid a copy by working directly in the vector
     }
   }
 }
@@ -122,38 +143,59 @@ pcl::extractLabeledEuclideanClusters (const PointCloud<PointT> &cloud,
 //////////////////////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////////////////////
 
-template <typename PointT> void 
-pcl::LabeledEuclideanClusterExtraction<PointT>::extract (std::vector<std::vector<PointIndices> > &labeled_clusters)
+template <typename PointT>
+void
+pcl::LabeledEuclideanClusterExtraction<PointT>::extract(
+    std::vector<std::vector<PointIndices>>& labeled_clusters)
 {
-  if (!initCompute () || 
-      (input_   && input_->points.empty ()) ||
-      (indices_ && indices_->empty ()))
-  {
-    labeled_clusters.clear ();
+  if (!initCompute() || (input_ && input_->empty()) ||
+      (indices_ && indices_->empty())) {
+    labeled_clusters.clear();
     return;
   }
 
   // Initialize the spatial locator
-  if (!tree_)
-  {
-    if (input_->isOrganized ())
-      tree_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
+  if (!tree_) {
+    if (input_->isOrganized())
+      tree_.reset(new pcl::search::OrganizedNeighbor<PointT>());
     else
-      tree_.reset (new pcl::search::KdTree<PointT> (false));
+      tree_.reset(new pcl::search::KdTree<PointT>(false));
   }
 
   // Send the input dataset to the spatial locator
-  tree_->setInputCloud (input_);
-  extractLabeledEuclideanClusters (*input_, tree_, static_cast<float> (cluster_tolerance_), labeled_clusters, min_pts_per_cluster_, max_pts_per_cluster_, max_label_);
+  tree_->setInputCloud(input_);
+  extractLabeledEuclideanClusters(*input_,
+                                  tree_,
+                                  static_cast<float>(cluster_tolerance_),
+                                  labeled_clusters,
+                                  min_pts_per_cluster_,
+                                  max_pts_per_cluster_);
 
   // Sort the clusters based on their size (largest one first)
-  for (auto &labeled_cluster : labeled_clusters)
-    std::sort (labeled_cluster.rbegin (), labeled_cluster.rend (), comparePointClusters);
+  for (auto& labeled_cluster : labeled_clusters)
+    std::sort(labeled_cluster.rbegin(), labeled_cluster.rend(), comparePointClusters);
 
-  deinitCompute ();
+  deinitCompute();
 }
 
-#define PCL_INSTANTIATE_LabeledEuclideanClusterExtraction(T) template class PCL_EXPORTS pcl::LabeledEuclideanClusterExtraction<T>;
-#define PCL_INSTANTIATE_extractLabeledEuclideanClusters(T) template void PCL_EXPORTS pcl::extractLabeledEuclideanClusters<T>(const pcl::PointCloud<T> &, const typename pcl::search::Search<T>::Ptr &, float , std::vector<std::vector<pcl::PointIndices> > &, unsigned int, unsigned int, unsigned int);
-
-#endif        // PCL_EXTRACT_CLUSTERS_IMPL_H_
+#define PCL_INSTANTIATE_LabeledEuclideanClusterExtraction(T)                           \
+  template class PCL_EXPORTS pcl::LabeledEuclideanClusterExtraction<T>;
+#define PCL_INSTANTIATE_extractLabeledEuclideanClusters_deprecated(T)                  \
+  template void PCL_EXPORTS pcl::extractLabeledEuclideanClusters<T>(                   \
+      const pcl::PointCloud<T>&,                                                       \
+      const typename pcl::search::Search<T>::Ptr&,                                     \
+      float,                                                                           \
+      std::vector<std::vector<pcl::PointIndices>>&,                                    \
+      unsigned int,                                                                    \
+      unsigned int,                                                                    \
+      unsigned int);
+#define PCL_INSTANTIATE_extractLabeledEuclideanClusters(T)                             \
+  template void PCL_EXPORTS pcl::extractLabeledEuclideanClusters<T>(                   \
+      const pcl::PointCloud<T>&,                                                       \
+      const typename pcl::search::Search<T>::Ptr&,                                     \
+      float,                                                                           \
+      std::vector<std::vector<pcl::PointIndices>>&,                                    \
+      unsigned int,                                                                    \
+      unsigned int);
+
+#endif // PCL_EXTRACT_CLUSTERS_IMPL_H_
diff --git a/segmentation/include/pcl/segmentation/impl/extract_polygonal_prism_data.hpp b/segmentation/include/pcl/segmentation/impl/extract_polygonal_prism_data.hpp
index 87757104..26392cdd 100644
--- a/segmentation/include/pcl/segmentation/impl/extract_polygonal_prism_data.hpp
+++ b/segmentation/include/pcl/segmentation/impl/extract_polygonal_prism_data.hpp
@@ -39,6 +39,7 @@
 #define PCL_SEGMENTATION_IMPL_EXTRACT_POLYGONAL_PRISM_DATA_H_
 
 #include <pcl/segmentation/extract_polygonal_prism_data.h>
+#include <pcl/sample_consensus/sac_model_plane.h> // for SampleConsensusModelPlane
 #include <pcl/common/centroid.h>
 #include <pcl/common/eigen.h>
 
@@ -85,7 +86,7 @@ pcl::isPointIn2DPolygon (const PointT &point, const pcl::PointCloud<PointT> &pol
   k2 = (k0 + 2) % 3;
   // Project the convex hull
   pcl::PointCloud<PointT> xy_polygon;
-  xy_polygon.points.resize (polygon.size ());
+  xy_polygon.resize (polygon.size ());
   for (std::size_t i = 0; i < polygon.size (); ++i)
   {
     Eigen::Vector4f pt (polygon[i].x, polygon[i].y, polygon[i].z, 0);
@@ -214,7 +215,7 @@ pcl::ExtractPolygonalPrismData<PointT>::segment (pcl::PointIndices &output)
   k2 = (k0 + 2) % 3;
   // Project the convex hull
   pcl::PointCloud<PointT> polygon;
-  polygon.points.resize (planar_hull_->size ());
+  polygon.resize (planar_hull_->size ());
   for (std::size_t i = 0; i < planar_hull_->size (); ++i)
   {
     Eigen::Vector4f pt ((*planar_hull_)[i].x, (*planar_hull_)[i].y, (*planar_hull_)[i].z, 0);
diff --git a/segmentation/include/pcl/segmentation/impl/grabcut_segmentation.hpp b/segmentation/include/pcl/segmentation/impl/grabcut_segmentation.hpp
index d3787656..177b599d 100644
--- a/segmentation/include/pcl/segmentation/impl/grabcut_segmentation.hpp
+++ b/segmentation/include/pcl/segmentation/impl/grabcut_segmentation.hpp
@@ -38,6 +38,7 @@
 #pragma once
 
 #include <pcl/common/distances.h>
+#include <pcl/common/io.h> // for getFieldIndex
 #include <pcl/common/point_tests.h> // for pcl::isFinite
 #include <pcl/search/organized.h>
 #include <pcl/search/kdtree.h>
@@ -88,7 +89,7 @@ GrabCut<PointT>::initCompute ()
   using namespace pcl::segmentation::grabcut;
   if (!pcl::PCLBase<PointT>::initCompute ())
   {
-    PCL_ERROR ("[pcl::GrabCut::initCompute ()] Init failed!");
+    PCL_ERROR ("[pcl::GrabCut::initCompute ()] Init failed!\n");
     return (false);
   }
 
@@ -96,7 +97,7 @@ GrabCut<PointT>::initCompute ()
   if ((pcl::getFieldIndex<PointT> ("rgb", in_fields_) == -1) &&
       (pcl::getFieldIndex<PointT> ("rgba", in_fields_) == -1))
   {
-    PCL_ERROR ("[pcl::GrabCut::initCompute ()] No RGB data available, aborting!");
+    PCL_ERROR ("[pcl::GrabCut::initCompute ()] No RGB data available, aborting!\n");
     return (false);
   }
 
@@ -167,7 +168,7 @@ GrabCut<PointT>::setBackgroundPointsIndices (const PointIndicesConstPtr &indices
 
   std::fill (trimap_.begin (), trimap_.end (), TrimapBackground);
   std::fill (hard_segmentation_.begin (), hard_segmentation_.end (), SegmentationBackground);
-  for (const int &index : indices->indices)
+  for (const auto &index : indices->indices)
   {
     trimap_[index] = TrimapUnknown;
     hard_segmentation_[index] = SegmentationForeground;
@@ -251,16 +252,16 @@ template <typename PointT> void
 GrabCut<PointT>::setTrimap (const PointIndicesConstPtr &indices, segmentation::grabcut::TrimapValue t)
 {
   using namespace pcl::segmentation::grabcut;
-  for (const int &index : indices->indices)
+  for (const auto &index : indices->indices)
     trimap_[index] = t;
 
   // Immediately set the hard segmentation as well so that the display will update.
   if (t == TrimapForeground)
-    for (const int &index : indices->indices)
+    for (const auto &index : indices->indices)
       hard_segmentation_[index] = SegmentationForeground;
   else
     if (t == TrimapBackground)
-      for (const int &index : indices->indices)
+      for (const auto &index : indices->indices)
         hard_segmentation_[index] = SegmentationBackground;
 }
 
@@ -316,11 +317,11 @@ GrabCut<PointT>::initGraph ()
     const NLinks &n_link = n_links_[i_point];
     if (n_link.nb_links > 0)
     {
-      int point_index = (*indices_) [i_point];
+      const auto point_index = (*indices_) [i_point];
       std::vector<float>::const_iterator weights_it  = n_link.weights.begin ();
       for (auto indices_it = n_link.indices.cbegin (); indices_it != n_link.indices.cend (); ++indices_it, ++weights_it)
       {
-        if ((*indices_it != point_index) && (*indices_it > -1))
+        if ((*indices_it != point_index) && (*indices_it != UNAVAILABLE))
         {
           addEdge (graph_nodes_[i_point], graph_nodes_[*indices_it], *weights_it, *weights_it);
         }
@@ -338,7 +339,7 @@ GrabCut<PointT>::computeNLinksNonOrganized ()
     NLinks &n_link = n_links_[i_point];
     if (n_link.nb_links > 0)
     {
-      int point_index = (*indices_) [i_point];
+      const auto point_index = (*indices_) [i_point];
       auto dists_it = n_link.dists.cbegin ();
       auto weights_it = n_link.weights.begin ();
       for (auto indices_it = n_link.indices.cbegin (); indices_it != n_link.indices.cend (); ++indices_it, ++dists_it, ++weights_it)
@@ -392,7 +393,7 @@ GrabCut<PointT>::computeBetaNonOrganized ()
 
   for (int i_point = 0; i_point < number_of_indices; i_point++)
   {
-    int point_index = (*indices_)[i_point];
+    const auto point_index = (*indices_)[i_point];
     const PointT& point = input_->points [point_index];
     if (pcl::isFinite (point))
     {
@@ -402,11 +403,11 @@ GrabCut<PointT>::computeBetaNonOrganized ()
       {
         links.nb_links = found - 1;
         links.weights.reserve (links.nb_links);
-        for (std::vector<int>::const_iterator nn_it = links.indices.begin (); nn_it != links.indices.end (); ++nn_it)
+        for (const auto& nn_index : links.indices)
         {
-          if (*nn_it != point_index)
+          if (nn_index != point_index)
           {
-            float color_distance = squaredEuclideanDistance ((*image_)[point_index], (*image_)[*nn_it]);
+            float color_distance = squaredEuclideanDistance ((*image_)[point_index], (*image_)[nn_index]);
             links.weights.push_back (color_distance);
             result+= color_distance;
             ++edges;
diff --git a/segmentation/include/pcl/segmentation/impl/min_cut_segmentation.hpp b/segmentation/include/pcl/segmentation/impl/min_cut_segmentation.hpp
index ac4c7c42..6d0b85ef 100644
--- a/segmentation/include/pcl/segmentation/impl/min_cut_segmentation.hpp
+++ b/segmentation/include/pcl/segmentation/impl/min_cut_segmentation.hpp
@@ -39,11 +39,10 @@
 #ifndef PCL_SEGMENTATION_MIN_CUT_SEGMENTATION_HPP_
 #define PCL_SEGMENTATION_MIN_CUT_SEGMENTATION_HPP_
 
-#include <pcl/segmentation/boost.h>
+#include <boost/graph/boykov_kolmogorov_max_flow.hpp> // for boykov_kolmogorov_max_flow
 #include <pcl/segmentation/min_cut_segmentation.h>
 #include <pcl/search/search.h>
 #include <pcl/search/kdtree.h>
-#include <cstdlib>
 #include <cmath>
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -334,9 +333,8 @@ pcl::MinCutSegmentation<PointT>::buildGraph ()
   source_ = vertices_[number_of_points];
   sink_ = vertices_[number_of_points + 1];
 
-  for (std::size_t i_point = 0; i_point < number_of_indices; i_point++)
+  for (const auto& point_index : (*indices_))
   {
-    index_t point_index = (*indices_)[i_point];
     double source_weight = 0.0;
     double sink_weight = 0.0;
     calculateUnaryPotential (point_index, source_weight, sink_weight);
@@ -344,7 +342,7 @@ pcl::MinCutSegmentation<PointT>::buildGraph ()
     addEdge (point_index, static_cast<int> (sink_), sink_weight);
   }
 
-  std::vector<int> neighbours;
+  pcl::Indices neighbours;
   std::vector<float> distances;
   search_->setInputCloud (input_, indices_);
   for (std::size_t i_point = 0; i_point < number_of_indices; i_point++)
@@ -376,11 +374,11 @@ pcl::MinCutSegmentation<PointT>::calculateUnaryPotential (int point, double& sou
   initial_point[0] = (*input_)[point].x;
   initial_point[1] = (*input_)[point].y;
 
-  for (std::size_t i_point = 0; i_point < foreground_points_.size (); i_point++)
+  for (const auto& fg_point : foreground_points_)
   {
     double dist = 0.0;
-    dist += (foreground_points_[i_point].x - initial_point[0]) * (foreground_points_[i_point].x - initial_point[0]);
-    dist += (foreground_points_[i_point].y - initial_point[1]) * (foreground_points_[i_point].y - initial_point[1]);
+    dist += (fg_point.x - initial_point[0]) * (fg_point.x - initial_point[0]);
+    dist += (fg_point.y - initial_point[1]) * (fg_point.y - initial_point[1]);
     if (min_dist_to_foreground > dist)
     {
       min_dist_to_foreground = dist;
@@ -395,16 +393,16 @@ pcl::MinCutSegmentation<PointT>::calculateUnaryPotential (int point, double& sou
   if (background_points_.size () == 0)
     return;
 
-  for (int i_point = 0; i_point < background_points_.size (); i_point++)
+  for (const auto& bg_point : background_points_)
   {
     double dist = 0.0;
-    dist += (background_points_[i_point].x - initial_point[0]) * (background_points_[i_point].x - initial_point[0]);
-    dist += (background_points_[i_point].y - initial_point[1]) * (background_points_[i_point].y - initial_point[1]);
+    dist += (bg_point.x - initial_point[0]) * (bg_point.x - initial_point[0]);
+    dist += (bg_point.y - initial_point[1]) * (bg_point.y - initial_point[1]);
     if (min_dist_to_background > dist)
     {
       min_dist_to_background = dist;
-      closest_background_point[0] = background_points_[i_point].x;
-      closest_background_point[1] = background_points_[i_point].y;
+      closest_background_point[0] = bg_point.x;
+      closest_background_point[1] = bg_point.y;
     }
   }
 
@@ -490,8 +488,6 @@ pcl::MinCutSegmentation<PointT>::recalculateUnaryPotentials ()
 template <typename PointT> bool
 pcl::MinCutSegmentation<PointT>::recalculateBinaryPotentials ()
 {
-  int number_of_points = static_cast<int> (indices_->size ());
-
   VertexIterator vertex_iter;
   VertexIterator vertex_end;
   OutEdgeIterator edge_iter;
@@ -500,7 +496,7 @@ pcl::MinCutSegmentation<PointT>::recalculateBinaryPotentials ()
   std::vector< std::set<VertexDescriptor> > edge_marker;
   std::set<VertexDescriptor> out_edges_marker;
   edge_marker.clear ();
-  edge_marker.resize (number_of_points + 2, out_edges_marker);
+  edge_marker.resize (indices_->size () + 2, out_edges_marker);
 
   for (boost::tie (vertex_iter, vertex_end) = boost::vertices (*graph_); vertex_iter != vertex_end; vertex_iter++)
   {
@@ -539,9 +535,8 @@ pcl::MinCutSegmentation<PointT>::assembleLabels (ResidualCapacityMap& residual_c
 {
   std::vector<int> labels;
   labels.resize (input_->size (), 0);
-  int number_of_indices = static_cast<int> (indices_->size ());
-  for (int i_point = 0; i_point < number_of_indices; i_point++)
-    labels[(*indices_)[i_point]] = 1;
+  for (const auto& i_point : (*indices_))
+    labels[i_point] = 1;
 
   clusters_.clear ();
 
@@ -569,20 +564,16 @@ pcl::MinCutSegmentation<PointT>::getColoredCloud ()
 
   if (!clusters_.empty ())
   {
-    int num_of_pts_in_first_cluster = static_cast<int> (clusters_[0].indices.size ());
-    int num_of_pts_in_second_cluster = static_cast<int> (clusters_[1].indices.size ());
-    int number_of_points = num_of_pts_in_first_cluster + num_of_pts_in_second_cluster;
     colored_cloud = (new pcl::PointCloud<pcl::PointXYZRGB>)->makeShared ();
     unsigned char foreground_color[3] = {255, 255, 255};
     unsigned char background_color[3] = {255, 0, 0};
-    colored_cloud->width = number_of_points;
+    colored_cloud->width = (clusters_[0].indices.size () + clusters_[1].indices.size ());
     colored_cloud->height = 1;
     colored_cloud->is_dense = input_->is_dense;
 
     pcl::PointXYZRGB point;
-    for (int i_point = 0; i_point < num_of_pts_in_first_cluster; i_point++)
+    for (const auto& point_index : (clusters_[0].indices))
     {
-      index_t point_index = clusters_[0].indices[i_point];
       point.x = *((*input_)[point_index].data);
       point.y = *((*input_)[point_index].data + 1);
       point.z = *((*input_)[point_index].data + 2);
@@ -592,9 +583,8 @@ pcl::MinCutSegmentation<PointT>::getColoredCloud ()
       colored_cloud->points.push_back (point);
     }
 
-    for (int i_point = 0; i_point < num_of_pts_in_second_cluster; i_point++)
+    for (const auto& point_index : (clusters_[1].indices))
     {
-      index_t point_index = clusters_[1].indices[i_point];
       point.x = *((*input_)[point_index].data);
       point.y = *((*input_)[point_index].data + 1);
       point.z = *((*input_)[point_index].data + 2);
diff --git a/segmentation/include/pcl/segmentation/impl/organized_connected_component_segmentation.hpp b/segmentation/include/pcl/segmentation/impl/organized_connected_component_segmentation.hpp
index f9112378..cdba4b46 100644
--- a/segmentation/include/pcl/segmentation/impl/organized_connected_component_segmentation.hpp
+++ b/segmentation/include/pcl/segmentation/impl/organized_connected_component_segmentation.hpp
@@ -122,7 +122,7 @@ pcl::OrganizedConnectedComponentSegmentation<PointT, PointLT>::segment (pcl::Poi
   unsigned invalid_label = std::numeric_limits<unsigned>::max ();
   PointLT invalid_pt;
   invalid_pt.label = std::numeric_limits<unsigned>::max ();
-  labels.points.resize (input_->size (), invalid_pt);
+  labels.resize (input_->size (), invalid_pt);
   labels.width = input_->width;
   labels.height = input_->height;
   std::size_t clust_id = 0;
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 37d241cd..ef52cbe7 100644
--- a/segmentation/include/pcl/segmentation/impl/organized_multi_plane_segmentation.hpp
+++ b/segmentation/include/pcl/segmentation/impl/organized_multi_plane_segmentation.hpp
@@ -40,7 +40,6 @@
 #ifndef PCL_SEGMENTATION_IMPL_ORGANIZED_MULTI_PLANE_SEGMENTATION_H_
 #define PCL_SEGMENTATION_IMPL_ORGANIZED_MULTI_PLANE_SEGMENTATION_H_
 
-#include <pcl/segmentation/boost.h>
 #include <pcl/segmentation/organized_connected_component_segmentation.h>
 #include <pcl/segmentation/organized_multi_plane_segmentation.h>
 #include <pcl/common/centroid.h>
@@ -212,7 +211,7 @@ pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segment (std::ve
   {
     boundary_cloud.resize (0);
     pcl::OrganizedConnectedComponentSegmentation<PointT,PointLT>::findLabeledRegionBoundary (inlier_indices[i].indices[0], labels, boundary_indices[i]);
-    boundary_cloud.points.resize (boundary_indices[i].indices.size ());
+    boundary_cloud.resize (boundary_indices[i].indices.size ());
     for (std::size_t j = 0; j < boundary_indices[i].indices.size (); j++)
       boundary_cloud[j] = (*input_)[boundary_indices[i].indices[j]];
     
@@ -251,7 +250,7 @@ pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segmentAndRefine
     boundary_cloud.resize (0);
     int max_inlier_idx = static_cast<int> (inlier_indices[i].indices.size ()) - 1;
     pcl::OrganizedConnectedComponentSegmentation<PointT,PointLT>::findLabeledRegionBoundary (inlier_indices[i].indices[max_inlier_idx], labels, boundary_indices[i]);
-    boundary_cloud.points.resize (boundary_indices[i].indices.size ());
+    boundary_cloud.resize (boundary_indices[i].indices.size ());
     for (std::size_t j = 0; j < boundary_indices[i].indices.size (); j++)
       boundary_cloud[j] = (*input_)[boundary_indices[i].indices[j]];
     
@@ -295,7 +294,7 @@ pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segmentAndRefine
     boundary_cloud.resize (0);
     int max_inlier_idx = static_cast<int> (inlier_indices[i].indices.size ()) - 1;
     pcl::OrganizedConnectedComponentSegmentation<PointT,PointLT>::findLabeledRegionBoundary (inlier_indices[i].indices[max_inlier_idx], labels, boundary_indices[i]);
-    boundary_cloud.points.resize (boundary_indices[i].indices.size ());
+    boundary_cloud.resize (boundary_indices[i].indices.size ());
     for (std::size_t j = 0; j < boundary_indices[i].indices.size (); j++)
       boundary_cloud[j] = (*input_)[boundary_indices[i].indices[j]];
 
@@ -306,7 +305,7 @@ pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segmentAndRefine
                                              model_coefficients[i].values[3]);
 
     Eigen::Vector3f vp (0.0, 0.0, 0.0);
-    if (project_points_ && !boundary_cloud.points.empty ())
+    if (project_points_ && !boundary_cloud.empty ())
       boundary_cloud = projectToPlaneFromViewpoint (boundary_cloud, model, centroid, vp);
 
     regions[i] = PlanarRegion<PointT> (centroid,
diff --git a/segmentation/include/pcl/segmentation/impl/progressive_morphological_filter.hpp b/segmentation/include/pcl/segmentation/impl/progressive_morphological_filter.hpp
index c8d68ff1..d930431a 100644
--- a/segmentation/include/pcl/segmentation/impl/progressive_morphological_filter.hpp
+++ b/segmentation/include/pcl/segmentation/impl/progressive_morphological_filter.hpp
@@ -42,7 +42,6 @@
 #include <pcl/common/common.h>
 #include <pcl/common/io.h>
 #include <pcl/filters/morphological_filter.h>
-#include <pcl/filters/extract_indices.h>
 #include <pcl/segmentation/progressive_morphological_filter.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -68,7 +67,7 @@ pcl::ProgressiveMorphologicalFilter<PointT>::~ProgressiveMorphologicalFilter ()
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
-pcl::ProgressiveMorphologicalFilter<PointT>::extract (std::vector<int>& ground)
+pcl::ProgressiveMorphologicalFilter<PointT>::extract (Indices& ground)
 {
   bool segmentation_is_possible = initCompute ();
   if (!segmentation_is_possible)
@@ -129,7 +128,7 @@ pcl::ProgressiveMorphologicalFilter<PointT>::extract (std::vector<int>& ground)
 
     // Find indices of the points whose difference between the source and
     // filtered point clouds is less than the current height threshold.
-    std::vector<int> pt_indices;
+    Indices pt_indices;
     for (std::size_t p_idx = 0; p_idx < ground.size (); ++p_idx)
     {
       float diff = (*cloud)[p_idx].z - (*cloud_f)[p_idx].z;
diff --git a/segmentation/include/pcl/segmentation/impl/region_growing.hpp b/segmentation/include/pcl/segmentation/impl/region_growing.hpp
index 10f8ccfe..237bc660 100644
--- a/segmentation/include/pcl/segmentation/impl/region_growing.hpp
+++ b/segmentation/include/pcl/segmentation/impl/region_growing.hpp
@@ -44,11 +44,11 @@
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl/common/point_tests.h> // for pcl::isFinite
+#include <pcl/console/print.h> // for PCL_ERROR
 #include <pcl/search/search.h>
 #include <pcl/search/kdtree.h>
 
 #include <queue>
-#include <list>
 #include <cmath>
 #include <ctime>
 
@@ -56,7 +56,7 @@
 template <typename PointT, typename NormalT>
 pcl::RegionGrowing<PointT, NormalT>::RegionGrowing () :
   min_pts_per_cluster_ (1),
-  max_pts_per_cluster_ (std::numeric_limits<int>::max ()),
+  max_pts_per_cluster_ (std::numeric_limits<pcl::uindex_t>::max ()),
   smooth_mode_flag_ (true),
   curvature_flag_ (true),
   residual_flag_ (false),
@@ -86,7 +86,7 @@ pcl::RegionGrowing<PointT, NormalT>::~RegionGrowing ()
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT, typename NormalT> int
+template <typename PointT, typename NormalT> pcl::uindex_t
 pcl::RegionGrowing<PointT, NormalT>::getMinClusterSize ()
 {
   return (min_pts_per_cluster_);
@@ -94,13 +94,13 @@ pcl::RegionGrowing<PointT, NormalT>::getMinClusterSize ()
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT, typename NormalT> void
-pcl::RegionGrowing<PointT, NormalT>::setMinClusterSize (int min_cluster_size)
+pcl::RegionGrowing<PointT, NormalT>::setMinClusterSize (pcl::uindex_t min_cluster_size)
 {
   min_pts_per_cluster_ = min_cluster_size;
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT, typename NormalT> int
+template <typename PointT, typename NormalT> pcl::uindex_t
 pcl::RegionGrowing<PointT, NormalT>::getMaxClusterSize ()
 {
   return (max_pts_per_cluster_);
@@ -108,7 +108,7 @@ pcl::RegionGrowing<PointT, NormalT>::getMaxClusterSize ()
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT, typename NormalT> void
-pcl::RegionGrowing<PointT, NormalT>::setMaxClusterSize (int max_cluster_size)
+pcl::RegionGrowing<PointT, NormalT>::setMaxClusterSize (pcl::uindex_t max_cluster_size)
 {
   max_pts_per_cluster_ = max_cluster_size;
 }
@@ -276,12 +276,12 @@ pcl::RegionGrowing<PointT, NormalT>::extract (std::vector <pcl::PointIndices>& c
 
   clusters.resize (clusters_.size ());
   std::vector<pcl::PointIndices>::iterator cluster_iter_input = clusters.begin ();
-  for (std::vector<pcl::PointIndices>::const_iterator cluster_iter = clusters_.begin (); cluster_iter != clusters_.end (); ++cluster_iter)
+  for (const auto& cluster : clusters_)
   {
-    if ((static_cast<int> (cluster_iter->indices.size ()) >= min_pts_per_cluster_) &&
-        (static_cast<int> (cluster_iter->indices.size ()) <= max_pts_per_cluster_))
+    if ((cluster.indices.size () >= min_pts_per_cluster_) &&
+        (cluster.indices.size () <= max_pts_per_cluster_))
     {
-      *cluster_iter_input = *cluster_iter;
+      *cluster_iter_input = cluster;
       ++cluster_iter_input;
     }
   }
@@ -343,7 +343,7 @@ template <typename PointT, typename NormalT> void
 pcl::RegionGrowing<PointT, NormalT>::findPointNeighbours ()
 {
   int point_number = static_cast<int> (indices_->size ());
-  std::vector<int> neighbours;
+  pcl::Indices neighbours;
   std::vector<float> distances;
 
   point_neighbours_.resize (input_->size (), neighbours);
@@ -480,7 +480,7 @@ pcl::RegionGrowing<PointT, NormalT>::growRegion (int initial_seed, int segment_n
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT, typename NormalT> bool
-pcl::RegionGrowing<PointT, NormalT>::validatePoint (int initial_seed, int point, int nghbr, bool& is_a_seed) const
+pcl::RegionGrowing<PointT, NormalT>::validatePoint (pcl::index_t initial_seed, pcl::index_t point, pcl::index_t nghbr, bool& is_a_seed) const
 {
   is_a_seed = true;
 
@@ -567,7 +567,7 @@ pcl::RegionGrowing<PointT, NormalT>::assembleRegions ()
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT, typename NormalT> void
-pcl::RegionGrowing<PointT, NormalT>::getSegmentFromPoint (int index, pcl::PointIndices& cluster)
+pcl::RegionGrowing<PointT, NormalT>::getSegmentFromPoint (pcl::index_t index, pcl::PointIndices& cluster)
 {
   cluster.indices.clear ();
 
@@ -580,9 +580,8 @@ pcl::RegionGrowing<PointT, NormalT>::getSegmentFromPoint (int index, pcl::PointI
 
   // first of all we need to find out if this point belongs to cloud
   bool point_was_found = false;
-  int number_of_points = static_cast <int> (indices_->size ());
-  for (int point = 0; point < number_of_points; point++)
-    if ( (*indices_)[point] == index)
+  for (const auto& point : (*indices_))
+    if (point == index)
     {
       point_was_found = true;
       break;
@@ -610,22 +609,15 @@ pcl::RegionGrowing<PointT, NormalT>::getSegmentFromPoint (int index, pcl::PointI
     }
     // if we have already made the segmentation, then find the segment
     // to which this point belongs
-    for (auto i_segment = clusters_.cbegin (); i_segment != clusters_.cend (); i_segment++)
+    for (const auto& i_segment : clusters_)
     {
-      bool segment_was_found = false;
-      for (std::size_t i_point = 0; i_point < i_segment->indices.size (); i_point++)
-      {
-        if (i_segment->indices[i_point] == index)
-        {
-          segment_was_found = true;
-          cluster.indices.clear ();
-          cluster.indices.reserve (i_segment->indices.size ());
-          std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.indices));
-          break;
-        }
-      }
-      if (segment_was_found)
+      const auto it = std::find (i_segment.indices.cbegin (), i_segment.indices.cend (), index);
+      if (it != i_segment.indices.cend())
       {
+        // if segment was found
+        cluster.indices.clear ();
+        cluster.indices.reserve (i_segment.indices.size ());
+        std::copy (i_segment.indices.begin (), i_segment.indices.end (), std::back_inserter (cluster.indices));
         break;
       }
     }// next segment
@@ -669,12 +661,10 @@ pcl::RegionGrowing<PointT, NormalT>::getColoredCloud ()
     }
 
     int next_color = 0;
-    for (auto i_segment = clusters_.cbegin (); i_segment != clusters_.cend (); i_segment++)
+    for (const auto& i_segment : clusters_)
     {
-      for (auto i_point = i_segment->indices.cbegin (); i_point != i_segment->indices.cend (); i_point++)
+      for (const auto& index : (i_segment.indices))
       {
-        int index;
-        index = *i_point;
         (*colored_cloud)[index].r = colors[3 * next_color];
         (*colored_cloud)[index].g = colors[3 * next_color + 1];
         (*colored_cloud)[index].b = colors[3 * next_color + 2];
@@ -722,11 +712,10 @@ pcl::RegionGrowing<PointT, NormalT>::getColoredCloudRGBA ()
     }
 
     int next_color = 0;
-    for (auto i_segment = clusters_.cbegin (); i_segment != clusters_.cend (); i_segment++)
+    for (const auto& i_segment : clusters_)
     {
-      for (auto i_point = i_segment->indices.cbegin (); i_point != i_segment->indices.cend (); i_point++)
+      for (const auto& index : (i_segment.indices))
       {
-        int index = *i_point;
         (*colored_cloud)[index].r = colors[3 * next_color];
         (*colored_cloud)[index].g = colors[3 * next_color + 1];
         (*colored_cloud)[index].b = colors[3 * next_color + 2];
diff --git a/segmentation/include/pcl/segmentation/impl/region_growing_rgb.hpp b/segmentation/include/pcl/segmentation/impl/region_growing_rgb.hpp
index 81be5c5c..6bb122d2 100644
--- a/segmentation/include/pcl/segmentation/impl/region_growing_rgb.hpp
+++ b/segmentation/include/pcl/segmentation/impl/region_growing_rgb.hpp
@@ -40,6 +40,7 @@
 #ifndef PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_
 #define PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_
 
+#include <pcl/console/print.h> // for PCL_ERROR
 #include <pcl/segmentation/region_growing_rgb.h>
 #include <pcl/search/search.h>
 #include <pcl/search/kdtree.h>
@@ -197,8 +198,8 @@ pcl::RegionGrowingRGB<PointT, NormalT>::extract (std::vector <pcl::PointIndices>
   std::vector<pcl::PointIndices>::iterator cluster_iter = clusters_.begin ();
   while (cluster_iter != clusters_.end ())
   {
-    if (static_cast<int> (cluster_iter->indices.size ()) < min_pts_per_cluster_ ||
-        static_cast<int> (cluster_iter->indices.size ()) > max_pts_per_cluster_)
+    if (cluster_iter->indices.size () < min_pts_per_cluster_ ||
+        cluster_iter->indices.size () > max_pts_per_cluster_)
     {
       cluster_iter = clusters_.erase (cluster_iter);
     }
@@ -272,7 +273,7 @@ template <typename PointT, typename NormalT> void
 pcl::RegionGrowingRGB<PointT, NormalT>::findPointNeighbours ()
 {
   int point_number = static_cast<int> (indices_->size ());
-  std::vector<int> neighbours;
+  pcl::Indices neighbours;
   std::vector<float> distances;
 
   point_neighbours_.resize (input_->size (), neighbours);
@@ -293,14 +294,14 @@ pcl::RegionGrowingRGB<PointT, NormalT>::findPointNeighbours ()
 template <typename PointT, typename NormalT> void
 pcl::RegionGrowingRGB<PointT, NormalT>::findSegmentNeighbours ()
 {
-  std::vector<int> neighbours;
+  pcl::Indices neighbours;
   std::vector<float> distances;
   segment_neighbours_.resize (number_of_segments_, neighbours);
   segment_distances_.resize (number_of_segments_, distances);
 
   for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
   {
-    std::vector<int> nghbrs;
+    pcl::Indices nghbrs;
     std::vector<float> dist;
     findRegionsKNN (i_seg, region_neighbour_number_, nghbrs, dist);
     segment_neighbours_[i_seg].swap (nghbrs);
@@ -310,25 +311,24 @@ pcl::RegionGrowingRGB<PointT, NormalT>::findSegmentNeighbours ()
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT,typename NormalT> void
-pcl::RegionGrowingRGB<PointT, NormalT>::findRegionsKNN (int index, int nghbr_number, std::vector<int>& nghbrs, std::vector<float>& dist)
+pcl::RegionGrowingRGB<PointT, NormalT>::findRegionsKNN (pcl::index_t index, pcl::uindex_t nghbr_number, pcl::Indices& nghbrs, std::vector<float>& dist)
 {
   std::vector<float> distances;
   float max_dist = std::numeric_limits<float>::max ();
   distances.resize (clusters_.size (), max_dist);
 
-  int number_of_points = num_pts_in_segment_[index];
+  const auto number_of_points = num_pts_in_segment_[index];
   //loop through every point in this segment and check neighbours
-  for (int i_point = 0; i_point < number_of_points; i_point++)
+  for (pcl::uindex_t i_point = 0; i_point < number_of_points; i_point++)
   {
-    int point_index = clusters_[index].indices[i_point];
-    int number_of_neighbours = static_cast<int> (point_neighbours_[point_index].size ());
+    const auto point_index = clusters_[index].indices[i_point];
+    const auto number_of_neighbours = point_neighbours_[point_index].size ();
     //loop through every neighbour of the current point, find out to which segment it belongs
     //and if it belongs to neighbouring segment and is close enough then remember segment and its distance
-    for (int i_nghbr = 0; i_nghbr < number_of_neighbours; i_nghbr++)
+    for (std::size_t i_nghbr = 0; i_nghbr < number_of_neighbours; i_nghbr++)
     {
       // find segment
-      int segment_index = -1;
-      segment_index = point_labels_[ point_neighbours_[point_index][i_nghbr] ];
+      const pcl::index_t segment_index = point_labels_[ point_neighbours_[point_index][i_nghbr] ];
 
       if ( segment_index != index )
       {
@@ -345,15 +345,15 @@ pcl::RegionGrowingRGB<PointT, NormalT>::findRegionsKNN (int index, int nghbr_num
     if (distances[i_seg] < max_dist)
     {
       segment_neighbours.push (std::make_pair (distances[i_seg], i_seg) );
-      if (int (segment_neighbours.size ()) > nghbr_number)
+      if (segment_neighbours.size () > nghbr_number)
         segment_neighbours.pop ();
     }
   }
 
-  int size = std::min<int> (static_cast<int> (segment_neighbours.size ()), nghbr_number);
+  const std::size_t size = std::min<std::size_t> (segment_neighbours.size (), static_cast<std::size_t>(nghbr_number));
   nghbrs.resize (size, 0);
   dist.resize (size, 0);
-  int counter = 0;
+  pcl::uindex_t counter = 0;
   while ( !segment_neighbours.empty () && counter < nghbr_number )
   {
     dist[counter] = segment_neighbours.top ().first;
@@ -367,17 +367,14 @@ pcl::RegionGrowingRGB<PointT, NormalT>::findRegionsKNN (int index, int nghbr_num
 template <typename PointT, typename NormalT> void
 pcl::RegionGrowingRGB<PointT, NormalT>::applyRegionMergingAlgorithm ()
 {
-  int number_of_points = static_cast<int> (indices_->size ());
-
   // calculate color of each segment
   std::vector< std::vector<unsigned int> > segment_color;
   std::vector<unsigned int> color;
   color.resize (3, 0);
   segment_color.resize (number_of_segments_, color);
 
-  for (int i_point = 0; i_point < number_of_points; i_point++)
+  for (const auto& point_index : (*indices_))
   {
-    int point_index = (*indices_)[i_point];
     int segment_index = point_labels_[point_index];
     segment_color[segment_index][0] += (*input_)[point_index].r;
     segment_color[segment_index][1] += (*input_)[point_index].g;
@@ -456,13 +453,13 @@ pcl::RegionGrowingRGB<PointT, NormalT>::applyRegionMergingAlgorithm ()
     counter[index] += 1;
   }
 
-  std::vector< std::vector< std::pair<float, int> > > region_neighbours;
+  std::vector< std::vector< std::pair<float, pcl::index_t> > > region_neighbours;
   findRegionNeighbours (region_neighbours, final_segments);
 
   int final_segment_number = homogeneous_region_number;
   for (int i_reg = 0; i_reg < homogeneous_region_number; i_reg++)
   {
-    if (static_cast<int> (num_pts_in_homogeneous_region[i_reg]) < min_pts_per_cluster_)
+    if (num_pts_in_homogeneous_region[i_reg] < min_pts_per_cluster_)
     {
       if ( region_neighbours[i_reg].empty () )
         continue;
@@ -484,24 +481,19 @@ pcl::RegionGrowingRGB<PointT, NormalT>::applyRegionMergingAlgorithm ()
       num_seg_in_homogeneous_region[i_reg] = 0;
       final_segment_number -= 1;
 
-      int nghbr_number = static_cast<int> (region_neighbours[reg_index].size ());
-      for (int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
+      for (auto& nghbr : region_neighbours[reg_index])
       {
-        if ( segment_labels_[ region_neighbours[reg_index][i_nghbr].second ] == reg_index )
+        if ( segment_labels_[ nghbr.second ] == reg_index )
         {
-          region_neighbours[reg_index][i_nghbr].first = std::numeric_limits<float>::max ();
-          region_neighbours[reg_index][i_nghbr].second = 0;
+          nghbr.first = std::numeric_limits<float>::max ();
+          nghbr.second = 0;
         }
       }
-      nghbr_number = static_cast<int> (region_neighbours[i_reg].size ());
-      for (int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
+      for (const auto& nghbr : region_neighbours[i_reg])
       {
-        if ( segment_labels_[ region_neighbours[i_reg][i_nghbr].second ] != reg_index )
+        if ( segment_labels_[ nghbr.second ] != reg_index )
         {
-          std::pair<float, int> pair;
-          pair.first = region_neighbours[i_reg][i_nghbr].first;
-          pair.second = region_neighbours[i_reg][i_nghbr].second;
-          region_neighbours[reg_index].push_back (pair);
+          region_neighbours[reg_index].push_back (nghbr);
         }
       }
       region_neighbours[i_reg].clear ();
@@ -527,7 +519,7 @@ pcl::RegionGrowingRGB<PointT, NormalT>::calculateColorimetricalDifference (std::
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT, typename NormalT> void
-pcl::RegionGrowingRGB<PointT, NormalT>::findRegionNeighbours (std::vector< std::vector< std::pair<float, int> > >& neighbours_out, std::vector< std::vector<int> >& regions_in)
+pcl::RegionGrowingRGB<PointT, NormalT>::findRegionNeighbours (std::vector< std::vector< std::pair<float, pcl::index_t> > >& neighbours_out, std::vector< std::vector<int> >& regions_in)
 {
   int region_number = static_cast<int> (regions_in.size ());
   neighbours_out.clear ();
@@ -535,16 +527,14 @@ pcl::RegionGrowingRGB<PointT, NormalT>::findRegionNeighbours (std::vector< std::
 
   for (int i_reg = 0; i_reg < region_number; i_reg++)
   {
-    int segment_num = static_cast<int> (regions_in[i_reg].size ());
-    neighbours_out[i_reg].reserve (segment_num * region_neighbour_number_);
-	for (int i_seg = 0; i_seg < segment_num; i_seg++)
+    neighbours_out[i_reg].reserve (regions_in[i_reg].size () * region_neighbour_number_);
+    for (const auto& curr_segment : regions_in[i_reg])
     {
-      int curr_segment = regions_in[i_reg][i_seg];
-      int nghbr_number = static_cast<int> (segment_neighbours_[curr_segment].size ());
-      std::pair<float, int> pair;
-      for (int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
+      const std::size_t nghbr_number = segment_neighbours_[curr_segment].size ();
+      std::pair<float, pcl::index_t> pair;
+      for (std::size_t i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
       {
-        int segment_index = segment_neighbours_[curr_segment][i_nghbr];
+        const auto segment_index = segment_neighbours_[curr_segment][i_nghbr];
         if ( segment_distances_[curr_segment][i_nghbr] == std::numeric_limits<float>::max () )
           continue;
         if (segment_labels_[segment_index] != i_reg)
@@ -573,10 +563,8 @@ pcl::RegionGrowingRGB<PointT, NormalT>::assembleRegions (std::vector<unsigned in
 
   std::vector<int> counter;
   counter.resize (num_regions, 0);
-  int point_number = static_cast<int> (indices_->size ());
-  for (int i_point = 0; i_point < point_number; i_point++)
+  for (const auto& point_index : (*indices_))
   {
-    int point_index = (*indices_)[i_point];
     int index = point_labels_[point_index];
     index = segment_labels_[index];
     clusters_[index].indices[ counter[index] ] = point_index;
@@ -608,7 +596,7 @@ pcl::RegionGrowingRGB<PointT, NormalT>::assembleRegions (std::vector<unsigned in
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT, typename NormalT> bool
-pcl::RegionGrowingRGB<PointT, NormalT>::validatePoint (int initial_seed, int point, int nghbr, bool& is_a_seed) const
+pcl::RegionGrowingRGB<PointT, NormalT>::validatePoint (pcl::index_t initial_seed, pcl::index_t point, pcl::index_t nghbr, bool& is_a_seed) const
 {
   is_a_seed = true;
 
@@ -687,7 +675,7 @@ pcl::RegionGrowingRGB<PointT, NormalT>::validatePoint (int initial_seed, int poi
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT, typename NormalT> void
-pcl::RegionGrowingRGB<PointT, NormalT>::getSegmentFromPoint (int index, pcl::PointIndices& cluster)
+pcl::RegionGrowingRGB<PointT, NormalT>::getSegmentFromPoint (pcl::index_t index, pcl::PointIndices& cluster)
 {
   cluster.indices.clear ();
 
@@ -700,9 +688,8 @@ pcl::RegionGrowingRGB<PointT, NormalT>::getSegmentFromPoint (int index, pcl::Poi
 
   // first of all we need to find out if this point belongs to cloud
   bool point_was_found = false;
-  int number_of_points = static_cast <int> (indices_->size ());
-  for (int point = 0; point < number_of_points; point++)
-    if ( (*indices_)[point] == index)
+  for (const auto& point : (*indices_))
+    if (point == index)
     {
       point_was_found = true;
       break;
@@ -738,22 +725,15 @@ pcl::RegionGrowingRGB<PointT, NormalT>::getSegmentFromPoint (int index, pcl::Poi
     }
     // if we have already made the segmentation, then find the segment
     // to which this point belongs
-    for (auto i_segment = clusters_.cbegin (); i_segment != clusters_.cend (); i_segment++)
+    for (const auto& i_segment : clusters_)
     {
-      bool segment_was_found = false;
-      for (std::size_t i_point = 0; i_point < i_segment->indices.size (); i_point++)
-      {
-        if (i_segment->indices[i_point] == index)
-        {
-          segment_was_found = true;
-          cluster.indices.clear ();
-          cluster.indices.reserve (i_segment->indices.size ());
-          std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.indices));
-          break;
-        }
-      }
-      if (segment_was_found)
+      const auto it = std::find (i_segment.indices.cbegin (), i_segment.indices.cend (), index);
+      if (it != i_segment.indices.cend())
       {
+        // if segment was found
+        cluster.indices.clear ();
+        cluster.indices.reserve (i_segment.indices.size ());
+        std::copy (i_segment.indices.begin (), i_segment.indices.end (), std::back_inserter (cluster.indices));
         break;
       }
     }// next segment
diff --git a/segmentation/include/pcl/segmentation/impl/sac_segmentation.hpp b/segmentation/include/pcl/segmentation/impl/sac_segmentation.hpp
index c8b667a6..3dfeb52e 100644
--- a/segmentation/include/pcl/segmentation/impl/sac_segmentation.hpp
+++ b/segmentation/include/pcl/segmentation/impl/sac_segmentation.hpp
@@ -108,13 +108,13 @@ pcl::SACSegmentation<PointT>::segment (PointIndices &inliers, ModelCoefficients
   sac_->getInliers (inliers.indices);
 
   // Get the model coefficients
-  Eigen::VectorXf coeff;
+  Eigen::VectorXf coeff (model_->getModelSize ());
   sac_->getModelCoefficients (coeff);
 
   // If the user needs optimized coefficients
   if (optimize_coefficients_)
   {
-    Eigen::VectorXf coeff_refined;
+    Eigen::VectorXf coeff_refined (model_->getModelSize ());
     model_->optimizeModelCoefficients (inliers.indices, coeff, coeff_refined);
     model_coefficients.values.resize (coeff_refined.size ());
     memcpy (&model_coefficients.values[0], &coeff_refined[0], coeff_refined.size () * sizeof (float));
diff --git a/segmentation/include/pcl/segmentation/impl/seeded_hue_segmentation.hpp b/segmentation/include/pcl/segmentation/impl/seeded_hue_segmentation.hpp
index 8c2e7d31..3976c143 100644
--- a/segmentation/include/pcl/segmentation/impl/seeded_hue_segmentation.hpp
+++ b/segmentation/include/pcl/segmentation/impl/seeded_hue_segmentation.hpp
@@ -40,6 +40,9 @@
 #define PCL_SEGMENTATION_IMPL_SEEDED_HUE_SEGMENTATION_H_
 
 #include <pcl/segmentation/seeded_hue_segmentation.h>
+#include <pcl/console/print.h> // for PCL_ERROR
+#include <pcl/search/organized.h> // for OrganizedNeighbor
+#include <pcl/search/kdtree.h> // for KdTree
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
@@ -61,18 +64,18 @@ pcl::seededHueSegmentation (const PointCloud<PointXYZRGB>          &cloud,
   // Create a bool vector of processed point indices, and initialize it to false
   std::vector<bool> processed (cloud.size (), false);
 
-  std::vector<int> nn_indices;
+  Indices nn_indices;
   std::vector<float> nn_distances;
 
   // Process all points in the indices vector
-  for (const int &i : indices_in.indices)
+  for (const auto &i : indices_in.indices)
   {
     if (processed[i])
       continue;
 
     processed[i] = true;
 
-    std::vector<int> seed_queue;
+    Indices seed_queue;
     int sq_idx = 0;
     seed_queue.push_back (i);
 
@@ -85,7 +88,7 @@ pcl::seededHueSegmentation (const PointCloud<PointXYZRGB>          &cloud,
     {
       int ret = tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, std::numeric_limits<int>::max());
       if(ret == -1)
-        PCL_ERROR("[pcl::seededHueSegmentation] radiusSearch returned error code -1");
+        PCL_ERROR("[pcl::seededHueSegmentation] radiusSearch returned error code -1\n");
       // Search for sq_idx
       if (!ret)
       {
@@ -113,7 +116,7 @@ pcl::seededHueSegmentation (const PointCloud<PointXYZRGB>          &cloud,
       sq_idx++;
     }
     // Copy the seed queue into the output indices
-    for (const int &l : seed_queue)
+    for (const auto &l : seed_queue)
       indices_out.indices.push_back(l);
   }
   // This is purely esthetical, can be removed for speed purposes
@@ -139,18 +142,18 @@ pcl::seededHueSegmentation (const PointCloud<PointXYZRGB>            &cloud,
   // Create a bool vector of processed point indices, and initialize it to false
   std::vector<bool> processed (cloud.size (), false);
 
-  std::vector<int> nn_indices;
+  Indices nn_indices;
   std::vector<float> nn_distances;
 
   // Process all points in the indices vector
-  for (const int &i : indices_in.indices)
+  for (const auto &i : indices_in.indices)
   {
     if (processed[i])
       continue;
 
     processed[i] = true;
 
-    std::vector<int> seed_queue;
+    Indices seed_queue;
     int sq_idx = 0;
     seed_queue.push_back (i);
 
@@ -163,7 +166,7 @@ pcl::seededHueSegmentation (const PointCloud<PointXYZRGB>            &cloud,
     {
       int ret = tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, std::numeric_limits<int>::max());
       if(ret == -1)
-        PCL_ERROR("[pcl::seededHueSegmentation] radiusSearch returned error code -1");
+        PCL_ERROR("[pcl::seededHueSegmentation] radiusSearch returned error code -1\n");
       // Search for sq_idx
       if (!ret)
       {
@@ -190,7 +193,7 @@ pcl::seededHueSegmentation (const PointCloud<PointXYZRGB>            &cloud,
       sq_idx++;
     }
     // Copy the seed queue into the output indices
-    for (const int &l : seed_queue)
+    for (const auto &l : seed_queue)
       indices_out.indices.push_back(l);
   }
   // This is purely esthetical, can be removed for speed purposes
diff --git a/segmentation/include/pcl/segmentation/impl/segment_differences.hpp b/segmentation/include/pcl/segmentation/impl/segment_differences.hpp
index 973ac836..ad4de5e9 100755
--- a/segmentation/include/pcl/segmentation/impl/segment_differences.hpp
+++ b/segmentation/include/pcl/segmentation/impl/segment_differences.hpp
@@ -41,6 +41,8 @@
 
 #include <pcl/common/io.h>
 #include <pcl/common/point_tests.h> // for pcl::isFinite
+#include <pcl/search/organized.h> // for OrganizedNeighbor
+#include <pcl/search/kdtree.h> // for KdTree
 
 
 //////////////////////////////////////////////////////////////////////////
@@ -52,14 +54,14 @@ pcl::getPointCloudDifference (
     pcl::PointCloud<PointT> &output)
 {
   // We're interested in a single nearest neighbor only
-  std::vector<int> nn_indices (1);
+  Indices nn_indices (1);
   std::vector<float> nn_distances (1);
 
   // The input cloud indices that do not have a neighbor in the target cloud
-  std::vector<int> src_indices;
+  Indices src_indices;
 
   // Iterate through the source data set
-  for (int i = 0; i < static_cast<int> (src.size ()); ++i)
+  for (index_t i = 0; i < static_cast<index_t> (src.size ()); ++i)
   {
     // Ignore invalid points in the inpout cloud
     if (!isFinite (src[i]))
@@ -93,7 +95,7 @@ pcl::SegmentDifferences<PointT>::segment (PointCloud &output)
   if (!initCompute ()) 
   {
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
diff --git a/segmentation/include/pcl/segmentation/impl/supervoxel_clustering.hpp b/segmentation/include/pcl/segmentation/impl/supervoxel_clustering.hpp
index 18b9b966..86de624f 100644
--- a/segmentation/include/pcl/segmentation/impl/supervoxel_clustering.hpp
+++ b/segmentation/include/pcl/segmentation/impl/supervoxel_clustering.hpp
@@ -41,6 +41,7 @@
 #define PCL_SEGMENTATION_SUPERVOXEL_CLUSTERING_HPP_
 
 #include <pcl/segmentation/supervoxel_clustering.h>
+#include <pcl/common/io.h> // for copyPointCloud
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT>
@@ -115,7 +116,7 @@ pcl::SupervoxelClustering<PointT>::extract (std::map<std::uint32_t,typename Supe
   
   //double t_prep = timer_.getTime ();
   //std::cout << "Placing Seeds" << std::endl;
-  std::vector<int> seed_indices;
+  Indices seed_indices;
   selectInitialSupervoxelSeeds (seed_indices);
   //std::cout << "Creating helpers "<<std::endl;
   createSupervoxelHelpers (seed_indices);
@@ -259,7 +260,7 @@ pcl::SupervoxelClustering<PointT>::computeVoxelData ()
     {
       VoxelData& new_voxel_data = (*leaf_itr)->getData ();
       //For every point, get its neighbors, build an index vector, compute normal
-      std::vector<int> indices;
+      Indices indices;
       indices.reserve (81); 
       //Push this point
       indices.push_back (new_voxel_data.idx_);
@@ -340,7 +341,7 @@ pcl::SupervoxelClustering<PointT>::makeSupervoxels (std::map<std::uint32_t,typen
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
-pcl::SupervoxelClustering<PointT>::createSupervoxelHelpers (std::vector<int> &seed_indices)
+pcl::SupervoxelClustering<PointT>::createSupervoxelHelpers (Indices &seed_indices)
 {
   
   supervoxel_helpers_.clear ();
@@ -362,7 +363,7 @@ pcl::SupervoxelClustering<PointT>::createSupervoxelHelpers (std::vector<int> &se
 }
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
-pcl::SupervoxelClustering<PointT>::selectInitialSupervoxelSeeds (std::vector<int> &seed_indices)
+pcl::SupervoxelClustering<PointT>::selectInitialSupervoxelSeeds (Indices &seed_indices)
 {
   //TODO THIS IS BAD - SEEDING SHOULD BE BETTER
   //TODO Switch to assigning leaves! Don't use Octree!
@@ -380,7 +381,7 @@ pcl::SupervoxelClustering<PointT>::selectInitialSupervoxelSeeds (std::vector<int
   std::vector<int> seed_indices_orig;
   seed_indices_orig.resize (num_seeds, 0);
   seed_indices.clear ();
-  std::vector<int> closest_index;
+  pcl::Indices closest_index;
   std::vector<float> distance;
   closest_index.resize(1,0);
   distance.resize(1,0);
@@ -396,20 +397,19 @@ pcl::SupervoxelClustering<PointT>::selectInitialSupervoxelSeeds (std::vector<int
     seed_indices_orig[i] = closest_index[0];
   }
   
-  std::vector<int> neighbors;
+  pcl::Indices neighbors;
   std::vector<float> sqr_distances;
   seed_indices.reserve (seed_indices_orig.size ());
   float search_radius = 0.5f*seed_resolution_;
   // This is 1/20th of the number of voxels which fit in a planar slice through search volume
   // Area of planar slice / area of voxel side. (Note: This is smaller than the value mentioned in the original paper)
   float min_points = 0.05f * (search_radius)*(search_radius) * 3.1415926536f  / (resolution_*resolution_);
-  for (const int &index_orig : seed_indices_orig)
+  for (const auto &index_orig : seed_indices_orig)
   {
     int num = voxel_kdtree_->radiusSearch (index_orig, search_radius , neighbors, sqr_distances);
-    int min_index = index_orig;
     if ( num > min_points)
     {
-      seed_indices.push_back (min_index);
+      seed_indices.push_back (index_orig);
     }
     
   }
@@ -428,7 +428,7 @@ pcl::SupervoxelClustering<PointT>::reseedSupervoxels ()
     sv_itr->removeAllLeaves ();
   }
   
-  std::vector<int> closest_index;
+  Indices closest_index;
   std::vector<float> distance;
   //Now go through each supervoxel, find voxel closest to its center, add it in
   for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
@@ -836,7 +836,7 @@ pcl::SupervoxelClustering<PointT>::SupervoxelHelper::refineNormals ()
   for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
   {
     VoxelData& voxel_data = (*leaf_itr)->getData ();
-    std::vector<int> indices;
+    Indices indices;
     indices.reserve (81); 
     //Push this point
     indices.push_back (voxel_data.idx_);
diff --git a/segmentation/include/pcl/segmentation/impl/unary_classifier.hpp b/segmentation/include/pcl/segmentation/impl/unary_classifier.hpp
index f911236f..354956f0 100644
--- a/segmentation/include/pcl/segmentation/impl/unary_classifier.hpp
+++ b/segmentation/include/pcl/segmentation/impl/unary_classifier.hpp
@@ -174,25 +174,24 @@ pcl::UnaryClassifier<PointT>::getCloudWithLabel (typename pcl::PointCloud<PointT
   // find the 'label' field index
   std::vector <pcl::PCLPointField> fields;
   int label_idx = -1;
-  pcl::PointCloud <PointT> point;
   label_idx = pcl::getFieldIndex<PointT> ("label", fields);
 
   if (label_idx != -1)
   {
-    for (std::size_t i = 0; i < in->size (); i++)
+    for (const auto& point : (*in))
     {
       // get the 'label' field                                                                       
       std::uint32_t label;
-      memcpy (&label, reinterpret_cast<char*> (&(*in)[i]) + fields[label_idx].offset, sizeof(std::uint32_t));
+      memcpy (&label, reinterpret_cast<const char*> (&point) + fields[label_idx].offset, sizeof(std::uint32_t));
 
       if (static_cast<int> (label) == label_num)
       {
-        pcl::PointXYZ point;
+        pcl::PointXYZ tmp;
         // X Y Z
-        point.x = (*in)[i].x;
-        point.y = (*in)[i].y;
-        point.z = (*in)[i].z;
-        out->points.push_back (point);
+        tmp.x = point.x;
+        tmp.y = point.y;
+        tmp.z = point.z;
+        out->push_back (tmp);
       }
     }
     out->width = out->size ();
@@ -273,7 +272,7 @@ pcl::UnaryClassifier<PointT>::kmeansClustering (pcl::PointCloud<pcl::FPFHSignatu
 template <typename PointT> void
 pcl::UnaryClassifier<PointT>::queryFeatureDistances (std::vector<pcl::PointCloud<pcl::FPFHSignature33>::Ptr> &trained_features,
                                                      pcl::PointCloud<pcl::FPFHSignature33>::Ptr query_features,
-                                                     std::vector<int> &indi,
+                                                     pcl::Indices &indi,
                                                      std::vector<float> &dist)
 {
   // estimate the total number of row's needed
@@ -328,7 +327,7 @@ pcl::UnaryClassifier<PointT>::queryFeatureDistances (std::vector<pcl::PointCloud
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
-pcl::UnaryClassifier<PointT>::assignLabels (std::vector<int> &indi,
+pcl::UnaryClassifier<PointT>::assignLabels (pcl::Indices &indi,
                                             std::vector<float> &dist,
                                             int n_feature_means,
                                             float feature_threshold,
@@ -416,7 +415,7 @@ pcl::UnaryClassifier<PointT>::segment (pcl::PointCloud<pcl::PointXYZRGBL>::Ptr &
     computeFPFH (tmp_cloud, input_cloud_features, normal_radius_search_, fpfh_radius_search_);
 
     // query the distances from the input data features to all trained features
-    std::vector<int> indices;
+    Indices indices;
     std::vector<float> distance;
     queryFeatureDistances (trained_features_, input_cloud_features, indices, distance);
 
diff --git a/segmentation/include/pcl/segmentation/lccp_segmentation.h b/segmentation/include/pcl/segmentation/lccp_segmentation.h
index ad7a2cc7..ed049681 100644
--- a/segmentation/include/pcl/segmentation/lccp_segmentation.h
+++ b/segmentation/include/pcl/segmentation/lccp_segmentation.h
@@ -37,7 +37,6 @@
 
 #pragma once
 
-#include <pcl/pcl_base.h>
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
 #include <pcl/segmentation/supervoxel_clustering.h>
diff --git a/segmentation/include/pcl/segmentation/min_cut_segmentation.h b/segmentation/include/pcl/segmentation/min_cut_segmentation.h
index f2a26897..d75d2f8c 100644
--- a/segmentation/include/pcl/segmentation/min_cut_segmentation.h
+++ b/segmentation/include/pcl/segmentation/min_cut_segmentation.h
@@ -38,7 +38,6 @@
 
 #pragma once
 
-#include <pcl/segmentation/boost.h>
 #include <pcl/memory.h>
 #include <pcl/pcl_base.h>
 #include <pcl/pcl_macros.h>
@@ -47,6 +46,7 @@
 #include <pcl/search/search.h>
 #include <string>
 #include <set>
+#include <boost/graph/adjacency_list.hpp> // for adjacency_list
 
 namespace pcl
 {
diff --git a/segmentation/include/pcl/segmentation/organized_multi_plane_segmentation.h b/segmentation/include/pcl/segmentation/organized_multi_plane_segmentation.h
index 0963348c..b4fa41a0 100644
--- a/segmentation/include/pcl/segmentation/organized_multi_plane_segmentation.h
+++ b/segmentation/include/pcl/segmentation/organized_multi_plane_segmentation.h
@@ -278,27 +278,6 @@ namespace pcl
               PointCloudLPtr& labels,
               std::vector<pcl::PointIndices>& label_indices);
 
-      /** \brief Perform a refinement of an initial segmentation, by comparing points to adjacent regions detected by the initial segmentation.
-        * \param [in] model_coefficients The list of segmented model coefficients
-        * \param [in] inlier_indices The list of segmented inlier indices, corresponding to each model
-        * \param [in] centroids The list of centroids corresponding to each segmented plane
-        * \param [in] covariances The list of covariances corresponding to each segemented plane
-        * \param [in] labels The labels produced by the initial segmentation
-        * \param [in] label_indices The list of indices corresponding to each label
-        */
-      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<ModelCoefficients>& model_coefficients, 
-              std::vector<PointIndices>& inlier_indices,
-              std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& centroids,
-              std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >& covariances,
-              PointCloudLPtr& labels,
-              std::vector<pcl::PointIndices>& label_indices)
-      {
-        pcl::utils::ignore(centroids, covariances);
-        refine(model_coefficients, inlier_indices, labels, label_indices);
-      }
-
     protected:
 
       /** \brief A pointer to the input normals */
diff --git a/segmentation/include/pcl/segmentation/progressive_morphological_filter.h b/segmentation/include/pcl/segmentation/progressive_morphological_filter.h
index d31b9f34..92748346 100644
--- a/segmentation/include/pcl/segmentation/progressive_morphological_filter.h
+++ b/segmentation/include/pcl/segmentation/progressive_morphological_filter.h
@@ -39,7 +39,6 @@
 #pragma once
 
 #include <pcl/pcl_base.h>
-#include <pcl/search/search.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 
@@ -133,7 +132,7 @@ namespace pcl
         * \param[out] ground indices of points determined to be ground returns.
         */
       virtual void
-      extract (std::vector<int>& ground);
+      extract (Indices& ground);
 
     protected:
 
diff --git a/segmentation/include/pcl/segmentation/random_walker.h b/segmentation/include/pcl/segmentation/random_walker.h
index 351e9696..5ec0e959 100644
--- a/segmentation/include/pcl/segmentation/random_walker.h
+++ b/segmentation/include/pcl/segmentation/random_walker.h
@@ -41,7 +41,7 @@
 #include <boost/graph/graph_concepts.hpp>
 #include <boost/concept/assert.hpp>
 
-#include <Eigen/Dense>
+#include <Eigen/Core> // for Matrix
 
 namespace pcl
 {
diff --git a/segmentation/include/pcl/segmentation/region_3d.h b/segmentation/include/pcl/segmentation/region_3d.h
index d68b093e..e9304d01 100644
--- a/segmentation/include/pcl/segmentation/region_3d.h
+++ b/segmentation/include/pcl/segmentation/region_3d.h
@@ -38,7 +38,6 @@
 #pragma once
 
 #include <Eigen/Core>
-#include <vector>
 
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
diff --git a/segmentation/include/pcl/segmentation/region_growing.h b/segmentation/include/pcl/segmentation/region_growing.h
index 445842c7..3e8a99eb 100644
--- a/segmentation/include/pcl/segmentation/region_growing.h
+++ b/segmentation/include/pcl/segmentation/region_growing.h
@@ -45,9 +45,6 @@
 #include <pcl/search/search.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
-#include <list>
-#include <cmath>
-#include <ctime>
 
 namespace pcl
 {
@@ -57,6 +54,7 @@ namespace pcl
     * "Segmentation of point clouds using smoothness constraint"
     * by T. Rabbania, F. A. van den Heuvelb, G. Vosselmanc.
     * In addition to residual test, the possibility to test curvature is added.
+    * \ingroup segmentation
     */
   template <typename PointT, typename NormalT>
   class PCL_EXPORTS RegionGrowing : public pcl::PCLBase<PointT>
@@ -86,20 +84,20 @@ namespace pcl
       ~RegionGrowing ();
 
       /** \brief Get the minimum number of points that a cluster needs to contain in order to be considered valid. */
-      int
+      pcl::uindex_t
       getMinClusterSize ();
 
       /** \brief Set the minimum number of points that a cluster needs to contain in order to be considered valid. */
       void
-      setMinClusterSize (int min_cluster_size);
+      setMinClusterSize (pcl::uindex_t min_cluster_size);
 
       /** \brief Get the maximum number of points that a cluster needs to contain in order to be considered valid. */
-      int
+      pcl::uindex_t
       getMaxClusterSize ();
 
       /** \brief Set the maximum number of points that a cluster needs to contain in order to be considered valid. */
       void
-      setMaxClusterSize (int max_cluster_size);
+      setMaxClusterSize (pcl::uindex_t max_cluster_size);
 
       /** \brief Returns the flag value. This flag signalizes which mode of algorithm will be used.
         * If it is set to true than it will work as said in the article. This means that
@@ -214,7 +212,7 @@ namespace pcl
         * \param[out] cluster cluster to which the point belongs.
         */
       virtual void
-      getSegmentFromPoint (int index, pcl::PointIndices& cluster);
+      getSegmentFromPoint (pcl::index_t index, pcl::PointIndices& cluster);
 
       /** \brief If the cloud was successfully segmented, then function
         * returns colored cloud. Otherwise it returns an empty pointer.
@@ -270,7 +268,7 @@ namespace pcl
         * \param[out] is_a_seed this value is set to true if the point with index 'nghbr' can serve as the seed
         */
       virtual bool
-      validatePoint (int initial_seed, int point, int nghbr, bool& is_a_seed) const;
+      validatePoint (pcl::index_t initial_seed, pcl::index_t point, pcl::index_t nghbr, bool& is_a_seed) const;
 
       /** \brief This function simply assembles the regions from list of point labels.
         * Each cluster is an array of point indices.
@@ -281,10 +279,10 @@ namespace pcl
     protected:
 
       /** \brief Stores the minimum number of points that a cluster needs to contain in order to be considered valid. */
-      int min_pts_per_cluster_;
+      pcl::uindex_t min_pts_per_cluster_;
 
       /** \brief Stores the maximum number of points that a cluster needs to contain in order to be considered valid. */
-      int max_pts_per_cluster_;
+      pcl::uindex_t max_pts_per_cluster_;
 
       /** \brief Flag that signalizes if the smoothness constraint will be used. */
       bool smooth_mode_flag_;
@@ -314,7 +312,7 @@ namespace pcl
       NormalPtr normals_;
 
       /** \brief Contains neighbours of each point. */
-      std::vector<std::vector<int> > point_neighbours_;
+      std::vector<pcl::Indices> point_neighbours_;
 
       /** \brief Point labels that tells to which segment each point belongs. */
       std::vector<int> point_labels_;
@@ -325,7 +323,7 @@ namespace pcl
       bool normal_flag_;
 
       /** \brief Tells how much points each segment contains. Used for reserving memory. */
-      std::vector<int> num_pts_in_segment_;
+      std::vector<pcl::uindex_t> num_pts_in_segment_;
 
       /** \brief After the segmentation it will contain the segments. */
       std::vector <pcl::PointIndices> clusters_;
diff --git a/segmentation/include/pcl/segmentation/region_growing_rgb.h b/segmentation/include/pcl/segmentation/region_growing_rgb.h
index f1799599..f8149a55 100644
--- a/segmentation/include/pcl/segmentation/region_growing_rgb.h
+++ b/segmentation/include/pcl/segmentation/region_growing_rgb.h
@@ -50,6 +50,7 @@ namespace pcl
     * Description can be found in the article
     * "Color-based segmentation of point clouds"
     * by Qingming Zhan, Yubin Liang, Yinghui Xiao
+    * \ingroup segmentation
     */
   template <typename PointT, typename NormalT = pcl::Normal>
   class PCL_EXPORTS RegionGrowingRGB : public RegionGrowing<PointT, NormalT>
@@ -175,7 +176,7 @@ namespace pcl
         * \param cluster
         */
       void
-      getSegmentFromPoint (int index, pcl::PointIndices& cluster) override;
+      getSegmentFromPoint (index_t index, pcl::PointIndices& cluster) override;
 
     protected:
 
@@ -204,7 +205,7 @@ namespace pcl
         * \param[out] dist the array of distances to the corresponding neighbours
         */
       void
-      findRegionsKNN (int index, int nghbr_number, std::vector<int>& nghbrs, std::vector<float>& dist);
+      findRegionsKNN (pcl::index_t index, pcl::uindex_t nghbr_number, Indices& nghbrs, std::vector<float>& dist);
 
       /** \brief This function implements the merging algorithm described in the article
         * "Color-based segmentation of point clouds"
@@ -229,7 +230,7 @@ namespace pcl
         * to the corresponding homogeneous region.
         */
       void
-      findRegionNeighbours (std::vector< std::vector< std::pair<float, int> > >& neighbours_out, std::vector< std::vector<int> >& regions_in);
+      findRegionNeighbours (std::vector< std::vector< std::pair<float, pcl::index_t> > >& neighbours_out, std::vector< std::vector<int> >& regions_in);
 
       /** \brief This function simply assembles the regions from list of point labels.
         * \param[in] num_pts_in_region for each final region it stores the corresponding number of points in it
@@ -246,7 +247,7 @@ namespace pcl
         * \param[out] is_a_seed this value is set to true if the point with index 'nghbr' can serve as the seed
         */
       bool
-      validatePoint (int initial_seed, int point, int nghbr, bool& is_a_seed) const override;
+      validatePoint (index_t initial_seed, index_t point, index_t nghbr, bool& is_a_seed) const override;
 
     protected:
 
@@ -266,7 +267,7 @@ namespace pcl
       std::vector< std::vector<float> > point_distances_;
 
       /** \brief Stores the neighboures for the corresponding segments. */
-      std::vector< std::vector<int> > segment_neighbours_;
+      std::vector< pcl::Indices > segment_neighbours_;
 
       /** \brief Stores distances for the segment neighbours from segment_neighbours_ */
       std::vector< std::vector<float> > segment_distances_;
diff --git a/segmentation/include/pcl/segmentation/rgb_plane_coefficient_comparator.h b/segmentation/include/pcl/segmentation/rgb_plane_coefficient_comparator.h
index 263ad8c1..5c5c5a92 100644
--- a/segmentation/include/pcl/segmentation/rgb_plane_coefficient_comparator.h
+++ b/segmentation/include/pcl/segmentation/rgb_plane_coefficient_comparator.h
@@ -39,7 +39,6 @@
 
 #pragma once
 
-#include <pcl/segmentation/boost.h>
 #include <pcl/segmentation/plane_coefficient_comparator.h>
 
 namespace pcl
diff --git a/segmentation/include/pcl/segmentation/seeded_hue_segmentation.h b/segmentation/include/pcl/segmentation/seeded_hue_segmentation.h
index e8cfa426..286bc92a 100644
--- a/segmentation/include/pcl/segmentation/seeded_hue_segmentation.h
+++ b/segmentation/include/pcl/segmentation/seeded_hue_segmentation.h
@@ -40,7 +40,7 @@
 
 #include <pcl/pcl_base.h>
 #include <pcl/point_types_conversion.h>
-#include <pcl/search/pcl_search.h>
+#include <pcl/search/search.h> // for Search
 
 namespace pcl
 {
diff --git a/segmentation/include/pcl/segmentation/segment_differences.h b/segmentation/include/pcl/segmentation/segment_differences.h
index 2428a61d..9b89cd2a 100755
--- a/segmentation/include/pcl/segmentation/segment_differences.h
+++ b/segmentation/include/pcl/segmentation/segment_differences.h
@@ -39,7 +39,7 @@
 
 #include <pcl/pcl_base.h>
 #include <pcl/pcl_macros.h>
-#include <pcl/search/pcl_search.h>
+#include <pcl/search/search.h> // for Search
 
 namespace pcl
 {
@@ -59,18 +59,6 @@ namespace pcl
       const typename pcl::search::Search<PointT>::Ptr &tree,
       pcl::PointCloud<PointT> &output);
 
-  template <typename PointT>
-  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<PointT> &src,
-      const pcl::PointCloud<PointT> & /* tgt */,
-      double threshold,
-      const typename pcl::search::Search<PointT>::Ptr &tree,
-      pcl::PointCloud<PointT> &output)
-  {
-    getPointCloudDifference<PointT> (src, threshold, tree, output);
-  }
-
   ////////////////////////////////////////////////////////////////////////////////////////////
   ////////////////////////////////////////////////////////////////////////////////////////////
   ////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/segmentation/include/pcl/segmentation/supervoxel_clustering.h b/segmentation/include/pcl/segmentation/supervoxel_clustering.h
index 1c43f406..02377111 100644
--- a/segmentation/include/pcl/segmentation/supervoxel_clustering.h
+++ b/segmentation/include/pcl/segmentation/supervoxel_clustering.h
@@ -51,7 +51,7 @@
 #include <pcl/octree/octree_search.h>
 #include <pcl/octree/octree_pointcloud_adjacency.h>
 #include <pcl/search/search.h>
-#include <pcl/segmentation/boost.h>
+#include <boost/ptr_container/ptr_list.hpp> // for ptr_list
 
 
 
@@ -195,9 +195,6 @@ namespace pcl
        */
       SupervoxelClustering (float voxel_resolution, float seed_resolution);
 
-      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
         * finding neighbors. In other words it frees memory.
         */
@@ -272,20 +269,6 @@ namespace pcl
       refineSupervoxels (int num_itr, std::map<std::uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
 
       ////////////////////////////////////////////////////////////
-      /** \brief Returns an RGB colorized cloud showing superpixels
-        * Otherwise it returns an empty pointer.
-        * Points that belong to the same supervoxel have the same color.
-        * But this function doesn't guarantee that different segments will have different
-        * 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(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<PointXYZRGBA>::Ptr
-      getColoredCloud () const
-      {
-        return pcl::PointCloud<PointXYZRGBA>::Ptr (new pcl::PointCloud<PointXYZRGBA>);
-      }
-
       /** \brief Returns a deep copy of the voxel centroid cloud */
       typename pcl::PointCloud<PointT>::Ptr
       getVoxelCentroidCloud () const;
@@ -339,13 +322,13 @@ namespace pcl
        *  \param[out] seed_indices The selected leaf indices
        */
       void
-      selectInitialSupervoxelSeeds (std::vector<int> &seed_indices);
+      selectInitialSupervoxelSeeds (Indices &seed_indices);
 
       /** \brief This method creates the internal supervoxel helpers based on the provided seed points
        *  \param[in] seed_indices Indices of the leaves to use as seeds
        */
       void
-      createSupervoxelHelpers (std::vector<int> &seed_indices);
+      createSupervoxelHelpers (Indices &seed_indices);
 
       /** \brief This performs the superpixel evolution */
       void
diff --git a/segmentation/include/pcl/segmentation/unary_classifier.h b/segmentation/include/pcl/segmentation/unary_classifier.h
index 25086923..d34116a5 100644
--- a/segmentation/include/pcl/segmentation/unary_classifier.h
+++ b/segmentation/include/pcl/segmentation/unary_classifier.h
@@ -47,9 +47,6 @@
 #include <pcl/features/fpfh.h>
 #include <pcl/features/normal_3d.h>
 
-#include <pcl/filters/filter_indices.h>
-#include <pcl/segmentation/extract_clusters.h>
-
 #include <pcl/ml/kmeans.h>
 
 namespace pcl
@@ -88,11 +85,11 @@ namespace pcl
       void
       queryFeatureDistances (std::vector<pcl::PointCloud<pcl::FPFHSignature33>::Ptr> &trained_features,
                              pcl::PointCloud<pcl::FPFHSignature33>::Ptr query_features,
-                             std::vector<int> &indi,
+                             pcl::Indices &indi,
                              std::vector<float> &dist);
 
       void
-      assignLabels (std::vector<int> &indi,
+      assignLabels (pcl::Indices &indi,
                     std::vector<float> &dist,
                     int n_feature_means,
                     float feature_threshold,
diff --git a/segmentation/src/extract_clusters.cpp b/segmentation/src/extract_clusters.cpp
index 29f852e3..85560542 100644
--- a/segmentation/src/extract_clusters.cpp
+++ b/segmentation/src/extract_clusters.cpp
@@ -38,20 +38,24 @@
  */
 
 #include <pcl/impl/instantiate.hpp>
-#include <pcl/point_types.h>
 #include <pcl/segmentation/impl/extract_clusters.hpp>
 #include <pcl/segmentation/impl/extract_labeled_clusters.hpp>
+#include <pcl/point_types.h>
 
 // Instantiations of specific point types
 #ifdef PCL_ONLY_CORE_POINT_TYPES
-  PCL_INSTANTIATE(EuclideanClusterExtraction, (pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGBA)(pcl::PointXYZRGB))
-  PCL_INSTANTIATE(extractEuclideanClusters, (pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGBA)(pcl::PointXYZRGB))
-  PCL_INSTANTIATE(extractEuclideanClusters_indices, (pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGBA)(pcl::PointXYZRGB))
+PCL_INSTANTIATE(EuclideanClusterExtraction,
+                (pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGBA)(pcl::PointXYZRGB))
+PCL_INSTANTIATE(extractEuclideanClusters,
+                (pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGBA)(pcl::PointXYZRGB))
+PCL_INSTANTIATE(extractEuclideanClusters_indices,
+                (pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGBA)(pcl::PointXYZRGB))
 #else
-  PCL_INSTANTIATE(EuclideanClusterExtraction, PCL_XYZ_POINT_TYPES)
-  PCL_INSTANTIATE(extractEuclideanClusters, PCL_XYZ_POINT_TYPES)
-  PCL_INSTANTIATE(extractEuclideanClusters_indices, PCL_XYZ_POINT_TYPES)
+PCL_INSTANTIATE(EuclideanClusterExtraction, PCL_XYZ_POINT_TYPES)
+PCL_INSTANTIATE(extractEuclideanClusters, PCL_XYZ_POINT_TYPES)
+PCL_INSTANTIATE(extractEuclideanClusters_indices, PCL_XYZ_POINT_TYPES)
 #endif
 PCL_INSTANTIATE(LabeledEuclideanClusterExtraction, PCL_XYZL_POINT_TYPES)
+PCL_INSTANTIATE(extractLabeledEuclideanClusters_deprecated, PCL_XYZL_POINT_TYPES)
 PCL_INSTANTIATE(extractLabeledEuclideanClusters, PCL_XYZL_POINT_TYPES)
 
diff --git a/segmentation/src/grabcut_segmentation.cpp b/segmentation/src/grabcut_segmentation.cpp
index 43729b55..543dde16 100644
--- a/segmentation/src/grabcut_segmentation.cpp
+++ b/segmentation/src/grabcut_segmentation.cpp
@@ -663,7 +663,7 @@ pcl::segmentation::grabcut::GMM::probabilityDensity (std::size_t i, const Color
 
 void
 pcl::segmentation::grabcut::buildGMMs (const Image& image,
-                                       const std::vector<int>& indices,
+                                       const Indices& indices,
                                        const std::vector<SegmentationValue>& hard_segmentation,
                                        std::vector<std::size_t>& components,
                                        GMM& background_GMM, GMM& foreground_GMM)
@@ -779,7 +779,7 @@ pcl::segmentation::grabcut::buildGMMs (const Image& image,
 
 void
 pcl::segmentation::grabcut::learnGMMs (const Image& image,
-                                       const std::vector<int>& indices,
+                                       const Indices& indices,
                                        const std::vector<SegmentationValue>& hard_segmentation,
                                        std::vector<std::size_t>& components,
                                        GMM& background_GMM, GMM& foreground_GMM)
diff --git a/simulation/CMakeLists.txt b/simulation/CMakeLists.txt
index 22dd6070..827c366a 100644
--- a/simulation/CMakeLists.txt
+++ b/simulation/CMakeLists.txt
@@ -4,12 +4,8 @@ set(SUBSYS_DEPS common io surface kdtree features search octree visualization fi
 
 set(build FALSE)
 find_package(OpenGL)
-if(APPLE)
-  # homebrew's FindGLEW module is not in good shape
-  find_package(glew CONFIG)
-ELSE()
-  find_package(GLEW)
-ENDIF()
+
+find_package(GLEW)
 
 PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" OFF)
 PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} EXT_DEPS opengl glew)
diff --git a/simulation/include/pcl/simulation/camera.h b/simulation/include/pcl/simulation/camera.h
index c680219c..9c0ef990 100644
--- a/simulation/include/pcl/simulation/camera.h
+++ b/simulation/include/pcl/simulation/camera.h
@@ -3,7 +3,7 @@
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
 
-#include <Eigen/Dense>
+#include <Eigen/Geometry> // for Isometry3d
 #include <Eigen/StdVector>
 
 namespace pcl {
diff --git a/simulation/include/pcl/simulation/model.h b/simulation/include/pcl/simulation/model.h
index 8b592f94..00f48ee9 100644
--- a/simulation/include/pcl/simulation/model.h
+++ b/simulation/include/pcl/simulation/model.h
@@ -1,11 +1,11 @@
 #pragma once
 
-#include <pcl/io/pcd_io.h>
 #include <pcl/simulation/glsl_shader.h>
 #include <pcl/PolygonMesh.h>
 #include <pcl/memory.h>
 #include <pcl/pcl_config.h>
 #include <pcl/pcl_macros.h>
+#include <pcl/point_cloud.h> // for PointCloud
 #include <pcl/point_types.h>
 
 #if defined(_WIN32) && !defined(APIENTRY) && !defined(__CYGWIN__)
diff --git a/simulation/include/pcl/simulation/range_likelihood.h b/simulation/include/pcl/simulation/range_likelihood.h
index 6b0d7e6e..aab4bbb4 100644
--- a/simulation/include/pcl/simulation/range_likelihood.h
+++ b/simulation/include/pcl/simulation/range_likelihood.h
@@ -1,6 +1,5 @@
 #pragma once
 
-#include <pcl/common/transforms.h>
 #include <pcl/range_image/range_image_planar.h>
 #include <pcl/simulation/camera.h>
 #include <pcl/simulation/glsl_shader.h>
diff --git a/simulation/src/camera.cpp b/simulation/src/camera.cpp
index 1e343c06..7980e84f 100644
--- a/simulation/src/camera.cpp
+++ b/simulation/src/camera.cpp
@@ -1,7 +1,5 @@
 #include <pcl/simulation/camera.h>
 
-#include <iostream>
-
 using namespace Eigen;
 using namespace pcl::simulation;
 
diff --git a/simulation/src/model.cpp b/simulation/src/model.cpp
index a951b164..d1bf600e 100644
--- a/simulation/src/model.cpp
+++ b/simulation/src/model.cpp
@@ -1,4 +1,5 @@
 #include <pcl/simulation/model.h>
+#include <pcl/conversions.h> // for fromPCLPointCloud2
 using namespace pcl::simulation;
 
 pcl::simulation::TriangleMeshModel::TriangleMeshModel(pcl::PolygonMesh::Ptr plg)
@@ -21,7 +22,7 @@ pcl::simulation::TriangleMeshModel::TriangleMeshModel(pcl::PolygonMesh::Ptr plg)
 
     Eigen::Vector4f tmp;
     for (const auto& polygon : plg->polygons) {
-      for (const unsigned int& point : polygon.vertices) {
+      for (const auto& point : polygon.vertices) {
         tmp = newcloud[point].getVector4fMap();
         vertices.push_back(Vertex(Eigen::Vector3f(tmp(0), tmp(1), tmp(2)),
                                   Eigen::Vector3f(newcloud[point].r / 255.0f,
@@ -36,7 +37,7 @@ pcl::simulation::TriangleMeshModel::TriangleMeshModel(pcl::PolygonMesh::Ptr plg)
     pcl::fromPCLPointCloud2(plg->cloud, newcloud);
     Eigen::Vector4f tmp;
     for (const auto& polygon : plg->polygons) {
-      for (const unsigned int& point : polygon.vertices) {
+      for (const auto& point : polygon.vertices) {
         tmp = newcloud[point].getVector4fMap();
         vertices.push_back(Vertex(Eigen::Vector3f(tmp(0), tmp(1), tmp(2)),
                                   Eigen::Vector3f(1.0, 1.0, 1.0)));
diff --git a/simulation/src/range_likelihood.cpp b/simulation/src/range_likelihood.cpp
index 2fa040aa..e40aeaa2 100644
--- a/simulation/src/range_likelihood.cpp
+++ b/simulation/src/range_likelihood.cpp
@@ -1,4 +1,5 @@
 #include <pcl/common/time.h>
+#include <pcl/common/transforms.h> // for transformPointCloud
 #include <pcl/simulation/range_likelihood.h>
 #include <pcl/pcl_config.h>
 
@@ -13,7 +14,6 @@
 #include <GL/glu.h>
 #endif
 
-#include <ctime>
 #include <random>
 
 // For adding noise:
diff --git a/simulation/tools/sim_terminal_demo.cpp b/simulation/tools/sim_terminal_demo.cpp
index be9cbfa4..5b5de285 100644
--- a/simulation/tools/sim_terminal_demo.cpp
+++ b/simulation/tools/sim_terminal_demo.cpp
@@ -11,10 +11,9 @@
  */
 
 #include <pcl/common/time.h> // for getTime
+#include <pcl/io/pcd_io.h>   // for PCDWriter
 #include <pcl/memory.h>
 
-#include <Eigen/Dense>
-
 #include "simulation_io.hpp"
 
 #include <cmath>
diff --git a/simulation/tools/sim_test_performance.cpp b/simulation/tools/sim_test_performance.cpp
index e8ff8e93..a6a8174e 100644
--- a/simulation/tools/sim_test_performance.cpp
+++ b/simulation/tools/sim_test_performance.cpp
@@ -5,24 +5,16 @@
  *
  */
 
-#include <pcl/common/common.h>
-#include <pcl/common/transforms.h>
-#include <pcl/console/parse.h>
 #include <pcl/console/print.h>
 #include <pcl/console/time.h>
-#include <pcl/features/normal_3d.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/io/vtk_lib_io.h>
 #include <pcl/simulation/camera.h>
 #include <pcl/simulation/model.h>
 #include <pcl/simulation/range_likelihood.h>
 #include <pcl/simulation/scene.h>
-#include <pcl/surface/gp3.h>
 #include <pcl/memory.h>
 #include <pcl/pcl_config.h>
-#include <pcl/point_types.h>
-
-#include <Eigen/Dense>
 
 #include <GL/glew.h>
 
diff --git a/simulation/tools/sim_test_simple.cpp b/simulation/tools/sim_test_simple.cpp
index 9a5722fb..805ec2e0 100644
--- a/simulation/tools/sim_test_simple.cpp
+++ b/simulation/tools/sim_test_simple.cpp
@@ -17,10 +17,7 @@
  */
 
 #include <pcl/common/common.h>
-#include <pcl/common/transforms.h>
-#include <pcl/console/parse.h>
 #include <pcl/console/print.h>
-#include <pcl/features/normal_3d.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/io/vtk_lib_io.h>
 #include <pcl/range_image/range_image_planar.h> // RangeImage
@@ -28,14 +25,11 @@
 #include <pcl/simulation/model.h>
 #include <pcl/simulation/range_likelihood.h>
 #include <pcl/simulation/scene.h>
-#include <pcl/surface/gp3.h>
 #include <pcl/visualization/cloud_viewer.h> // Pop-up viewer
 #include <pcl/memory.h>
 #include <pcl/pcl_config.h>
 #include <pcl/point_types.h>
 
-#include <Eigen/Dense>
-
 #include <GL/glew.h>
 
 #ifdef OPENGL_IS_A_FRAMEWORK
diff --git a/simulation/tools/sim_viewer.cpp b/simulation/tools/sim_viewer.cpp
index 7cb82857..52a73683 100644
--- a/simulation/tools/sim_viewer.cpp
+++ b/simulation/tools/sim_viewer.cpp
@@ -39,11 +39,9 @@
 
 #include <pcl/common/common.h>
 #include <pcl/common/time.h> // for getTime
-#include <pcl/common/transforms.h>
 #include <pcl/console/parse.h>
 #include <pcl/console/print.h>
 #include <pcl/console/time.h>
-#include <pcl/features/normal_3d.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/io/vtk_lib_io.h>
 #include <pcl/range_image/range_image_planar.h> // RangeImage
@@ -51,18 +49,15 @@
 #include <pcl/simulation/model.h>
 #include <pcl/simulation/range_likelihood.h>
 #include <pcl/simulation/scene.h>
-#include <pcl/surface/gp3.h>
 #include <pcl/visualization/cloud_viewer.h> // Pop-up viewer
 #include <pcl/visualization/histogram_visualizer.h>
 #include <pcl/visualization/keyboard_event.h>
 #include <pcl/visualization/pcl_visualizer.h>
-#include <pcl/visualization/point_cloud_handlers.h>
 #include <pcl/visualization/point_picking_event.h>
 #include <pcl/memory.h>
 #include <pcl/pcl_config.h>
 #include <pcl/point_types.h>
 
-#include <Eigen/Dense>
 #include <Eigen/Geometry>
 
 #include <vtkPolyDataReader.h>
@@ -221,19 +216,19 @@ pp_callback(const pcl::visualization::PointPickingEvent& event, void* cookie)
   }
 
   // Else, if a single point has been selected
-  std::stringstream ss;
-  ss << event.getPointIndex();
+  std::string pointIndexStr = std::to_string(event.getPointIndex());
   // Get the cloud's fields
   for (std::size_t i = 0; i < cloud->fields.size(); ++i) {
     if (!isMultiDimensionalFeatureField(cloud->fields[i]))
       continue;
     ph_global.addFeatureHistogram(
-        *cloud, cloud->fields[i].name, event.getPointIndex(), ss.str());
+        *cloud, cloud->fields[i].name, event.getPointIndex(), pointIndexStr);
   }
   if (p) {
     pcl::PointXYZ pos;
     event.getPoint(pos.x, pos.y, pos.z);
-    p->addText3D<pcl::PointXYZ>(ss.str(), pos, 0.0005, 1.0, 1.0, 1.0, ss.str());
+    p->addText3D<pcl::PointXYZ>(
+        pointIndexStr, pos, 0.0005, 1.0, 1.0, 1.0, pointIndexStr);
   }
   ph_global.spinOnce();
 }
@@ -552,12 +547,13 @@ main(int argc, char** argv)
 
   // Go through VTK files
   for (std::size_t i = 0; i < vtk_file_indices.size(); ++i) {
+    const char* vtk_file = argv[vtk_file_indices[i]];
     // Load file
     tt.tic();
     print_highlight(stderr, "Loading ");
-    print_value(stderr, "%s ", argv[vtk_file_indices.at(i)]);
+    print_value(stderr, "%s ", vtk_file);
     vtkPolyDataReader* reader = vtkPolyDataReader::New();
-    reader->SetFileName(argv[vtk_file_indices.at(i)]);
+    reader->SetFileName(vtk_file);
     reader->Update();
     vtkSmartPointer<vtkPolyData> polydata = reader->GetOutput();
     if (!polydata)
@@ -584,9 +580,9 @@ main(int argc, char** argv)
     }
 
     // Add as actor
-    std::stringstream cloud_name("vtk-");
-    cloud_name << argv[vtk_file_indices.at(i)] << "-" << i;
-    p->addModelFromPolyData(polydata, cloud_name.str(), viewport);
+    const std::string cloud_name =
+        "vtk-" + std::string(vtk_file) + "-" + std::to_string(i);
+    p->addModelFromPolyData(polydata, cloud_name, viewport);
 
     // Change the shape rendered color
     if (fcolorparam && fcolor_r.size() > i && fcolor_g.size() > i &&
@@ -595,40 +591,37 @@ main(int argc, char** argv)
                                      fcolor_r[i],
                                      fcolor_g[i],
                                      fcolor_b[i],
-                                     cloud_name.str());
+                                     cloud_name);
 
     // Change the shape rendered point size
     if (!psize.empty())
       p->setShapeRenderingProperties(
-          pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize.at(i), cloud_name.str());
+          pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize.at(i), cloud_name);
 
     // Change the shape rendered opacity
     if (!opaque.empty())
       p->setShapeRenderingProperties(
-          pcl::visualization::PCL_VISUALIZER_OPACITY, opaque.at(i), cloud_name.str());
+          pcl::visualization::PCL_VISUALIZER_OPACITY, opaque.at(i), cloud_name);
   }
 
   pcl::PCLPointCloud2::Ptr cloud;
   // Go through PCD files
   for (std::size_t i = 0; i < p_file_indices.size(); ++i) {
+    const std::string p_file = argv[p_file_indices[i]];
     cloud.reset(new pcl::PCLPointCloud2);
     Eigen::Vector4f origin;
     Eigen::Quaternionf orientation;
     int version;
 
     print_highlight(stderr, "Loading ");
-    print_value(stderr, "%s ", argv[p_file_indices.at(i)]);
+    print_value(stderr, "%s ", p_file.c_str());
 
     tt.tic();
     if (pcd.read(argv[p_file_indices.at(i)], *cloud, origin, orientation, version) < 0)
       return (-1);
 
-    std::stringstream cloud_name;
-
     // ---[ Special check for 1-point multi-dimension histograms
     if (cloud->fields.size() == 1 && isMultiDimensionalFeatureField(cloud->fields[0])) {
-      cloud_name << argv[p_file_indices.at(i)];
-
       if (!ph)
         ph.reset(new pcl::visualization::PCLHistogramVisualizer);
       print_info("[done, ");
@@ -638,12 +631,10 @@ main(int argc, char** argv)
       print_info(" points]\n");
 
       pcl::getMinMax(*cloud, 0, cloud->fields[0].name, min_p, max_p);
-      ph->addFeatureHistogram(*cloud, cloud->fields[0].name, cloud_name.str());
+      ph->addFeatureHistogram(*cloud, cloud->fields[0].name, p_file);
       continue;
     }
 
-    cloud_name << argv[p_file_indices.at(i)] << "-" << i;
-
     // Create the PCLVisualizer object here on the first encountered XYZ file
     if (!p) {
       p.reset(new pcl::visualization::PCLVisualizer(argc, argv, "PCD viewer"));
@@ -705,16 +696,16 @@ main(int argc, char** argv)
         new pcl::visualization::PointCloudGeometryHandlerXYZ<pcl::PCLPointCloud2>(
             cloud));
     // Add the cloud to the renderer
-    // p->addPointCloud<pcl::PointXYZ> (cloud_xyz, geometry_handler, color_handler,
-    // cloud_name.str (), viewport);
+    const std::string cloud_name = p_file + "-" + std::to_string(i);
     p->addPointCloud(cloud,
                      geometry_handler,
                      color_handler,
                      origin,
                      orientation,
-                     cloud_name.str(),
+                     cloud_name,
                      viewport);
 
+    const std::string cloud_name_normals = cloud_name + "-normals";
     // If normal lines are enabled
     if (normals != 0) {
       int normal_idx = pcl::getFieldIndex(*cloud, "normal_x");
@@ -732,13 +723,11 @@ main(int argc, char** argv)
 
       pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
       pcl::fromPCLPointCloud2(*cloud, *cloud_normals);
-      std::stringstream cloud_name_normals;
-      cloud_name_normals << argv[p_file_indices.at(i)] << "-" << i << "-normals";
       p->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud_xyz,
                                                           cloud_normals,
                                                           normals,
                                                           normals_scale,
-                                                          cloud_name_normals.str(),
+                                                          cloud_name_normals,
                                                           viewport);
     }
 
@@ -770,35 +759,28 @@ main(int argc, char** argv)
       pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr cloud_pc(
           new pcl::PointCloud<pcl::PrincipalCurvatures>);
       pcl::fromPCLPointCloud2(*cloud, *cloud_pc);
-      std::stringstream cloud_name_normals_pc;
-      cloud_name_normals_pc << argv[p_file_indices.at(i)] << "-" << i << "-normals";
       int factor = (std::min)(normals, pc);
       p->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud_xyz,
                                                           cloud_normals,
                                                           factor,
                                                           normals_scale,
-                                                          cloud_name_normals_pc.str(),
+                                                          cloud_name_normals,
                                                           viewport);
-      p->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,
-                                          1.0,
-                                          0.0,
-                                          0.0,
-                                          cloud_name_normals_pc.str());
-      p->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH,
-                                          3,
-                                          cloud_name_normals_pc.str());
-      cloud_name_normals_pc << "-pc";
+      p->setPointCloudRenderingProperties(
+          pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, cloud_name_normals);
+      p->setPointCloudRenderingProperties(
+          pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, cloud_name_normals);
+      const auto cloud_name_normals_pc = cloud_name_normals + "-pc";
       p->addPointCloudPrincipalCurvatures<pcl::PointXYZ, pcl::Normal>(
           cloud_xyz,
           cloud_normals,
           cloud_pc,
           factor,
           pc_scale,
-          cloud_name_normals_pc.str(),
+          cloud_name_normals_pc,
           viewport);
-      p->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH,
-                                          3,
-                                          cloud_name_normals_pc.str());
+      p->setPointCloudRenderingProperties(
+          pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, cloud_name_normals_pc);
     }
 
     // Add every dimension as a possible color
@@ -815,10 +797,8 @@ main(int argc, char** argv)
                   pcl::PCLPointCloud2>(cloud, cloud->fields[f].name));
         }
         // Add the cloud to the renderer
-        // p->addPointCloud<pcl::PointXYZ> (cloud_xyz, color_handler, cloud_name.str (),
-        // viewport);
         p->addPointCloud(
-            cloud, color_handler, origin, orientation, cloud_name.str(), viewport);
+            cloud, color_handler, origin, orientation, cloud_name, viewport);
       }
     }
     // Additionally, add normals as a handler
@@ -826,29 +806,22 @@ main(int argc, char** argv)
         new pcl::visualization::PointCloudGeometryHandlerSurfaceNormal<
             pcl::PCLPointCloud2>(cloud));
     if (geometry_handler->isCapable())
-      // p->addPointCloud<pcl::PointXYZ> (cloud_xyz, geometry_handler, cloud_name.str
-      // (), viewport);
       p->addPointCloud(
-          cloud, geometry_handler, origin, orientation, cloud_name.str(), viewport);
+          cloud, geometry_handler, origin, orientation, cloud_name, viewport);
 
     // Set immediate mode rendering ON
     p->setPointCloudRenderingProperties(
-        pcl::visualization::PCL_VISUALIZER_IMMEDIATE_RENDERING, 1.0, cloud_name.str());
+        pcl::visualization::PCL_VISUALIZER_IMMEDIATE_RENDERING, 1.0, cloud_name);
 
     // Change the cloud rendered point size
     if (!psize.empty())
       p->setPointCloudRenderingProperties(
-          pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize.at(i), cloud_name.str());
+          pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize.at(i), cloud_name);
 
     // Change the cloud rendered opacity
     if (!opaque.empty())
       p->setPointCloudRenderingProperties(
-          pcl::visualization::PCL_VISUALIZER_OPACITY, opaque.at(i), cloud_name.str());
-
-    // Reset camera viewpoint to center of cloud if camera parameters were not passed
-    // manually and this is the first loaded cloud
-    // if (i == 0 && !p->cameraParamsSet ())
-    // p->resetCameraViewpoint (cloud_name.str ());
+          pcl::visualization::PCL_VISUALIZER_OPACITY, opaque.at(i), cloud_name);
   }
 
   ////////////////////////////////////////////////////////////////
diff --git a/simulation/tools/simulation_io.hpp b/simulation/tools/simulation_io.hpp
index c74e2211..eb861202 100644
--- a/simulation/tools/simulation_io.hpp
+++ b/simulation/tools/simulation_io.hpp
@@ -1,6 +1,5 @@
 #pragma once
 
-#include <pcl/io/pcd_io.h>
 #include <pcl/io/vtk_lib_io.h>
 #include <pcl/simulation/camera.h>
 #include <pcl/simulation/range_likelihood.h>
diff --git a/surface/CMakeLists.txt b/surface/CMakeLists.txt
index e5e42311..d8a8566e 100644
--- a/surface/CMakeLists.txt
+++ b/surface/CMakeLists.txt
@@ -4,7 +4,7 @@ set(SUBSYS_DEPS common search kdtree octree)
 
 set(build TRUE)
 PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON)
-PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS qhull)
+PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS qhull vtk)
 
 PCL_ADD_DOC("${SUBSYS_NAME}")
 
@@ -49,6 +49,21 @@ endif()
 
 set(BUILD_surface_on_nurbs 0 CACHE BOOL "Fitting NURBS to point-clouds using openNURBS")
 if(BUILD_surface_on_nurbs)
+  # Add the GCC exclusive rules for -Werror only for 3rd party OpenNURBS compile
+  # -Wno-error = (deprecated-declarations, class-memaccess, deprecated-copy, uninitialized, parentheses)
+  # Note: class-memaccess warning added exactly in GCC 8.0, see here: https://www.gnu.org/software/gcc/gcc-8/changes.html
+  #       deprecated-copy warning added exactly in GCC 9.1, see here: https://www.gnu.org/software/gcc/gcc-9/changes.html
+  #       Since GCC8 and GCC9 are still widely-used, compile version check is required for the two options.
+  if(CMAKE_COMPILER_IS_GNUCXX)
+    add_compile_options("-Wno-error=deprecated-declarations" "-Wno-error=uninitialized" "-Wno-error=parentheses")
+    if (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 8.0)
+      add_compile_options("-Wno-error=class-memaccess")
+    endif()
+    if (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 9.1)
+      add_compile_options("-Wno-error=deprecated-copy")
+    endif()
+  endif()
+  
   include(src/3rdparty/opennurbs/openNURBS.cmake)
   include(src/on_nurbs/on_nurbs.cmake)
 endif()
@@ -149,19 +164,32 @@ set(impl_incs
 )
 
 set(LIB_NAME "pcl_${SUBSYS_NAME}")
+
 include_directories(
   "${CMAKE_CURRENT_SOURCE_DIR}/include"
   "${CMAKE_CURRENT_SOURCE_DIR}"
 )
-include_directories(SYSTEM
-  ${VTK_INCLUDE_DIRS}
-)
-link_directories(${VTK_LIBRARY_DIRS})
 PCL_ADD_LIBRARY(${LIB_NAME} COMPONENT ${SUBSYS_NAME} SOURCES ${srcs} ${incs} ${impl_incs} ${VTK_SMOOTHING_INCLUDES} ${POISSON_INCLUDES} ${OPENNURBS_INCLUDES} ${ON_NURBS_INCLUDES})
-target_link_libraries("${LIB_NAME}" pcl_common pcl_search pcl_kdtree pcl_octree ${VTK_LIBRARIES} ${ON_NURBS_LIBRARIES})
+
+target_link_libraries("${LIB_NAME}" pcl_common pcl_search pcl_kdtree pcl_octree ${ON_NURBS_LIBRARIES})
+
+if(VTK_FOUND)
+  if(${VTK_VERSION} VERSION_LESS 9.0)
+    include_directories(SYSTEM ${VTK_INCLUDE_DIRS})
+    link_directories(${VTK_LIBRARY_DIRS})
+    target_link_libraries("${LIB_NAME}" ${VTK_LIBRARIES})
+  else()
+    target_link_libraries("${LIB_NAME}" VTK::CommonDataModel
+                                        VTK::CommonExecutionModel
+                                        VTK::FiltersModeling
+                                        VTK::FiltersCore)
+  endif()
+endif()
+
 if(QHULL_FOUND)
   target_link_libraries("${LIB_NAME}" ${QHULL_LIBRARIES})
 endif()
+
 PCL_MAKE_PKGCONFIG(${LIB_NAME} COMPONENT ${SUBSYS_NAME} DESC ${SUBSYS_DESC} PCL_DEPS ${SUBSYS_DEPS})
 
 # Install include files
@@ -178,6 +206,7 @@ endif()
 if(VTK_FOUND AND NOT ANDROID)
   PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/vtk_smoothing" ${VTK_SMOOTHING_INCLUDES})
 endif()
+
 if(WIN32)
-  target_link_libraries("${LIB_NAME}" Rpcrt4.lib)
+  target_link_libraries("${LIB_NAME}" rpcrt4.lib)
 endif()
diff --git a/surface/include/pcl/surface/3rdparty/poisson4/marching_cubes_poisson.h b/surface/include/pcl/surface/3rdparty/poisson4/marching_cubes_poisson.h
index 26fedac8..bdd7fb04 100644
--- a/surface/include/pcl/surface/3rdparty/poisson4/marching_cubes_poisson.h
+++ b/surface/include/pcl/surface/3rdparty/poisson4/marching_cubes_poisson.h
@@ -29,7 +29,6 @@ DAMAGE.
 #ifndef MARCHING_CUBES_INCLUDED
 #define MARCHING_CUBES_INCLUDED
 #include <pcl/pcl_macros.h>
-#include <vector>
 #include "geometry.h"
 
 
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 5168eacf..10f49476 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
@@ -812,6 +812,7 @@ namespace pcl
           normal[0] = (*input_)[cnt].normal_x;
           normal[1] = (*input_)[cnt].normal_y;
           normal[2] = (*input_)[cnt].normal_z;
+          cnt++;
 
           for( i=0 ; i<DIMENSION ; i++ ) position[i] = ( position[i]-center[i] ) / scale;
           myCenter[0] = myCenter[1] = myCenter[2] = Real(0.5);
@@ -838,7 +839,6 @@ namespace pcl
             d++;
           }
           NonLinearUpdateWeightContribution( temp , position , weight );
-          cnt++;
         }
       }
 
diff --git a/surface/include/pcl/surface/3rdparty/poisson4/octree_poisson.hpp b/surface/include/pcl/surface/3rdparty/poisson4/octree_poisson.hpp
index 9cae3118..7ed8aaf9 100644
--- a/surface/include/pcl/surface/3rdparty/poisson4/octree_poisson.hpp
+++ b/surface/include/pcl/surface/3rdparty/poisson4/octree_poisson.hpp
@@ -27,7 +27,6 @@ DAMAGE.
 */
 
 #include <stdlib.h>
-#include <math.h>
 #include <algorithm>
 
 #include "poisson_exceptions.h"
diff --git a/surface/include/pcl/surface/3rdparty/poisson4/polynomial.hpp b/surface/include/pcl/surface/3rdparty/poisson4/polynomial.hpp
index 1d7bffa7..37a32502 100644
--- a/surface/include/pcl/surface/3rdparty/poisson4/polynomial.hpp
+++ b/surface/include/pcl/surface/3rdparty/poisson4/polynomial.hpp
@@ -31,8 +31,6 @@ DAMAGE.
 #include <float.h>
 #include <math.h>
 
-#include <algorithm>
-
 #include <cstdio>
 #include <cstring>
 
diff --git a/surface/include/pcl/surface/3rdparty/poisson4/ppolynomial.hpp b/surface/include/pcl/surface/3rdparty/poisson4/ppolynomial.hpp
index 6d1d78a4..8c7efbf2 100644
--- a/surface/include/pcl/surface/3rdparty/poisson4/ppolynomial.hpp
+++ b/surface/include/pcl/surface/3rdparty/poisson4/ppolynomial.hpp
@@ -29,6 +29,7 @@ DAMAGE.
 #include "factor.h"
 
 #include <cstdio>
+#include <cstdlib>  // for malloc, needed by gcc-5
 #include <cstring>
 
 ////////////////////////
diff --git a/surface/include/pcl/surface/3rdparty/poisson4/sparse_matrix.hpp b/surface/include/pcl/surface/3rdparty/poisson4/sparse_matrix.hpp
index e8fae969..e6564cdc 100644
--- a/surface/include/pcl/surface/3rdparty/poisson4/sparse_matrix.hpp
+++ b/surface/include/pcl/surface/3rdparty/poisson4/sparse_matrix.hpp
@@ -26,7 +26,6 @@ ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF S
 DAMAGE.
 */
 
-#include <float.h>
 #ifdef _WIN32
 # ifndef WIN32_LEAN_AND_MEAN
 #  define WIN32_LEAN_AND_MEAN
diff --git a/surface/include/pcl/surface/boost.h b/surface/include/pcl/surface/boost.h
index eb076d1f..86aca4ea 100644
--- a/surface/include/pcl/surface/boost.h
+++ b/surface/include/pcl/surface/boost.h
@@ -42,5 +42,6 @@
 #if defined __GNUC__
 #  pragma GCC system_header
 #endif
+PCL_DEPRECATED_HEADER(1, 15, "Please include the needed boost headers directly.")
 
 #include <boost/dynamic_bitset/dynamic_bitset.hpp>
diff --git a/surface/include/pcl/surface/concave_hull.h b/surface/include/pcl/surface/concave_hull.h
index b53fd7b9..c9640a6a 100644
--- a/surface/include/pcl/surface/concave_hull.h
+++ b/surface/include/pcl/surface/concave_hull.h
@@ -42,7 +42,7 @@
 #include <pcl/pcl_config.h>
 #ifdef HAVE_QHULL
 
-#include <pcl/surface/convex_hull.h>
+#include <pcl/surface/reconstruction.h> // for MeshConstruction
 
 namespace pcl
 {
diff --git a/surface/include/pcl/surface/convex_hull.h b/surface/include/pcl/surface/convex_hull.h
index 2db2e92a..917b42c4 100644
--- a/surface/include/pcl/surface/convex_hull.h
+++ b/surface/include/pcl/surface/convex_hull.h
@@ -46,7 +46,6 @@
 
 // PCL includes
 #include <pcl/surface/reconstruction.h>
-#include <pcl/ModelCoefficients.h>
 #include <pcl/PolygonMesh.h>
 
 namespace pcl
@@ -89,11 +88,11 @@ namespace pcl
       using PointCloudConstPtr = typename PointCloud::ConstPtr;
 
       /** \brief Empty constructor. */
-      ConvexHull () : compute_area_ (false), total_area_ (0), total_volume_ (0), dimension_ (0), 
+      ConvexHull () : compute_area_ (false), total_area_ (0), total_volume_ (0), dimension_ (0),
                       projection_angle_thresh_ (std::cos (0.174532925) ), qhull_flags ("qhull "),
                       x_axis_ (1.0, 0.0, 0.0), y_axis_ (0.0, 1.0, 0.0), z_axis_ (0.0, 0.0, 1.0)
       {
-      };
+      }
       
       /** \brief Empty destructor */
       ~ConvexHull () {}
diff --git a/surface/include/pcl/surface/ear_clipping.h b/surface/include/pcl/surface/ear_clipping.h
index c6a54e86..e811a706 100644
--- a/surface/include/pcl/surface/ear_clipping.h
+++ b/surface/include/pcl/surface/ear_clipping.h
@@ -87,7 +87,18 @@ namespace pcl
         * \param[in] vertices the vertices representing the polygon 
         */
       float
-      area (const std::vector<std::uint32_t>& vertices);
+      area (const Indices& vertices);
+
+      /** \brief Compute the signed area of a polygon.
+        * \param[in] vertices the vertices representing the polygon
+        */
+      template <typename T = pcl::index_t, std::enable_if_t<!std::is_same<T, std::uint32_t>::value, pcl::index_t> = 0>
+      PCL_DEPRECATED(1, 14, "This method creates a useless copy of the vertices vector. Use  area method which accepts Indices instead")
+      float
+      area (const std::vector<std::uint32_t>& vertices)
+      {
+        return area(Indices (vertices.cbegin(), vertices.cend()));
+      }
 
       /** \brief Check if the triangle (u,v,w) is an ear. 
         * \param[in] u the first triangle vertex 
@@ -96,7 +107,21 @@ namespace pcl
         * \param[in] vertices a set of input vertices
         */
       bool
-      isEar (int u, int v, int w, const std::vector<std::uint32_t>& vertices);
+      isEar (int u, int v, int w, const Indices& vertices);
+
+      /** \brief Check if the triangle (u,v,w) is an ear. 
+        * \param[in] u the first triangle vertex 
+        * \param[in] v the second triangle vertex 
+        * \param[in] w the third triangle vertex 
+        * \param[in] vertices a set of input vertices
+        */
+      template <typename T = pcl::index_t, std::enable_if_t<!std::is_same<T, std::uint32_t>::value, pcl::index_t> = 0>
+      PCL_DEPRECATED(1, 14, "This method creates a useless copy of the vertices vector. Use  isEar method which accepts Indices instead")
+      bool
+      isEar (int u, int v, int w, const std::vector<std::uint32_t>& vertices)
+      {
+        return isEar(u, v, w, Indices (vertices.cbegin(), vertices.cend()));
+      }
 
       /** \brief Check if p is inside the triangle (u,v,w). 
         * \param[in] u the first triangle vertex 
diff --git a/surface/include/pcl/surface/eigen.h b/surface/include/pcl/surface/eigen.h
index c1cad11d..e3f46b0d 100644
--- a/surface/include/pcl/surface/eigen.h
+++ b/surface/include/pcl/surface/eigen.h
@@ -42,5 +42,6 @@
 #if defined __GNUC__
 #  pragma GCC system_header 
 #endif
+PCL_DEPRECATED_HEADER(1, 15, "Please include the needed eigen headers directly.")
 
 #include <Eigen/SVD>
diff --git a/surface/include/pcl/surface/gp3.h b/surface/include/pcl/surface/gp3.h
index 08505c99..27be42cc 100644
--- a/surface/include/pcl/surface/gp3.h
+++ b/surface/include/pcl/surface/gp3.h
@@ -46,7 +46,7 @@
 
 #include <fstream>
 
-
+#include <Eigen/Geometry> // for cross
 
 namespace pcl
 {
@@ -278,11 +278,11 @@ namespace pcl
 
 
       /** \brief Get the sfn list. */
-      inline std::vector<int>
+      inline pcl::Indices
       getSFN () const { return (sfn_); }
 
       /** \brief Get the ffn list. */
-      inline std::vector<int>
+      inline pcl::Indices
       getFFN () const { return (ffn_); }
 
     protected:
@@ -315,8 +315,8 @@ namespace pcl
       struct nnAngle
       {
         double angle;
-        int index;
-        int nnIndex;
+        pcl::index_t index;
+        pcl::index_t nnIndex;
         bool visible;
       };
 
@@ -339,15 +339,15 @@ namespace pcl
       /** \brief A list of angles to neighbors **/
       std::vector<nnAngle> angles_;
       /** \brief Index of the current query point **/
-      int R_;
+      pcl::index_t R_;
       /** \brief List of point states **/
       std::vector<int> state_;
       /** \brief List of sources **/
-      std::vector<int> source_;
+      pcl::Indices source_;
       /** \brief List of fringe neighbors in one direction **/
-      std::vector<int> ffn_;
+      pcl::Indices ffn_;
       /** \brief List of fringe neighbors in other direction **/
-      std::vector<int> sfn_;
+      pcl::Indices sfn_;
       /** \brief Connected component labels for each point **/
       std::vector<int> part_;
       /** \brief Points on the outer edge from which the mesh has to be grown **/
@@ -356,7 +356,7 @@ namespace pcl
       /** \brief Flag to set if the current point is free **/
       bool is_current_free_;
       /** \brief Current point's index **/
-      int current_index_;
+      pcl::index_t current_index_;
       /** \brief Flag to set if the previous point is the first fringe neighbor **/
       bool prev_is_ffn_;
       /** \brief Flag to set if the next point is the second fringe neighbor **/
@@ -370,7 +370,7 @@ namespace pcl
       /** \brief Flag to set if the second fringe neighbor was changed **/
       bool changed_2nd_fn_;
       /** \brief New boundary point **/
-      int new2boundary_;
+      pcl::index_t new2boundary_;
       
       /** \brief Flag to set if the next neighbor was already connected in the previous step.
         * To avoid inconsistency it should not be connected again.
@@ -429,9 +429,9 @@ namespace pcl
         */
       void 
       connectPoint (std::vector<pcl::Vertices> &polygons, 
-                    const int prev_index, 
-                    const int next_index, 
-                    const int next_next_index, 
+                    const pcl::index_t prev_index, 
+                    const pcl::index_t next_index, 
+                    const pcl::index_t next_next_index, 
                     const Eigen::Vector2f &uvn_current, 
                     const Eigen::Vector2f &uvn_prev, 
                     const Eigen::Vector2f &uvn_next);
@@ -456,7 +456,7 @@ namespace pcl
         * \param[out] polygons the polygon mesh to be updated
         */
       inline void
-      addTriangle (int a, int b, int c, std::vector<pcl::Vertices> &polygons)
+      addTriangle (pcl::index_t a, pcl::index_t b, pcl::index_t c, std::vector<pcl::Vertices> &polygons)
       {
         triangle_.vertices.resize (3);
         if (consistent_ordering_)
diff --git a/surface/include/pcl/surface/grid_projection.h b/surface/include/pcl/surface/grid_projection.h
index e1d5db84..92f04aae 100644
--- a/surface/include/pcl/surface/grid_projection.h
+++ b/surface/include/pcl/surface/grid_projection.h
@@ -39,9 +39,9 @@
 
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
-#include <pcl/surface/boost.h>
 #include <pcl/surface/reconstruction.h>
 
+#include <boost/dynamic_bitset/dynamic_bitset.hpp> // for dynamic_bitset
 #include <unordered_map>
 
 namespace pcl
@@ -91,7 +91,7 @@ namespace pcl
       {
         Leaf () {}
 
-        std::vector<int> data_indices;
+        pcl::Indices data_indices;
         Eigen::Vector4f pt_on_surface; 
         Eigen::Vector3f vect_at_grid_pt;
       };
@@ -315,7 +315,7 @@ namespace pcl
         * \param pt_union_indices the union of input data points within the cell and padding cells
         */
       void 
-      getDataPtsUnion (const Eigen::Vector3i &index, std::vector <int> &pt_union_indices);
+      getDataPtsUnion (const Eigen::Vector3i &index, pcl::Indices &pt_union_indices);
 
       /** \brief Given the index of a cell, exam it's up, left, front edges, and add
         * the vectices to m_surface list.the up, left, front edges only share 4
@@ -324,7 +324,7 @@ namespace pcl
         * \param pt_union_indices the union of input data points within the cell and padding cells
         */
       void 
-      createSurfaceForCell (const Eigen::Vector3i &index, std::vector <int> &pt_union_indices);
+      createSurfaceForCell (const Eigen::Vector3i &index, pcl::Indices &pt_union_indices);
 
 
       /** \brief Given the coordinates of one point, project it onto the surface, 
@@ -335,7 +335,7 @@ namespace pcl
         * \param projection the resultant point projected
         */
       void
-      getProjection (const Eigen::Vector4f &p, std::vector<int> &pt_union_indices, Eigen::Vector4f &projection);
+      getProjection (const Eigen::Vector4f &p, pcl::Indices &pt_union_indices, Eigen::Vector4f &projection);
 
       /** \brief Given the coordinates of one point, project it onto the surface,
         * return the projected point. Find the plane which fits all the points in
@@ -346,7 +346,7 @@ namespace pcl
         */
       void 
       getProjectionWithPlaneFit (const Eigen::Vector4f &p, 
-                                 std::vector<int> &pt_union_indices, 
+                                 pcl::Indices &pt_union_indices, 
                                  Eigen::Vector4f &projection);
 
 
@@ -357,7 +357,7 @@ namespace pcl
         */
       void
       getVectorAtPoint (const Eigen::Vector4f &p, 
-                        std::vector <int> &pt_union_indices, Eigen::Vector3f &vo);
+                        pcl::Indices &pt_union_indices, Eigen::Vector3f &vo);
 
       /** \brief Given the location of a point, get it's vector
         * \param p the coordinates of the input point
@@ -368,7 +368,7 @@ namespace pcl
         */
       void
       getVectorAtPointKNN (const Eigen::Vector4f &p, 
-                           std::vector<int> &k_indices, 
+                           pcl::Indices &k_indices, 
                            std::vector<float> &k_squared_distances,
                            Eigen::Vector3f &vo);
 
@@ -377,7 +377,7 @@ namespace pcl
         * \param pt_union_indices the union of input data points within the cell and padding cells
         */
       double 
-      getMagAtPoint (const Eigen::Vector4f &p, const std::vector <int> &pt_union_indices);
+      getMagAtPoint (const Eigen::Vector4f &p, const pcl::Indices &pt_union_indices);
 
       /** \brief Get the 1st derivative
         * \param p the coordinate of the input point
@@ -386,7 +386,7 @@ namespace pcl
         */
       double 
       getD1AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec, 
-                    const std::vector <int> &pt_union_indices);
+                    const pcl::Indices &pt_union_indices);
 
       /** \brief Get the 2nd derivative
         * \param p the coordinate of the input point
@@ -395,7 +395,7 @@ namespace pcl
         */
       double 
       getD2AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec, 
-                    const std::vector <int> &pt_union_indices);
+                    const pcl::Indices &pt_union_indices);
 
       /** \brief Test whether the edge is intersected by the surface by 
         * doing the dot product of the vector at two end points. Also test 
@@ -408,7 +408,7 @@ namespace pcl
       bool 
       isIntersected (const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts, 
                      std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts, 
-                     std::vector <int> &pt_union_indices);
+                     pcl::Indices &pt_union_indices);
 
       /** \brief Find point where the edge intersects the surface.
         * \param level binary search level
@@ -423,7 +423,7 @@ namespace pcl
                         const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts, 
                         const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts, 
                         const Eigen::Vector4f &start_pt, 
-                        std::vector<int> &pt_union_indices,
+                        pcl::Indices &pt_union_indices,
                         Eigen::Vector4f &intersection);
 
       /** \brief Go through all the entries in the hash table and update the
@@ -442,7 +442,7 @@ namespace pcl
        */
       void
       storeVectAndSurfacePoint (int index_1d, const Eigen::Vector3i &index_3d, 
-                                std::vector<int> &pt_union_indices, const Leaf &cell_data);
+                                pcl::Indices &pt_union_indices, const Leaf &cell_data);
 
       /** \brief Go through all the entries in the hash table and update the cellData. 
         * When creating the hash table, the pt_on_surface field store the center point
diff --git a/surface/include/pcl/surface/impl/bilateral_upsampling.hpp b/surface/include/pcl/surface/impl/bilateral_upsampling.hpp
index e8ff3fd2..faf636dc 100644
--- a/surface/include/pcl/surface/impl/bilateral_upsampling.hpp
+++ b/surface/include/pcl/surface/impl/bilateral_upsampling.hpp
@@ -43,6 +43,8 @@
 #include <algorithm>
 #include <pcl/console/print.h>
 
+#include <Eigen/LU> // for inverse
+
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointInT, typename PointOutT> void
 pcl::BilateralUpsampling<PointInT, PointOutT>::process (pcl::PointCloud<PointOutT> &output)
@@ -53,7 +55,7 @@ pcl::BilateralUpsampling<PointInT, PointOutT>::process (pcl::PointCloud<PointOut
   if (!initCompute ())
   {
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
diff --git a/surface/include/pcl/surface/impl/concave_hull.hpp b/surface/include/pcl/surface/impl/concave_hull.hpp
index 02591d79..0019b2d3 100644
--- a/surface/include/pcl/surface/impl/concave_hull.hpp
+++ b/surface/include/pcl/surface/impl/concave_hull.hpp
@@ -62,13 +62,13 @@ pcl::ConcaveHull<PointInT>::reconstruct (PointCloud &output)
   if (alpha_ <= 0)
   {
     PCL_ERROR ("[pcl::%s::reconstruct] Alpha parameter must be set to a positive number!\n", getClassName ().c_str ());
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
   if (!initCompute ())
   {
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
@@ -91,13 +91,13 @@ pcl::ConcaveHull<PointInT>::reconstruct (PointCloud &output, std::vector<pcl::Ve
   if (alpha_ <= 0)
   {
     PCL_ERROR ("[pcl::%s::reconstruct] Alpha parameter must be set to a positive number!\n", getClassName ().c_str ());
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
   if (!initCompute ())
   {
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
@@ -194,8 +194,13 @@ pcl::ConcaveHull<PointInT>::performReconstruction (PointCloud &alpha_shape, std:
       points[i * dim_ + 2] = static_cast<coordT> (cloud_transformed[i].z);
   }
 
+  qhT qh_qh;
+  qhT* qh = &qh_qh;
+  QHULL_LIB_CHECK
+  qh_zero(qh, errfile);
+
   // Compute concave hull
-  exitcode = qh_new_qhull (dim_, static_cast<int> (cloud_transformed.size ()), points, ismalloc, flags, outfile, errfile);
+  exitcode = qh_new_qhull (qh, dim_, static_cast<int> (cloud_transformed.size ()), points, ismalloc, flags, outfile, errfile);
 
   if (exitcode != 0)
   {
@@ -223,21 +228,21 @@ pcl::ConcaveHull<PointInT>::performReconstruction (PointCloud &alpha_shape, std:
         PCL_ERROR ("[pcl::%s::performReconstruction] ERROR: point cloud contains NaN values, consider running pcl::PassThrough filter first to remove NaNs!\n", getClassName ().c_str ());
     }
 
-    alpha_shape.points.resize (0);
+    alpha_shape.resize (0);
     alpha_shape.width = alpha_shape.height = 0;
     polygons.resize (0);
 
-    qh_freeqhull (!qh_ALL);
+    qh_freeqhull (qh, !qh_ALL);
     int curlong, totlong;
-    qh_memfreeshort (&curlong, &totlong);
+    qh_memfreeshort (qh, &curlong, &totlong);
 
     return;
   }
 
-  qh_setvoronoi_all ();
+  qh_setvoronoi_all (qh);
 
-  int num_vertices = qh num_vertices;
-  alpha_shape.points.resize (num_vertices);
+  int num_vertices = qh->num_vertices;
+  alpha_shape.resize (num_vertices);
 
   vertexT *vertex;
   // Max vertex id
@@ -253,11 +258,11 @@ pcl::ConcaveHull<PointInT>::performReconstruction (PointCloud &alpha_shape, std:
   ++max_vertex_id;
   std::vector<int> qhid_to_pcidx (max_vertex_id);
 
-  int num_facets = qh num_facets;
+  int num_facets = qh->num_facets;
 
   if (dim_ == 3)
   {
-    setT *triangles_set = qh_settemp (4 * num_facets);
+    setT *triangles_set = qh_settemp (qh, 4 * num_facets);
     if (voronoi_centers_)
       voronoi_centers_->points.resize (num_facets);
 
@@ -283,29 +288,29 @@ pcl::ConcaveHull<PointInT>::performReconstruction (PointCloud &alpha_shape, std:
         if (r <= alpha_)
         {
           // all triangles in tetrahedron are good, add them all to the alpha shape (triangles_set)
-          qh_makeridges (facet);
+          qh_makeridges (qh, facet);
           facet->good = true;
-          facet->visitid = qh visit_id;
+          facet->visitid = qh->visit_id;
           ridgeT *ridge, **ridgep;
           FOREACHridge_ (facet->ridges)
           {
             facetT *neighb = otherfacet_ (ridge, facet);
-            if ((neighb->visitid != qh visit_id))
-              qh_setappend (&triangles_set, ridge);
+            if ((neighb->visitid != qh->visit_id))
+              qh_setappend (qh, &triangles_set, ridge);
           }
         }
         else
         {
           // consider individual triangles from the tetrahedron...
           facet->good = false;
-          facet->visitid = qh visit_id;
-          qh_makeridges (facet);
+          facet->visitid = qh->visit_id;
+          qh_makeridges (qh, facet);
           ridgeT *ridge, **ridgep;
           FOREACHridge_ (facet->ridges)
           {
             facetT *neighb;
             neighb = otherfacet_ (ridge, facet);
-            if ((neighb->visitid != qh visit_id))
+            if ((neighb->visitid != qh->visit_id))
             {
               // check if individual triangle is good and add it to triangles_set
 
@@ -322,7 +327,7 @@ pcl::ConcaveHull<PointInT>::performReconstruction (PointCloud &alpha_shape, std:
 
               double r = pcl::getCircumcircleRadius (a, b, c);
               if (r <= alpha_)
-                qh_setappend (&triangles_set, ridge);
+                qh_setappend (qh, &triangles_set, ridge);
             }
           }
         }
@@ -354,7 +359,7 @@ pcl::ConcaveHull<PointInT>::performReconstruction (PointCloud &alpha_shape, std:
       {
         polygons[triangles].vertices.resize (3);
         int vertex_n, vertex_i;
-        FOREACHvertex_i_ ((*ridge).vertices)  //3 vertices per ridge!
+        FOREACHvertex_i_ (qh, (*ridge).vertices)  //3 vertices per ridge!
         {
           if (!added_vertices[vertex->id])
           {
@@ -375,7 +380,7 @@ pcl::ConcaveHull<PointInT>::performReconstruction (PointCloud &alpha_shape, std:
       }
     }
 
-    alpha_shape.points.resize (vertices);
+    alpha_shape.resize (vertices);
     alpha_shape.width = alpha_shape.size ();
     alpha_shape.height = 1;
   }
@@ -383,7 +388,7 @@ pcl::ConcaveHull<PointInT>::performReconstruction (PointCloud &alpha_shape, std:
   {
     // Compute the alpha complex for the set of points
     // Filters the delaunay triangles
-    setT *edges_set = qh_settemp (3 * num_facets);
+    setT *edges_set = qh_settemp (qh, 3 * num_facets);
     if (voronoi_centers_)
       voronoi_centers_->points.resize (num_facets);
 
@@ -403,12 +408,12 @@ pcl::ConcaveHull<PointInT>::performReconstruction (PointCloud &alpha_shape, std:
         if (r <= alpha_)
         {
           pcl::Vertices facet_vertices;   //TODO: is not used!!
-          qh_makeridges (facet);
+          qh_makeridges (qh, facet);
           facet->good = true;
 
           ridgeT *ridge, **ridgep;
           FOREACHridge_ (facet->ridges)
-          qh_setappend (&edges_set, ridge);
+          qh_setappend (qh, &edges_set, ridge);
 
           if (voronoi_centers_)
           {
@@ -438,7 +443,7 @@ pcl::ConcaveHull<PointInT>::performReconstruction (PointCloud &alpha_shape, std:
         std::vector<int> pcd_indices;
         pcd_indices.resize (2);
 
-        FOREACHvertex_i_ ((*ridge).vertices)  //in 2-dim, 2 vertices per ridge!
+        FOREACHvertex_i_ (qh, (*ridge).vertices)  //in 2-dim, 2 vertices per ridge!
         {
           if (!added_vertices[vertex->id])
           {
@@ -469,10 +474,10 @@ pcl::ConcaveHull<PointInT>::performReconstruction (PointCloud &alpha_shape, std:
       }
     }
 
-    alpha_shape.points.resize (vertices);
+    alpha_shape.resize (vertices);
 
     PointCloud alpha_shape_sorted;
-    alpha_shape_sorted.points.resize (vertices);
+    alpha_shape_sorted.resize (vertices);
 
     // iterate over edges until they are empty!
     std::map<int, std::vector<int> >::iterator curr = edges.begin ();
@@ -487,7 +492,7 @@ pcl::ConcaveHull<PointInT>::performReconstruction (PointCloud &alpha_shape, std:
     {
       alpha_shape_sorted[sorted_idx] = alpha_shape[(*curr).first];
       // check where we can go from (*curr).first
-      for (const int &i : (*curr).second)
+      for (const auto &i : (*curr).second)
       {
         if (!used[i])
         {
@@ -540,9 +545,9 @@ pcl::ConcaveHull<PointInT>::performReconstruction (PointCloud &alpha_shape, std:
       voronoi_centers_->points.resize (dd);
   }
 
-  qh_freeqhull (!qh_ALL);
+  qh_freeqhull (qh, !qh_ALL);
   int curlong, totlong;
-  qh_memfreeshort (&curlong, &totlong);
+  qh_memfreeshort (qh, &curlong, &totlong);
 
   Eigen::Affine3d transInverse = transform1.inverse ();
   pcl::transformPointCloud (alpha_shape, alpha_shape, transInverse);
@@ -564,7 +569,7 @@ pcl::ConcaveHull<PointInT>::performReconstruction (PointCloud &alpha_shape, std:
     pcl::KdTreeFLANN<PointInT> tree (true);
     tree.setInputCloud (input_, indices_);
 
-    std::vector<int> neighbor;
+    pcl::Indices neighbor;
     std::vector<float> distances;
     neighbor.resize (1);
     distances.resize (1);
diff --git a/surface/include/pcl/surface/impl/convex_hull.hpp b/surface/include/pcl/surface/impl/convex_hull.hpp
index ef8330ac..aa36cdd9 100644
--- a/surface/include/pcl/surface/impl/convex_hull.hpp
+++ b/surface/include/pcl/surface/impl/convex_hull.hpp
@@ -95,7 +95,7 @@ pcl::ConvexHull<PointInT>::performReconstruction2D (PointCloud &hull, std::vecto
   }
     
   pcl::PointCloud<PointInT> normal_calc_cloud;
-  normal_calc_cloud.points.resize (3);
+  normal_calc_cloud.resize (3);
   normal_calc_cloud[0] = p0;
   normal_calc_cloud[1] = p1;
   normal_calc_cloud[2] = p2;
@@ -136,10 +136,8 @@ pcl::ConvexHull<PointInT>::performReconstruction2D (PointCloud &hull, std::vecto
   // output from qh_produce_output(), use NULL to skip qh_produce_output()
   FILE *outfile = nullptr;
 
-#ifndef HAVE_QHULL_2011
   if (compute_area_)
     outfile = stderr;
-#endif
 
   // option flags for qhull, see qh_opt.htm
   const char* flags = qhull_flags.c_str ();
@@ -180,28 +178,31 @@ pcl::ConvexHull<PointInT>::performReconstruction2D (PointCloud &hull, std::vecto
     // This should only happen if we had invalid input
     PCL_ERROR ("[pcl::%s::performReconstruction2D] Invalid input!\n", getClassName ().c_str ());
   }
+
+  qhT qh_qh;
+  qhT* qh = &qh_qh;
+  QHULL_LIB_CHECK
+  qh_zero(qh, errfile);
    
   // Compute convex hull
-  int exitcode = qh_new_qhull (dimension, static_cast<int> (indices_->size ()), points, ismalloc, const_cast<char*> (flags), outfile, errfile);
-#ifdef HAVE_QHULL_2011
+  int exitcode = qh_new_qhull (qh, dimension, static_cast<int> (indices_->size ()), points, ismalloc, const_cast<char*> (flags), outfile, errfile);
   if (compute_area_)
   {
-    qh_prepare_output();
+    qh_prepare_output(qh);
   }
-#endif
     
   // 0 if no error from qhull or it doesn't find any vertices
-  if (exitcode != 0 || qh num_vertices == 0)
+  if (exitcode != 0 || qh->num_vertices == 0)
   {
     PCL_ERROR ("[pcl::%s::performReconstrution2D] ERROR: qhull was unable to compute a convex hull for the given point cloud (%lu)!\n", getClassName ().c_str (), indices_->size ());
 
-    hull.points.resize (0);
+    hull.resize (0);
     hull.width = hull.height = 0;
     polygons.resize (0);
 
-    qh_freeqhull (!qh_ALL);
+    qh_freeqhull (qh, !qh_ALL);
     int curlong, totlong;
-    qh_memfreeshort (&curlong, &totlong);
+    qh_memfreeshort (qh, &curlong, &totlong);
 
     return;
   }
@@ -209,25 +210,24 @@ pcl::ConvexHull<PointInT>::performReconstruction2D (PointCloud &hull, std::vecto
   // Qhull returns the area in volume for 2D
   if (compute_area_)
   {
-    total_area_ = qh totvol;
+    total_area_ = qh->totvol;
     total_volume_ = 0.0;
   }
 
-  int num_vertices = qh num_vertices;
-  hull.points.resize (num_vertices);
-  memset (&hull.points[0], hull.size (), sizeof (PointInT));
+  int num_vertices = qh->num_vertices;
+
+  hull.clear();
+  hull.resize(num_vertices, PointInT{});
 
   vertexT * vertex;
   int i = 0;
 
-  std::vector<std::pair<int, Eigen::Vector4f>, Eigen::aligned_allocator<std::pair<int, Eigen::Vector4f> > > idx_points (num_vertices);
-  idx_points.resize (hull.size ());
-  memset (&idx_points[0], hull.size (), sizeof (std::pair<int, Eigen::Vector4f>));
+  AlignedVector<std::pair<int, Eigen::Vector4f>> idx_points (num_vertices);
 
   FORALLvertices
   {
-    hull[i] = (*input_)[(*indices_)[qh_pointid (vertex->point)]];
-    idx_points[i].first = qh_pointid (vertex->point);
+    hull[i] = (*input_)[(*indices_)[qh_pointid (qh, vertex->point)]];
+    idx_points[i].first = qh_pointid (qh, vertex->point);
     ++i;
   }
 
@@ -274,9 +274,9 @@ pcl::ConvexHull<PointInT>::performReconstruction2D (PointCloud &hull, std::vecto
     polygons[0].vertices[j] = static_cast<unsigned int> (j);
   }
     
-  qh_freeqhull (!qh_ALL);
+  qh_freeqhull (qh, !qh_ALL);
   int curlong, totlong;
-  qh_memfreeshort (&curlong, &totlong);
+  qh_memfreeshort (qh, &curlong, &totlong);
 
   hull.width = hull.size ();
   hull.height = 1;
@@ -299,10 +299,8 @@ pcl::ConvexHull<PointInT>::performReconstruction3D (
   // output from qh_produce_output(), use NULL to skip qh_produce_output()
   FILE *outfile = nullptr;
 
-#ifndef HAVE_QHULL_2011
   if (compute_area_)
     outfile = stderr;
-#endif
 
   // option flags for qhull, see qh_opt.htm
   const char *flags = qhull_flags.c_str ();
@@ -320,14 +318,17 @@ pcl::ConvexHull<PointInT>::performReconstruction3D (
     points[j + 2] = static_cast<coordT> ((*input_)[(*indices_)[i]].z);
   }
 
+  qhT qh_qh;
+  qhT* qh = &qh_qh;
+  QHULL_LIB_CHECK
+  qh_zero(qh, errfile);
+
   // Compute convex hull
-  int exitcode = qh_new_qhull (dimension, static_cast<int> (indices_->size ()), points, ismalloc, const_cast<char*> (flags), outfile, errfile);
-#ifdef HAVE_QHULL_2011
+  int exitcode = qh_new_qhull (qh, dimension, static_cast<int> (indices_->size ()), points, ismalloc, const_cast<char*> (flags), outfile, errfile);
   if (compute_area_)
   {
-    qh_prepare_output();
+    qh_prepare_output(qh);
   }
-#endif
 
   // 0 if no error from qhull
   if (exitcode != 0)
@@ -337,23 +338,23 @@ pcl::ConvexHull<PointInT>::performReconstruction3D (
               getClassName().c_str(),
               static_cast<std::size_t>(input_->size()));
 
-    hull.points.resize (0);
+    hull.resize (0);
     hull.width = hull.height = 0;
     polygons.resize (0);
 
-    qh_freeqhull (!qh_ALL);
+    qh_freeqhull (qh, !qh_ALL);
     int curlong, totlong;
-    qh_memfreeshort (&curlong, &totlong);
+    qh_memfreeshort (qh, &curlong, &totlong);
 
     return;
   }
 
-  qh_triangulate ();
+  qh_triangulate (qh);
 
-  int num_facets = qh num_facets;
+  int num_facets = qh->num_facets;
 
-  int num_vertices = qh num_vertices;
-  hull.points.resize (num_vertices);
+  int num_vertices = qh->num_vertices;
+  hull.resize (num_vertices);
 
   vertexT * vertex;
   int i = 0;
@@ -375,7 +376,7 @@ pcl::ConvexHull<PointInT>::performReconstruction3D (
   FORALLvertices
   {
     // Add vertices to hull point_cloud and store index
-    hull_indices_.indices.push_back ((*indices_)[qh_pointid (vertex->point)]);
+    hull_indices_.indices.push_back ((*indices_)[qh_pointid (qh, vertex->point)]);
     hull[i] = (*input_)[hull_indices_.indices.back ()];
 
     qhid_to_pcidx[vertex->id] = i; // map the vertex id of qhull to the point cloud index
@@ -384,8 +385,8 @@ pcl::ConvexHull<PointInT>::performReconstruction3D (
 
   if (compute_area_)
   {
-    total_area_  = qh totarea;
-    total_volume_ = qh totvol;
+    total_area_  = qh->totarea;
+    total_volume_ = qh->totvol;
   }
 
   if (fill_polygon_data)
@@ -400,16 +401,16 @@ pcl::ConvexHull<PointInT>::performReconstruction3D (
 
       // Needed by FOREACHvertex_i_
       int vertex_n, vertex_i;
-      FOREACHvertex_i_ ((*facet).vertices)
+      FOREACHvertex_i_ (qh, (*facet).vertices)
       //facet_vertices.vertices.push_back (qhid_to_pcidx[vertex->id]);
       polygons[dd].vertices[vertex_i] = qhid_to_pcidx[vertex->id];
       ++dd;
     }
   }
   // Deallocates memory (also the points)
-  qh_freeqhull (!qh_ALL);
+  qh_freeqhull (qh, !qh_ALL);
   int curlong, totlong;
-  qh_memfreeshort (&curlong, &totlong);
+  qh_memfreeshort (qh, &curlong, &totlong);
 
   hull.width = hull.size ();
   hull.height = 1;
@@ -441,7 +442,7 @@ pcl::ConvexHull<PointInT>::reconstruct (PointCloud &points)
   points.header = input_->header;
   if (!initCompute () || input_->points.empty () || indices_->empty ())
   {
-    points.points.clear ();
+    points.clear ();
     return;
   }
 
@@ -484,7 +485,7 @@ pcl::ConvexHull<PointInT>::reconstruct (PointCloud &points, std::vector<pcl::Ver
   points.header = input_->header;
   if (!initCompute () || input_->points.empty () || indices_->empty ())
   {
-    points.points.clear ();
+    points.clear ();
     return;
   }
 
diff --git a/surface/include/pcl/surface/impl/gp3.hpp b/surface/include/pcl/surface/impl/gp3.hpp
index 2ac14590..7091caf4 100644
--- a/surface/include/pcl/surface/impl/gp3.hpp
+++ b/surface/include/pcl/surface/impl/gp3.hpp
@@ -83,7 +83,7 @@ pcl::GreedyProjectionTriangulation<PointInT>::reconstructPolygons (std::vector<p
     nnn_ = static_cast<int> (indices_->size ());
 
   // Variables to hold the results of nearest neighbor searches
-  std::vector<int> nnIdx (nnn_);
+  pcl::Indices nnIdx (nnn_);
   std::vector<float> sqrDists (nnn_);
 
   // current number of connected components
@@ -116,11 +116,11 @@ pcl::GreedyProjectionTriangulation<PointInT>::reconstructPolygons (std::vector<p
   if (!input_->is_dense)
   {
     // Skip invalid points from the indices list
-    for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
-      if (!std::isfinite ((*input_)[*it].x) ||
-          !std::isfinite ((*input_)[*it].y) ||
-          !std::isfinite ((*input_)[*it].z))
-        state_[*it] = NONE;
+    for (const auto& idx : (*indices_))
+      if (!std::isfinite ((*input_)[idx].x) ||
+          !std::isfinite ((*input_)[idx].y) ||
+          !std::isfinite ((*input_)[idx].z))
+        state_[idx] = NONE;
   }
 
   // Saving coordinates and point to index mapping
@@ -188,7 +188,7 @@ pcl::GreedyProjectionTriangulation<PointInT>::reconstructPolygons (std::vector<p
         angles_[i].index = nnIdx[i];
         if (
             (state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY)
-            || (state_[nnIdx[i]] == NONE) || (nnIdx[i] == -1) /// NOTE: discarding NaN points and those that are not in indices_
+            || (state_[nnIdx[i]] == NONE) || (nnIdx[i] == UNAVAILABLE) /// NOTE: discarding NaN points and those that are not in indices_
             || (sqrDists[i] > sqr_dist_threshold)
            )
           angles_[i].visible = false;
@@ -364,7 +364,7 @@ pcl::GreedyProjectionTriangulation<PointInT>::reconstructPolygons (std::vector<p
         angles_[i].nnIndex = i;
         if (
             (state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY)
-            || (state_[nnIdx[i]] == NONE) || (nnIdx[i] == -1) /// NOTE: discarding NaN points and those that are not in indices_
+            || (state_[nnIdx[i]] == NONE) || (nnIdx[i] == UNAVAILABLE) /// NOTE: discarding NaN points and those that are not in indices_
             || (sqrDists[i] > sqr_dist_threshold)
            )
           angles_[i].visible = false;
@@ -461,13 +461,13 @@ pcl::GreedyProjectionTriangulation<PointInT>::reconstructPolygons (std::vector<p
           {
             if (doubleEdges[j].index != i)
             {
-              int f = ffn_[nnIdx[doubleEdges[j].index]];
+              const auto& f = ffn_[nnIdx[doubleEdges[j].index]];
               if ((f != nnIdx[i]) && (f != R_))
                 visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].first, Eigen::Vector2f::Zero());
               if (!visibility)
                 break;
 
-              int s = sfn_[nnIdx[doubleEdges[j].index]];
+              const auto& s = sfn_[nnIdx[doubleEdges[j].index]];
               if ((s != nnIdx[i]) && (s != R_))
                 visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].second, Eigen::Vector2f::Zero());
               if (!visibility)
@@ -878,10 +878,10 @@ pcl::GreedyProjectionTriangulation<PointInT>::reconstructPolygons (std::vector<p
           else
             angle_so_far = 0;
         }
-        for (const int &it : to_erase)
+        for (const auto &idx : to_erase)
         {
           for (std::vector<int>::iterator iter = angleIdx.begin(); iter != angleIdx.end(); ++iter)
-            if (it == *iter)
+            if (idx == *iter)
             {
               angleIdx.erase(iter);
               break;
@@ -1087,7 +1087,7 @@ pcl::GreedyProjectionTriangulation<PointInT>::closeTriangle (std::vector<pcl::Ve
 template <typename PointInT> void
 pcl::GreedyProjectionTriangulation<PointInT>::connectPoint (
     std::vector<pcl::Vertices> &polygons, 
-    const int prev_index, const int next_index, const int next_next_index, 
+    const pcl::index_t prev_index, const pcl::index_t next_index, const pcl::index_t next_next_index, 
     const Eigen::Vector2f &uvn_current, 
     const Eigen::Vector2f &uvn_prev, 
     const Eigen::Vector2f &uvn_next)
@@ -1383,7 +1383,7 @@ pcl::GreedyProjectionTriangulation<PointInT>::connectPoint (
           case 2://next2f:
           {
             addTriangle (current_index_, ffn_[current_index_], next_index, polygons);
-            int neighbor_update = next_index;
+            auto neighbor_update = next_index;
 
             /* updating next_index */
             if (state_[next_index] <= FREE)
@@ -1518,7 +1518,7 @@ pcl::GreedyProjectionTriangulation<PointInT>::connectPoint (
           case 3://next2s:
           {
             addTriangle (current_index_, sfn_[current_index_], next_index, polygons);
-            int neighbor_update = next_index;
+            auto neighbor_update = next_index;
 
             /* updating next_index */
             if (state_[next_index] <= FREE)
diff --git a/surface/include/pcl/surface/impl/grid_projection.hpp b/surface/include/pcl/surface/impl/grid_projection.hpp
index 34072778..01483f6f 100644
--- a/surface/include/pcl/surface/impl/grid_projection.hpp
+++ b/surface/include/pcl/surface/impl/grid_projection.hpp
@@ -144,7 +144,7 @@ pcl::GridProjection<PointNT>::getVertexFromCellCenter (
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointNT> void
 pcl::GridProjection<PointNT>::getDataPtsUnion (const Eigen::Vector3i &index,
-                                               std::vector <int> &pt_union_indices)
+                                               pcl::Indices &pt_union_indices)
 {
   for (int i = index[0] - padding_size_; i <= index[0] + padding_size_; ++i)
   {
@@ -170,7 +170,7 @@ pcl::GridProjection<PointNT>::getDataPtsUnion (const Eigen::Vector3i &index,
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointNT> void
 pcl::GridProjection<PointNT>::createSurfaceForCell (const Eigen::Vector3i &index,
-                                                    std::vector <int> &pt_union_indices)
+                                                    pcl::Indices &pt_union_indices)
 {
   // 8 vertices of the cell
   std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > vertices (8);
@@ -269,7 +269,7 @@ pcl::GridProjection<PointNT>::createSurfaceForCell (const Eigen::Vector3i &index
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointNT> void
 pcl::GridProjection<PointNT>::getProjection (const Eigen::Vector4f &p,
-                                             std::vector <int> &pt_union_indices, Eigen::Vector4f &projection)
+                                             pcl::Indices &pt_union_indices, Eigen::Vector4f &projection)
 {
   const double projection_distance = leaf_size_ * 3;
   std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > end_pt (2);
@@ -308,7 +308,7 @@ pcl::GridProjection<PointNT>::getProjection (const Eigen::Vector4f &p,
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointNT> void
 pcl::GridProjection<PointNT>::getProjectionWithPlaneFit (const Eigen::Vector4f &p,
-                                                         std::vector<int> (&pt_union_indices),
+                                                         pcl::Indices (&pt_union_indices),
                                                          Eigen::Vector4f &projection)
 {
   // Compute the plane coefficients
@@ -343,7 +343,7 @@ pcl::GridProjection<PointNT>::getProjectionWithPlaneFit (const Eigen::Vector4f &
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointNT> void
 pcl::GridProjection<PointNT>::getVectorAtPoint (const Eigen::Vector4f &p,
-                                                std::vector <int> &pt_union_indices,
+                                                pcl::Indices &pt_union_indices,
                                                 Eigen::Vector3f &vo)
 {
   std::vector <double> pt_union_dist (pt_union_indices.size ());
@@ -390,7 +390,7 @@ pcl::GridProjection<PointNT>::getVectorAtPoint (const Eigen::Vector4f &p,
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointNT> void
 pcl::GridProjection<PointNT>::getVectorAtPointKNN (const Eigen::Vector4f &p,
-                                                   std::vector <int> &k_indices,
+                                                   pcl::Indices &k_indices,
                                                    std::vector <float> &k_squared_distances,
                                                    Eigen::Vector3f &vo)
 {
@@ -424,10 +424,9 @@ pcl::GridProjection<PointNT>::getVectorAtPointKNN (const Eigen::Vector4f &p,
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointNT> double
 pcl::GridProjection<PointNT>::getMagAtPoint (const Eigen::Vector4f &p,
-                                             const std::vector <int> &pt_union_indices)
+                                             const pcl::Indices &pt_union_indices)
 {
   std::vector <double> pt_union_dist (pt_union_indices.size ());
-  std::vector <double> pt_union_weight (pt_union_indices.size ());
   double sum = 0.0;
   for (std::size_t i = 0; i < pt_union_indices.size (); ++i)
   {
@@ -441,7 +440,7 @@ pcl::GridProjection<PointNT>::getMagAtPoint (const Eigen::Vector4f &p,
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointNT> double
 pcl::GridProjection<PointNT>::getD1AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec,
-                                            const std::vector <int> &pt_union_indices)
+                                            const pcl::Indices &pt_union_indices)
 {
   double sz = 0.01 * leaf_size_;
   Eigen::Vector3f v = vec * static_cast<float> (sz);
@@ -454,7 +453,7 @@ pcl::GridProjection<PointNT>::getD1AtPoint (const Eigen::Vector4f &p, const Eige
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointNT> double
 pcl::GridProjection<PointNT>::getD2AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec,
-                                            const std::vector <int> &pt_union_indices)
+                                            const pcl::Indices &pt_union_indices)
 {
   double sz = 0.01 * leaf_size_;
   Eigen::Vector3f v = vec * static_cast<float> (sz);
@@ -468,7 +467,7 @@ pcl::GridProjection<PointNT>::getD2AtPoint (const Eigen::Vector4f &p, const Eige
 template <typename PointNT> bool
 pcl::GridProjection<PointNT>::isIntersected (const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
                                              std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
-                                             std::vector <int> &pt_union_indices)
+                                             pcl::Indices &pt_union_indices)
 {
   assert (end_pts.size () == 2);
   assert (vect_at_end_pts.size () == 2);
@@ -505,7 +504,7 @@ pcl::GridProjection<PointNT>::findIntersection (int level,
                                                 const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
                                                 const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
                                                 const Eigen::Vector4f &start_pt,
-                                                std::vector <int> &pt_union_indices,
+                                                pcl::Indices &pt_union_indices,
                                                 Eigen::Vector4f &intersection)
 {
   assert (end_pts.size () == 2);
@@ -574,7 +573,7 @@ pcl::GridProjection<PointNT>::fillPad (const Eigen::Vector3i &index)
 template <typename PointNT> void
 pcl::GridProjection<PointNT>::storeVectAndSurfacePoint (int index_1d,
                                                         const Eigen::Vector3i &,
-                                                        std::vector <int> &pt_union_indices,
+                                                        pcl::Indices &pt_union_indices,
                                                         const Leaf &cell_data)
 {
   // Get point on grid
@@ -599,7 +598,7 @@ pcl::GridProjection<PointNT>::storeVectAndSurfacePointKNN (int index_1d, const E
       cell_center.y () + static_cast<float> (leaf_size_) / 2.0f,
       cell_center.z () + static_cast<float> (leaf_size_) / 2.0f, 0.0f);
 
-  std::vector <int> k_indices;
+  pcl::Indices k_indices;
   k_indices.resize (k_);
   std::vector <float> k_squared_distances;
   k_squared_distances.resize (k_);
@@ -624,7 +623,7 @@ pcl::GridProjection<PointNT>::reconstructPolygons (std::vector<pcl::Vertices> &p
   cell_hash_map_.rehash (data_->size () / static_cast<long unsigned int> (cell_hash_map_.max_load_factor ()));
 
   // Go over all points and insert them into the right leaf
-  for (int cp = 0; cp < static_cast<int> (data_->size ()); ++cp)
+  for (pcl::index_t cp = 0; cp < static_cast<pcl::index_t> (data_->size ()); ++cp)
   {
     // Check if the point is invalid
     if (!std::isfinite ((*data_)[cp].x) ||
@@ -676,7 +675,7 @@ pcl::GridProjection<PointNT>::reconstructPolygons (std::vector<pcl::Vertices> &p
   for (const auto &entry : cell_hash_map_)
   {
     getIndexIn3D (entry.first, index);
-    std::vector <int> pt_union_indices;
+    pcl::Indices pt_union_indices;
     getDataPtsUnion (index, pt_union_indices);
 
     // Needs at least 10 points (?)
@@ -693,7 +692,7 @@ pcl::GridProjection<PointNT>::reconstructPolygons (std::vector<pcl::Vertices> &p
   for (const auto &entry : cell_hash_map_)
   {
     getIndexIn3D (entry.first, index);
-    std::vector <int> pt_union_indices;
+    pcl::Indices pt_union_indices;
     getDataPtsUnion (index, pt_union_indices);
 
     // Needs at least 10 points (?)
@@ -730,7 +729,7 @@ pcl::GridProjection<PointNT>::performReconstruction (pcl::PolygonMesh &output)
   cloud.height = 1;
   cloud.is_dense = true;
 
-  cloud.points.resize (surface_.size ());
+  cloud.resize (surface_.size ());
   // Copy the data from surface_ to cloud
   for (std::size_t i = 0; i < cloud.size (); ++i)
   {
diff --git a/surface/include/pcl/surface/impl/marching_cubes.hpp b/surface/include/pcl/surface/impl/marching_cubes.hpp
index 39dbca05..03984fd9 100644
--- a/surface/include/pcl/surface/impl/marching_cubes.hpp
+++ b/surface/include/pcl/surface/impl/marching_cubes.hpp
@@ -224,8 +224,7 @@ pcl::MarchingCubes<PointNT>::performReconstruction (pcl::PointCloud<PointNT> &po
   {
     PCL_ERROR ("[pcl::%s::performReconstruction] Invalid iso level %f! Please use a number between 0 and 1.\n", 
         getClassName ().c_str (), iso_level_);
-    points.width = points.height = 0;
-    points.points.clear ();
+    points.clear ();
     polygons.clear ();
     return;
   }
diff --git a/surface/include/pcl/surface/impl/marching_cubes_hoppe.hpp b/surface/include/pcl/surface/impl/marching_cubes_hoppe.hpp
index 51fa52b1..91e7d680 100644
--- a/surface/include/pcl/surface/impl/marching_cubes_hoppe.hpp
+++ b/surface/include/pcl/surface/impl/marching_cubes_hoppe.hpp
@@ -37,9 +37,6 @@
 #define PCL_SURFACE_IMPL_MARCHING_CUBES_HOPPE_H_
 
 #include <pcl/surface/marching_cubes_hoppe.h>
-#include <pcl/common/common.h>
-#include <pcl/common/vector_average.h>
-#include <pcl/Vertices.h>
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointNT>
@@ -64,7 +61,7 @@ pcl::MarchingCubesHoppe<PointNT>::voxelizeData ()
 
       for (int z = 0; z < res_z_; ++z)
       {
-        std::vector<int> nn_indices (1, 0);
+        pcl::Indices nn_indices (1, 0);
         std::vector<float> nn_sqr_dists (1, 0.0f);
         const Eigen::Vector3f point = (lower_boundary_ + size_voxel_ * Eigen::Array3f (x, y, z)).matrix ();
         PointNT p;
diff --git a/surface/include/pcl/surface/impl/marching_cubes_rbf.hpp b/surface/include/pcl/surface/impl/marching_cubes_rbf.hpp
index 3aa178ed..f3335120 100644
--- a/surface/include/pcl/surface/impl/marching_cubes_rbf.hpp
+++ b/surface/include/pcl/surface/impl/marching_cubes_rbf.hpp
@@ -40,9 +40,6 @@
 #define PCL_SURFACE_IMPL_MARCHING_CUBES_RBF_H_
 
 #include <pcl/surface/marching_cubes_rbf.h>
-#include <pcl/common/common.h>
-#include <pcl/common/vector_average.h>
-#include <pcl/Vertices.h>
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointNT>
diff --git a/surface/include/pcl/surface/impl/mls.hpp b/surface/include/pcl/surface/impl/mls.hpp
index e2bf36b9..7f78a106 100644
--- a/surface/include/pcl/surface/impl/mls.hpp
+++ b/surface/include/pcl/surface/impl/mls.hpp
@@ -42,15 +42,16 @@
 
 #include <pcl/type_traits.h>
 #include <pcl/surface/mls.h>
-#include <pcl/common/io.h>
 #include <pcl/common/common.h> // for getMinMax3D
 #include <pcl/common/copy_point.h>
 #include <pcl/common/centroid.h>
 #include <pcl/common/eigen.h>
-#include <pcl/common/geometry.h>
 #include <pcl/search/kdtree.h> // for KdTree
 #include <pcl/search/organized.h> // for OrganizedNeighbor
 
+#include <Eigen/Geometry> // for cross
+#include <Eigen/LU> // for inverse
+
 #ifdef _OPENMP
 #include <omp.h>
 #endif
@@ -76,7 +77,7 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::process (PointCloudOut &output)
   // Copy the header
   output.header = input_->header;
   output.width = output.height = 0;
-  output.points.clear ();
+  output.clear ();
 
   if (search_radius_ <= 0 || sqr_gauss_param_ <= 0)
   {
@@ -170,8 +171,8 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::process (PointCloudOut &output)
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointInT, typename PointOutT> void
-pcl::MovingLeastSquares<PointInT, PointOutT>::computeMLSPointNormal (int index,
-                                                                     const std::vector<int> &nn_indices,
+pcl::MovingLeastSquares<PointInT, PointOutT>::computeMLSPointNormal (pcl::index_t index,
+                                                                     const pcl::Indices &nn_indices,
                                                                      PointCloudOut &projected_points,
                                                                      NormalCloud &projected_points_normals,
                                                                      PointIndices &corresponding_input_indices,
@@ -248,7 +249,7 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::computeMLSPointNormal (int index,
 }
 
 template <typename PointInT, typename PointOutT> void
-pcl::MovingLeastSquares<PointInT, PointOutT>::addProjectedPointNormal (int index,
+pcl::MovingLeastSquares<PointInT, PointOutT>::addProjectedPointNormal (pcl::index_t index,
                                                                        const Eigen::Vector3d &point,
                                                                        const Eigen::Vector3d &normal,
                                                                        double curvature,
@@ -304,7 +305,7 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::performProcessing (PointCloudOut &
   {
     // Allocate enough space to hold the results of nearest neighbor searches
     // \note resize is irrelevant for a radiusSearch ().
-    std::vector<int> nn_indices;
+    pcl::Indices nn_indices;
     std::vector<float> nn_sqr_dists;
 
     // Get the initial estimates of point positions and their neighborhoods
@@ -380,10 +381,10 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::performUpsampling (PointCloudOut &
 
       // Get 3D position of point
       //Eigen::Vector3f pos = (*distinct_cloud_)[dp_i].getVector3fMap ();
-      std::vector<int> nn_indices;
+      pcl::Indices nn_indices;
       std::vector<float> nn_dists;
       tree_->nearestKSearch ((*distinct_cloud_)[dp_i], 1, nn_indices, nn_dists);
-      int input_index = nn_indices.front ();
+      const auto input_index = nn_indices.front ();
 
       // If the closest point did not have a valid MLS fitting result
       // OR if it is too far away from the sampled point
@@ -417,10 +418,10 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::performUpsampling (PointCloudOut &
       p.y = pos[1];
       p.z = pos[2];
 
-      std::vector<int> nn_indices;
+      pcl::Indices nn_indices;
       std::vector<float> nn_dists;
       tree_->nearestKSearch (p, 1, nn_indices, nn_dists);
-      int input_index = nn_indices.front ();
+      const auto input_index = nn_indices.front ();
 
       // If the closest point did not have a valid MLS fitting result
       // OR if it is too far away from the sampled point
@@ -533,14 +534,14 @@ pcl::MLSResult::getPolynomialPartialDerivative (const double u, const double v)
 }
 
 Eigen::Vector2f
-pcl::MLSResult::calculatePrincipleCurvatures (const double u, const double v) const
+pcl::MLSResult::calculatePrincipalCurvatures (const double u, const double v) const
 {
   Eigen::Vector2f k (1e-5, 1e-5);
 
   // Note: this use the Monge Patch to derive the Gaussian curvature and Mean Curvature found here http://mathworld.wolfram.com/MongePatch.html
   // Then:
   //      k1 = H + sqrt(H^2 - K)
-  //      k1 = H - sqrt(H^2 - K)
+  //      k2 = H - sqrt(H^2 - K)
   if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && std::isfinite (c_vec[0]))
   {
     const PolynomialPartialDerivative d = getPolynomialPartialDerivative (u, v);
@@ -558,7 +559,7 @@ pcl::MLSResult::calculatePrincipleCurvatures (const double u, const double v) co
   }
   else
   {
-    PCL_ERROR ("No Polynomial fit data, unable to calculate the principle curvatures!\n");
+    PCL_ERROR ("No Polynomial fit data, unable to calculate the principal curvatures!\n");
   }
 
   return (k);
@@ -719,8 +720,8 @@ pcl::MLSResult::projectQueryPoint (ProjectionMethod method, int required_neighbo
 
 template <typename PointT> void
 pcl::MLSResult::computeMLSSurface (const pcl::PointCloud<PointT> &cloud,
-                                   int index,
-                                   const std::vector<int> &nn_indices,
+                                   pcl::index_t index,
+                                   const pcl::Indices &nn_indices,
                                    double search_radius,
                                    int polynomial_order,
                                    std::function<double(const double)> weight_func)
@@ -800,7 +801,7 @@ pcl::MLSResult::computeMLSSurface (const pcl::PointCloud<PointT> &cloud,
       }
 
       // Go through neighbors, transform them in the local coordinate system,
-      // save height and the evaluation of the polynome's terms
+      // save height and the evaluation of the polynomial's terms
       for (std::size_t ni = 0; ni < static_cast<std::size_t>(num_neighbors); ++ni)
       {
         // Transforming coordinates
diff --git a/surface/include/pcl/surface/impl/organized_fast_mesh.hpp b/surface/include/pcl/surface/impl/organized_fast_mesh.hpp
index 929fe899..c9331b4c 100644
--- a/surface/include/pcl/surface/impl/organized_fast_mesh.hpp
+++ b/surface/include/pcl/surface/impl/organized_fast_mesh.hpp
@@ -42,6 +42,7 @@
 #define PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_
 
 #include <pcl/surface/organized_fast_mesh.h>
+#include <pcl/common/io.h> // for getFieldIndex
 
 /////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointInT> void
diff --git a/surface/include/pcl/surface/impl/poisson.hpp b/surface/include/pcl/surface/impl/poisson.hpp
index dcd3e495..1f626e69 100644
--- a/surface/include/pcl/surface/impl/poisson.hpp
+++ b/surface/include/pcl/surface/impl/poisson.hpp
@@ -81,6 +81,7 @@ pcl::Poisson<PointNT>::Poisson ()
   , show_residual_ (false)
   , min_iterations_ (8)
   , solver_accuracy_ (1e-3f)
+  , threads_(1)
 {
 }
 
@@ -90,6 +91,20 @@ pcl::Poisson<PointNT>::~Poisson ()
 {
 }
 
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointNT> void
+pcl::Poisson<PointNT>::setThreads (int threads)
+{
+  if (threads == 0)
+#ifdef _OPENMP
+    threads_ = omp_get_num_procs();
+#else
+    threads_ = 1;
+#endif
+  else
+    threads_ = threads;
+}
+      
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointNT> template <int Degree> void
 pcl::Poisson<PointNT>::execute (poisson::CoredVectorMeshData &mesh,
@@ -100,8 +115,8 @@ pcl::Poisson<PointNT>::execute (poisson::CoredVectorMeshData &mesh,
   poisson::TreeNodeData::UseIndex = 1;
   poisson::Octree<Degree> tree;
 
-  /// TODO OPENMP stuff
-  //    tree.threads = Threads.value;
+  
+  tree.threads = threads_;
   center.coords[0] = center.coords[1] = center.coords[2] = 0;
 
 
@@ -190,7 +205,7 @@ pcl::Poisson<PointNT>::performReconstruction (PolygonMesh &output)
 
   // Write output PolygonMesh
   pcl::PointCloud<pcl::PointXYZ> cloud;
-  cloud.points.resize (int (mesh.outOfCorePointCount () + mesh.inCorePoints.size ()));
+  cloud.resize (int (mesh.outOfCorePointCount () + mesh.inCorePoints.size ()));
   poisson::Point3D<float> p;
   for (int i = 0; i < int (mesh.inCorePoints.size ()); i++)
   {
diff --git a/surface/include/pcl/surface/impl/processing.hpp b/surface/include/pcl/surface/impl/processing.hpp
index 4c262cdc..aa75ebbb 100644
--- a/surface/include/pcl/surface/impl/processing.hpp
+++ b/surface/include/pcl/surface/impl/processing.hpp
@@ -52,7 +52,7 @@ CloudSurfaceProcessing<PointInT, PointOutT>::process (pcl::PointCloud<PointOutT>
   if (!initCompute ())
   {
     output.width = output.height = 0;
-    output.points.clear ();
+    output.clear ();
     return;
   }
 
diff --git a/surface/include/pcl/surface/impl/reconstruction.hpp b/surface/include/pcl/surface/impl/reconstruction.hpp
index 33a30436..d2e5a27e 100644
--- a/surface/include/pcl/surface/impl/reconstruction.hpp
+++ b/surface/include/pcl/surface/impl/reconstruction.hpp
@@ -40,7 +40,9 @@
 #ifndef PCL_SURFACE_RECONSTRUCTION_IMPL_H_
 #define PCL_SURFACE_RECONSTRUCTION_IMPL_H_
 
-#include <pcl/search/pcl_search.h>
+#include <pcl/conversions.h> // for toPCLPointCloud2
+#include <pcl/search/kdtree.h> // for KdTree
+#include <pcl/search/organized.h> // for OrganizedNeighbor
 
 
 namespace pcl
diff --git a/surface/include/pcl/surface/impl/surfel_smoothing.hpp b/surface/include/pcl/surface/impl/surfel_smoothing.hpp
index a0b1ea97..e7d85468 100644
--- a/surface/include/pcl/surface/impl/surfel_smoothing.hpp
+++ b/surface/include/pcl/surface/impl/surfel_smoothing.hpp
@@ -38,8 +38,11 @@
 #ifndef PCL_SURFACE_IMPL_SURFEL_SMOOTHING_H_
 #define PCL_SURFACE_IMPL_SURFEL_SMOOTHING_H_
 
+#include <pcl/search/organized.h> // for OrganizedNeighbor
+#include <pcl/search/kdtree.h> // for KdTree
 #include <pcl/surface/surfel_smoothing.h>
 #include <pcl/common/distances.h>
+#include <pcl/console/print.h> // for PCL_ERROR, PCL_DEBUG
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT, typename PointNT> bool
@@ -89,7 +92,7 @@ pcl::SurfelSmoothing<PointT, PointNT>::smoothCloudIteration (PointCloudInPtr &ou
   output_normals = NormalCloudPtr (new NormalCloud);
   output_normals->points.resize (interm_cloud_->size ());
 
-  std::vector<int> nn_indices;
+  pcl::Indices nn_indices;
   std::vector<float> nn_distances;
 
   std::vector<float> diffs (interm_cloud_->size ());
@@ -168,7 +171,7 @@ pcl::SurfelSmoothing<PointT, PointNT>::smoothPoint (std::size_t &point_index,
   float error_residual = error_residual_threshold_ + 1;
   float last_error_residual = error_residual + 100.0f;
 
-  std::vector<int> nn_indices;
+  pcl::Indices nn_indices;
   std::vector<float> nn_distances;
 
   int big_iterations = 0;
@@ -286,7 +289,7 @@ pcl::SurfelSmoothing<PointT, PointNT>::extractSalientFeaturesBetweenScales (Poin
     diffs[i] = (*cloud2_normals)[i].getNormalVector4fMap ().dot ((*cloud2)[i].getVector4fMap () - 
                                                                       (*interm_cloud_)[i].getVector4fMap ());
 
-  std::vector<int> nn_indices;
+  pcl::Indices nn_indices;
   std::vector<float> nn_distances;
 
   output_features->resize (cloud2->size ());
@@ -297,7 +300,7 @@ pcl::SurfelSmoothing<PointT, PointNT>::extractSalientFeaturesBetweenScales (Poin
 
     bool largest = true;
     bool smallest = true;
-    for (const int &nn_index : nn_indices)
+    for (const auto &nn_index : nn_indices)
     {
       if (diffs[point_i] < diffs[nn_index])
         largest = false;
diff --git a/surface/include/pcl/surface/impl/texture_mapping.hpp b/surface/include/pcl/surface/impl/texture_mapping.hpp
index 7ea4e22f..34077edf 100644
--- a/surface/include/pcl/surface/impl/texture_mapping.hpp
+++ b/surface/include/pcl/surface/impl/texture_mapping.hpp
@@ -185,9 +185,7 @@ pcl::TextureMapping<PointInT>::mapTexture2Mesh (pcl::TextureMesh &tex_mesh)
     }// end faces
 
     // texture materials
-    std::stringstream tex_name;
-    tex_name << "material_" << m;
-    tex_name >> tex_material_.tex_name;
+    tex_material_.tex_name = "material_" + std::to_string(m);
     tex_material_.tex_file = tex_files_[m];
     tex_mesh.tex_materials.push_back (tex_material_);
 
@@ -271,9 +269,7 @@ pcl::TextureMapping<PointInT>::mapTexture2MeshUV (pcl::TextureMesh &tex_mesh)
     }// end faces
 
     // texture materials
-    std::stringstream tex_name;
-    tex_name << "material_" << m;
-    tex_name >> tex_material_.tex_name;
+    tex_material_.tex_name = "material_" + std::to_string(m);
     tex_material_.tex_file = tex_files_[m];
     tex_mesh.tex_materials.push_back (tex_material_);
 
@@ -321,7 +317,7 @@ pcl::TextureMapping<PointInT>::mapMultipleTexturesToMeshUV (pcl::TextureMesh &te
     {
       Eigen::Vector2f tmp_VT;
       // for each point of this face
-      for (const unsigned int &vertex : tex_polygon.vertices)
+      for (const auto &vertex : tex_polygon.vertices)
       {
         // get point
         PointInT pt = (*camera_transformed_cloud)[vertex];
@@ -333,9 +329,7 @@ pcl::TextureMapping<PointInT>::mapMultipleTexturesToMeshUV (pcl::TextureMesh &te
     }// end faces
 
     // texture materials
-    std::stringstream tex_name;
-    tex_name << "material_" << m;
-    tex_name >> tex_material_.tex_name;
+    tex_material_.tex_name = "material_" + std::to_string(m);
     tex_material_.tex_file = current_cam.texture_file;
     tex_mesh.tex_materials.push_back (tex_material_);
 
@@ -371,7 +365,7 @@ pcl::TextureMapping<PointInT>::isPointOccluded (const PointInT &pt, OctreePtr oc
   direction (1) = pt.y;
   direction (2) = pt.z;
 
-  std::vector<int> indices;
+  pcl::Indices indices;
 
   PointCloudConstPtr cloud (new PointCloud());
   cloud = octree->getInputCloud();
@@ -382,7 +376,7 @@ pcl::TextureMapping<PointInT>::isPointOccluded (const PointInT &pt, OctreePtr oc
   octree->getIntersectedVoxelIndices(direction, -direction, indices);
 
   int nbocc = static_cast<int> (indices.size ());
-  for (const int &index : indices)
+  for (const auto &index : indices)
   {
    // if intersected point is on the over side of the camera
    if (pt.z * (*cloud)[index].z < 0)
@@ -405,8 +399,8 @@ pcl::TextureMapping<PointInT>::isPointOccluded (const PointInT &pt, OctreePtr oc
 template<typename PointInT> void
 pcl::TextureMapping<PointInT>::removeOccludedPoints (const PointCloudPtr &input_cloud,
                                                      PointCloudPtr &filtered_cloud,
-                                                     const double octree_voxel_size, std::vector<int> &visible_indices,
-                                                     std::vector<int> &occluded_indices)
+                                                     const double octree_voxel_size, pcl::Indices &visible_indices,
+                                                     pcl::Indices &occluded_indices)
 {
   // variable used to filter occluded points by depth
   double maxDeltaZ = octree_voxel_size;
@@ -424,7 +418,7 @@ pcl::TextureMapping<PointInT>::removeOccludedPoints (const PointCloudPtr &input_
 
   // for each point of the cloud, raycast toward camera and check intersected voxels.
   Eigen::Vector3f direction;
-  std::vector<int> indices;
+  pcl::Indices indices;
   for (std::size_t i = 0; i < input_cloud->size (); ++i)
   {
     direction (0) = (*input_cloud)[i].x;
@@ -435,7 +429,7 @@ pcl::TextureMapping<PointInT>::removeOccludedPoints (const PointCloudPtr &input_
     octree.getIntersectedVoxelIndices (direction, -direction, indices);
 
     int nbocc = static_cast<int> (indices.size ());
-    for (const int &index : indices)
+    for (const auto &index : indices)
     {
       // if intersected point is on the over side of the camera
       if ((*input_cloud)[i].z * (*input_cloud)[index].z < 0)
@@ -455,11 +449,11 @@ pcl::TextureMapping<PointInT>::removeOccludedPoints (const PointCloudPtr &input_
     {
       // point is added in the filtered mesh
       filtered_cloud->points.push_back ((*input_cloud)[i]);
-      visible_indices.push_back (static_cast<int> (i));
+      visible_indices.push_back (static_cast<pcl::index_t> (i));
     }
     else
     {
-      occluded_indices.push_back (static_cast<int> (i));
+      occluded_indices.push_back (static_cast<pcl::index_t> (i));
     }
   }
 
@@ -478,7 +472,7 @@ pcl::TextureMapping<PointInT>::removeOccludedPoints (const pcl::TextureMesh &tex
   // load points into a PCL format
   pcl::fromPCLPointCloud2 (tex_mesh.cloud, *cloud);
 
-  std::vector<int> visible, occluded;
+  pcl::Indices visible, occluded;
   removeOccludedPoints (cloud, filtered_cloud, octree_voxel_size, visible, occluded);
 
   // Now that we know which points are visible, let's iterate over each face.
@@ -492,14 +486,11 @@ pcl::TextureMapping<PointInT>::removeOccludedPoints (const pcl::TextureMesh &tex
     {
       // check if all the face's points are visible
       bool faceIsVisible = true;
-      std::vector<int>::iterator it;
 
       // iterate over face's vertex
-      for (const unsigned int &vertex : tex_mesh.tex_polygons[polygons][faces].vertices)
+      for (const auto &vertex : tex_mesh.tex_polygons[polygons][faces].vertices)
       {
-        it = find (occluded.begin (), occluded.end (), vertex);
-
-        if (it == occluded.end ())
+        if (find (occluded.begin (), occluded.end (), vertex) == occluded.end ())
         {
           // point is not in the occluded vector
           // PCL_INFO ("  VISIBLE!\n");
@@ -531,7 +522,7 @@ pcl::TextureMapping<PointInT>::removeOccludedPoints (const pcl::TextureMesh &tex
   // load points into a PCL format
   pcl::fromPCLPointCloud2 (tex_mesh.cloud, *cloud);
 
-  std::vector<int> visible, occluded;
+  pcl::Indices visible, occluded;
   removeOccludedPoints (cloud, filtered_cloud, octree_voxel_size, visible, occluded);
 
 }
@@ -576,7 +567,7 @@ pcl::TextureMapping<PointInT>::sortFacesByCamera (pcl::TextureMesh &tex_mesh, pc
     pcl::transformPointCloud (*original_cloud, *transformed_cloud, cam_trans.inverse ());
 
     // find occlusions on transformed cloud
-    std::vector<int> visible, occluded;
+    pcl::Indices visible, occluded;
     removeOccludedPoints (transformed_cloud, filtered_cloud, octree_voxel_size, visible, occluded);
     visible_pts = *filtered_cloud;
 
@@ -644,7 +635,7 @@ pcl::TextureMapping<PointInT>::showOcclusions (const PointCloudPtr &input_cloud,
   // ray direction
   Eigen::Vector3f direction;
 
-  std::vector<int> indices;
+  pcl::Indices indices;
   // point from where we ray-trace
   pcl::PointXYZI pt;
 
@@ -662,7 +653,7 @@ pcl::TextureMapping<PointInT>::showOcclusions (const PointCloudPtr &input_cloud,
     nbocc = static_cast<int> (indices.size ());
 
     // TODO need to clean this up and find tricks to get remove aliasaing effect on planes
-    for (const int &index : indices)
+    for (const auto &index : indices)
     {
       // if intersected point is on the over side of the camera
       if (pt.z * (*input_cloud)[index].z < 0)
@@ -806,7 +797,7 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras (pcl::TextureMesh
         pcl::KdTreeFLANN<pcl::PointXY> kdtree;
         kdtree.setInputCloud (projections);
 
-        std::vector<int> idxNeighbors;
+        pcl::Indices idxNeighbors;
         std::vector<float> neighborsSquaredDistance;
         // af first (idx_pcan < current_cam), check if some of the faces attached to previous cameras occlude the current faces
         // then (idx_pcam == current_cam), check for self occlusions. At this stage, we skip faces that were already marked as occluded
@@ -849,7 +840,7 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras (pcl::TextureMesh
               if (kdtree.radiusSearch (center, radius, idxNeighbors, neighborsSquaredDistance) > 0 )
               {
                 // for each neighbor
-                for (const int &idxNeighbor : idxNeighbors)
+                for (const auto &idxNeighbor : idxNeighbors)
                 {
                   if (std::max ((*camera_cloud)[mesh.tex_polygons[idx_pcam][idx_face].vertices[0]].z,
                                 std::max ((*camera_cloud)[mesh.tex_polygons[idx_pcam][idx_face].vertices[1]].z, 
diff --git a/surface/include/pcl/surface/marching_cubes.h b/surface/include/pcl/surface/marching_cubes.h
index 514c2545..e6fd2ada 100644
--- a/surface/include/pcl/surface/marching_cubes.h
+++ b/surface/include/pcl/surface/marching_cubes.h
@@ -37,7 +37,6 @@
 
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
-#include <pcl/surface/boost.h>
 #include <pcl/surface/reconstruction.h>
 
 namespace pcl
diff --git a/surface/include/pcl/surface/marching_cubes_hoppe.h b/surface/include/pcl/surface/marching_cubes_hoppe.h
index 3912b177..9e4b0722 100644
--- a/surface/include/pcl/surface/marching_cubes_hoppe.h
+++ b/surface/include/pcl/surface/marching_cubes_hoppe.h
@@ -37,7 +37,6 @@
 
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
-#include <pcl/surface/boost.h>
 #include <pcl/surface/marching_cubes.h>
 
 namespace pcl
diff --git a/surface/include/pcl/surface/marching_cubes_rbf.h b/surface/include/pcl/surface/marching_cubes_rbf.h
index f8b69193..fda659c3 100644
--- a/surface/include/pcl/surface/marching_cubes_rbf.h
+++ b/surface/include/pcl/surface/marching_cubes_rbf.h
@@ -37,7 +37,6 @@
 
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
-#include <pcl/surface/boost.h>
 #include <pcl/surface/marching_cubes.h>
 
 namespace pcl
diff --git a/surface/include/pcl/surface/mls.h b/surface/include/pcl/surface/mls.h
index 228d983c..1af7a8dd 100644
--- a/surface/include/pcl/surface/mls.h
+++ b/surface/include/pcl/surface/mls.h
@@ -82,7 +82,7 @@ namespace pcl
       MLSProjectionResults () : u (0), v (0) {}
 
       double u;               /**< \brief The u-coordinate of the projected point in local MLS frame. */
-      double v;               /**< \brief The u-coordinate of the projected point in local MLS frame. */
+      double v;               /**< \brief The v-coordinate of the projected point in local MLS frame. */
       Eigen::Vector3d point;  /**< \brief The projected point. */
       Eigen::Vector3d normal; /**< \brief The projected point's normal. */
       PCL_MAKE_ALIGNED_OPERATOR_NEW
@@ -102,7 +102,7 @@ namespace pcl
                const float a_curvature,
                const int a_order);
 
-    /** \brief Given a point calculate it's 3D location in the MLS frame.
+    /** \brief Given a point calculate its 3D location in the MLS frame.
       * \param[in] pt The point
       * \param[out] u The u-coordinate of the point in local MLS frame.
       * \param[out] v The v-coordinate of the point in local MLS frame.
@@ -111,7 +111,7 @@ namespace pcl
     inline void
     getMLSCoordinates (const Eigen::Vector3d &pt, double &u, double &v, double &w) const;
 
-    /** \brief Given a point calculate it's 2D location in the MLS frame.
+    /** \brief Given a point calculate its 2D location in the MLS frame.
       * \param[in] pt The point
       * \param[out] u The u-coordinate of the point in local MLS frame.
       * \param[out] v The v-coordinate of the point in local MLS frame.
@@ -122,7 +122,7 @@ namespace pcl
     /** \brief Calculate the polynomial
       * \param[in] u The u-coordinate of the point in local MLS frame.
       * \param[in] v The v-coordinate of the point in local MLS frame.
-      * \return The polynomial value at the provide uv coordinates.
+      * \return The polynomial value at the provided uv coordinates.
       */
     inline double
     getPolynomialValue (const double u, const double v) const;
@@ -130,19 +130,29 @@ namespace pcl
     /** \brief Calculate the polynomial's first and second partial derivatives.
       * \param[in] u The u-coordinate of the point in local MLS frame.
       * \param[in] v The v-coordinate of the point in local MLS frame.
-      * \return The polynomial partial derivatives at the provide uv coordinates.
+      * \return The polynomial partial derivatives at the provided uv coordinates.
       */
     inline PolynomialPartialDerivative
     getPolynomialPartialDerivative (const double u, const double v) const;
 
-    /** \brief Calculate the principle curvatures using the polynomial surface.
+    /** \brief Calculate the principal curvatures using the polynomial surface.
       * \param[in] u The u-coordinate of the point in local MLS frame.
       * \param[in] v The v-coordinate of the point in local MLS frame.
-      * \return The principle curvature [k1, k2] at the provided ub coordinates.
-      * \note If an error occurs the MLS_MINIMUM_PRINCIPLE_CURVATURE is returned.
+      * \return The principal curvature [k1, k2] at the provided uv coordinates.
+      * \note If an error occurs then 1e-5 is returned.
       */
+    Eigen::Vector2f
+    calculatePrincipalCurvatures (const double u, const double v) const;
+
+    /** \brief Calculate the principal curvatures using the polynomial surface.
+      * \param[in] u The u-coordinate of the point in local MLS frame.
+      * \param[in] v The v-coordinate of the point in local MLS frame.
+      * \return The principal curvature [k1, k2] at the provided ub coordinates.
+      * \note If an error occurs then 1e-5 is returned.
+      */
+    PCL_DEPRECATED(1, 15, "use calculatePrincipalCurvatures() instead")
     inline Eigen::Vector2f
-    calculatePrincipleCurvatures (const double u, const double v) const;
+    calculatePrincipleCurvatures (const double u, const double v) const { return calculatePrincipalCurvatures(u, v); };
 
     /** \brief Project a point orthogonal to the polynomial surface.
       * \param[in] u The u-coordinate of the point in local MLS frame.
@@ -178,7 +188,7 @@ namespace pcl
      * \param[in] pt The point to be project.
      * \param[in] method The projection method to be used.
      * \param[in] required_neighbors The minimum number of neighbors required.
-     * \note If required_neighbors then any number of neighbors is allowed.
+     * \note If required_neighbors is 0 then any number of neighbors is allowed.
      * \note If required_neighbors is not satisfied it projects to the mls plane.
      * \return The MLSProjectionResults for the input data.
      */
@@ -189,7 +199,7 @@ namespace pcl
      * \brief Project the query point used to generate the mls surface about using the specified method.
      * \param[in] method The projection method to be used.
      * \param[in] required_neighbors The minimum number of neighbors required.
-     * \note If required_neighbors then any number of neighbors is allowed.
+     * \note If required_neighbors is 0 then any number of neighbors is allowed.
      * \note If required_neighbors is not satisfied it projects to the mls plane.
      * \return The MLSProjectionResults for the input data.
      */
@@ -205,8 +215,8 @@ namespace pcl
       */
     template <typename PointT> void
     computeMLSSurface (const pcl::PointCloud<PointT> &cloud,
-                       int index,
-                       const std::vector<int> &nn_indices,
+                       pcl::index_t index,
+                       const pcl::Indices &nn_indices,
                        double search_radius,
                        int polynomial_order = 2,
                        std::function<double(const double)> weight_func = {});
@@ -273,7 +283,7 @@ namespace pcl
       using PointCloudInPtr = typename PointCloudIn::Ptr;
       using PointCloudInConstPtr = typename PointCloudIn::ConstPtr;
 
-      using SearchMethod = std::function<int (int, double, std::vector<int> &, std::vector<float> &)>;
+      using SearchMethod = std::function<int (pcl::index_t, double, pcl::Indices &, std::vector<float> &)>;
 
       enum UpsamplingMethod
       {
@@ -331,7 +341,7 @@ namespace pcl
       {
         tree_ = tree;
         // Declare the search locator definition
-        search_method_ = [this] (int index, double radius, std::vector<int>& k_indices, std::vector<float>& k_sqr_distances)
+        search_method_ = [this] (pcl::index_t index, double radius, pcl::Indices& k_indices, std::vector<float>& k_sqr_distances)
         {
           return tree_->radiusSearch (index, radius, k_indices, k_sqr_distances, 0);
         };
@@ -352,31 +362,6 @@ namespace pcl
       inline int
       getPolynomialOrder () const { return (order_); }
 
-      /** \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(1, 12, "use setPolynomialOrder() instead")
-      inline void
-      setPolynomialFit (bool polynomial_fit)
-      {
-        if (polynomial_fit)
-        {
-          if (order_ < 2)
-          {
-            order_ = 2;
-          }
-        }
-        else
-        {
-          order_ = 0;
-        }
-      }
-
-      /** \brief Get the polynomial_fit value (true if the surface and normal are approximated using a polynomial). */
-      PCL_DEPRECATED(1, 12, "use getPolynomialOrder() instead")
-      inline bool
-      getPolynomialFit () const { return (order_ > 1); }
-
       /** \brief Set the sphere radius that is to be used for determining the k-nearest neighbors used for fitting.
         * \param[in] radius the sphere radius that is to contain all k-nearest neighbors
         * \note Calling this method resets the squared Gaussian parameter to radius * radius !
@@ -485,8 +470,8 @@ namespace pcl
 
       /** \brief Set whether the mls results should be stored for each point in the input cloud
         * \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.
+        * \note The cache_mls_results_ is forced to be true when using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.
+        * \note If memory consumption is a concern, then set it to false when not using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.
         */
       inline void
       setCacheMLSResults (bool cache_mls_results) { cache_mls_results_ = cache_mls_results; }
@@ -509,7 +494,7 @@ namespace pcl
 
       /** \brief Get the MLSResults for input cloud
         * \note The results are only stored if setCacheMLSResults(true) was called or when using the upsampling method DISTINCT_CLOUD or VOXEL_GRID_DILATION.
-        * \note This vector is align with the input cloud indices, so use getCorrespondingIndices to get the correct results when using output cloud indices.
+        * \note This vector is aligned with the input cloud indices, so use getCorrespondingIndices to get the correct results when using output cloud indices.
         */
       inline const std::vector<MLSResult>&
       getMLSResults () const { return (mls_results_); }
@@ -579,7 +564,7 @@ namespace pcl
       int desired_num_points_in_radius_;
 
       /** \brief True if the mls results for the input cloud should be stored
-        * \note This is forced to true when using upsampling methods VOXEL_GRID_DILATION or DISTINCT_CLOUD.
+        * \note This is forced to be true when using upsampling methods VOXEL_GRID_DILATION or DISTINCT_CLOUD.
         */
       bool cache_mls_results_;
 
@@ -664,13 +649,13 @@ namespace pcl
       /** \brief Collects for each point in output the corrseponding point in the input. */
       PointIndicesPtr corresponding_input_indices_;
 
-      /** \brief Search for the closest nearest neighbors of a given point using a radius search
+      /** \brief Search for the nearest neighbors of a given point using a radius search
         * \param[in] index the index of the query point
-        * \param[out] indices the resultant vector of indices representing the k-nearest neighbors
-        * \param[out] sqr_distances the resultant squared distances from the query point to the k-nearest neighbors
+        * \param[out] indices the resultant vector of indices representing the neighbors within search_radius_
+        * \param[out] sqr_distances the resultant squared distances from the query point to the neighbors within search_radius_
         */
       inline int
-      searchForNeighbors (int index, std::vector<int> &indices, std::vector<float> &sqr_distances) const
+      searchForNeighbors (pcl::index_t index, pcl::Indices &indices, std::vector<float> &sqr_distances) const
       {
         return (search_method_ (index, search_radius_, indices, sqr_distances));
       }
@@ -678,7 +663,7 @@ namespace pcl
       /** \brief Smooth a given point and its neighborghood using Moving Least Squares.
         * \param[in] index the index of the query point in the input cloud
         * \param[in] nn_indices the set of nearest neighbors indices for pt
-        * \param[out] projected_points the set of points projected points around the query point
+        * \param[out] projected_points the set of projected points around the query point
         * (in the case of upsampling method NONE, only the query point projected to its own fitted surface will be returned,
         * in the case of the other upsampling methods, multiple points will be returned)
         * \param[out] projected_points_normals the normals corresponding to the projected points
@@ -687,15 +672,15 @@ namespace pcl
         * (used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling)
         */
       void
-      computeMLSPointNormal (int index,
-                             const std::vector<int> &nn_indices,
+      computeMLSPointNormal (pcl::index_t index,
+                             const pcl::Indices &nn_indices,
                              PointCloudOut &projected_points,
                              NormalCloud &projected_points_normals,
                              PointIndices &corresponding_input_indices,
                              MLSResult &mls_result) const;
 
 
-      /** \brief This is a helper function for add projected points
+      /** \brief This is a helper function for adding projected points
         * \param[in] index the index of the query point in the input cloud
         * \param[in] point the projected point to be added
         * \param[in] normal the projected point's normal to be added
@@ -705,7 +690,7 @@ namespace pcl
         * \param[out] corresponding_input_indices the set of indices with each point in output having the corresponding point in input
         */
       void
-      addProjectedPointNormal (int index,
+      addProjectedPointNormal (pcl::index_t index,
                                const Eigen::Vector3d &point,
                                const Eigen::Vector3d &normal,
                                double curvature,
@@ -743,9 +728,6 @@ namespace pcl
       std::string
       getClassName () const { return ("MovingLeastSquares"); }
   };
-
-  template <typename PointInT, typename PointOutT>
-  using MovingLeastSquaresOMP PCL_DEPRECATED(1, 12, "use MovingLeastSquares instead, it supports OpenMP now") = MovingLeastSquares<PointInT, PointOutT>;
 }
 
 #ifdef PCL_NO_PRECOMPILE
diff --git a/surface/include/pcl/surface/on_nurbs/nurbs_data.h b/surface/include/pcl/surface/on_nurbs/nurbs_data.h
index 7639c0ac..d74f3503 100644
--- a/surface/include/pcl/surface/on_nurbs/nurbs_data.h
+++ b/surface/include/pcl/surface/on_nurbs/nurbs_data.h
@@ -38,8 +38,6 @@
 #pragma once
 
 #include <vector>
-#include <list>
-#include <cstdio>
 
 #undef Success
 #include <Eigen/StdVector>
diff --git a/surface/include/pcl/surface/on_nurbs/nurbs_solve.h b/surface/include/pcl/surface/on_nurbs/nurbs_solve.h
index de6ae41d..491da21a 100644
--- a/surface/include/pcl/surface/on_nurbs/nurbs_solve.h
+++ b/surface/include/pcl/surface/on_nurbs/nurbs_solve.h
@@ -38,8 +38,6 @@
 #pragma once
 
 #undef Success
-#include <Eigen/Dense>
-
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
 #include <pcl/surface/on_nurbs/sparse_mat.h>
diff --git a/surface/include/pcl/surface/on_nurbs/nurbs_tools.h b/surface/include/pcl/surface/on_nurbs/nurbs_tools.h
index 3d0448e1..f683ba6b 100644
--- a/surface/include/pcl/surface/on_nurbs/nurbs_tools.h
+++ b/surface/include/pcl/surface/on_nurbs/nurbs_tools.h
@@ -42,7 +42,6 @@
 #include <pcl/surface/3rdparty/opennurbs/opennurbs.h>
 
 #undef Success
-#include <Eigen/Dense>
 
 namespace pcl
 {
diff --git a/surface/include/pcl/surface/on_nurbs/sequential_fitter.h b/surface/include/pcl/surface/on_nurbs/sequential_fitter.h
index 79131069..570128ed 100644
--- a/surface/include/pcl/surface/on_nurbs/sequential_fitter.h
+++ b/surface/include/pcl/surface/on_nurbs/sequential_fitter.h
@@ -217,7 +217,7 @@ namespace pcl
 
         /** \brief Convert point-cloud */
         static unsigned
-        PCL2ON (pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pcl_cloud, const std::vector<int> &indices, vector_vec3d &cloud);
+        PCL2ON (pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pcl_cloud, const pcl::Indices &indices, vector_vec3d &cloud);
 
       public:
         PCL_MAKE_ALIGNED_OPERATOR_NEW
diff --git a/surface/include/pcl/surface/on_nurbs/sparse_mat.h b/surface/include/pcl/surface/on_nurbs/sparse_mat.h
index 277136f7..3cd091cf 100644
--- a/surface/include/pcl/surface/on_nurbs/sparse_mat.h
+++ b/surface/include/pcl/surface/on_nurbs/sparse_mat.h
@@ -39,7 +39,6 @@
 
 #include <vector>
 #include <map>
-#include <cstdio>
 
 namespace pcl
 {
diff --git a/surface/include/pcl/surface/poisson.h b/surface/include/pcl/surface/poisson.h
index 0c6ceeae..a9ae8656 100644
--- a/surface/include/pcl/surface/poisson.h
+++ b/surface/include/pcl/surface/poisson.h
@@ -216,6 +216,20 @@ namespace pcl
       inline bool
       getManifold () { return manifold_; }
 
+      /** \brief Set the number of threads to use.
+       * \param[in] threads the number of threads
+       */
+      void
+      setThreads(int threads);
+      
+
+      /** \brief Get the number of threads*/
+      inline int
+      getThreads()
+      {
+        return threads_;
+      }
+
     protected:
       /** \brief Class get name method. */
       std::string
@@ -243,6 +257,7 @@ namespace pcl
       bool show_residual_;
       int min_iterations_;
       float solver_accuracy_;
+      int threads_;
 
       template<int Degree> void
       execute (poisson::CoredVectorMeshData &mesh,
diff --git a/surface/include/pcl/surface/qhull.h b/surface/include/pcl/surface/qhull.h
index 39eee73e..da370497 100644
--- a/surface/include/pcl/surface/qhull.h
+++ b/surface/include/pcl/surface/qhull.h
@@ -48,25 +48,14 @@
 
 extern "C"
 {
-#ifdef HAVE_QHULL_2011
-#  include "libqhull/libqhull.h"
-#  include "libqhull/mem.h"
-#  include "libqhull/qset.h"
-#  include "libqhull/geom.h"
-#  include "libqhull/merge.h"
-#  include "libqhull/poly.h"
-#  include "libqhull/io.h"
-#  include "libqhull/stat.h"
-#else
-#  include "qhull/qhull.h"
-#  include "qhull/mem.h"
-#  include "qhull/qset.h"
-#  include "qhull/geom.h"
-#  include "qhull/merge.h"
-#  include "qhull/poly.h"
-#  include "qhull/io.h"
-#  include "qhull/stat.h"
-#endif
+#include "libqhull_r/libqhull_r.h"
+#include "libqhull_r/mem_r.h"
+#include "libqhull_r/qset_r.h"
+#include "libqhull_r/geom_r.h"
+#include "libqhull_r/merge_r.h"
+#include "libqhull_r/poly_r.h"
+#include "libqhull_r/io_r.h"
+#include "libqhull_r/stat_r.h"
 }
 
 #endif
diff --git a/surface/include/pcl/surface/reconstruction.h b/surface/include/pcl/surface/reconstruction.h
index fb7436de..9a196d80 100644
--- a/surface/include/pcl/surface/reconstruction.h
+++ b/surface/include/pcl/surface/reconstruction.h
@@ -41,9 +41,7 @@
 
 #include <pcl/pcl_base.h>
 #include <pcl/PolygonMesh.h>
-#include <pcl/search/pcl_search.h>
-#include <pcl/conversions.h>
-#include <pcl/surface/boost.h>
+#include <pcl/search/search.h> // for Search
 
 namespace pcl
 {
@@ -55,6 +53,7 @@ namespace pcl
     *  - \b reconstruct(&PolygonMesh): creates a PolygonMesh object from the input data
     *
     * \author Radu B. Rusu, Michael Dixon, Alexandru E. Ichim
+    * \ingroup surface
     */
   template <typename PointInT>
   class PCLSurfaceBase: public PCLBase<PointInT>
diff --git a/surface/include/pcl/surface/simplification_remove_unused_vertices.h b/surface/include/pcl/surface/simplification_remove_unused_vertices.h
index 0ed51245..447d0ce3 100644
--- a/surface/include/pcl/surface/simplification_remove_unused_vertices.h
+++ b/surface/include/pcl/surface/simplification_remove_unused_vertices.h
@@ -40,6 +40,7 @@
 #include <vector> // for vector
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
+#include <pcl/types.h> // for pcl::Indices
 
 namespace pcl
 {
@@ -65,7 +66,7 @@ namespace pcl
         inline void
         simplify (const pcl::PolygonMesh& input, pcl::PolygonMesh& output)
         {
-          std::vector<int> indices;
+          pcl::Indices indices;
           simplify (input, output, indices);
         }
 
@@ -75,7 +76,7 @@ namespace pcl
           * \param[out] indices the resultant vector of indices
           */
         void
-        simplify (const pcl::PolygonMesh& input, pcl::PolygonMesh& output, std::vector<int>& indices);
+        simplify (const pcl::PolygonMesh& input, pcl::PolygonMesh& output, pcl::Indices& indices);
 
     };
   }
diff --git a/surface/include/pcl/surface/surfel_smoothing.h b/surface/include/pcl/surface/surfel_smoothing.h
index 3f935100..60814042 100644
--- a/surface/include/pcl/surface/surfel_smoothing.h
+++ b/surface/include/pcl/surface/surfel_smoothing.h
@@ -38,7 +38,7 @@
 #pragma once
 
 #include <pcl/pcl_base.h>
-#include <pcl/search/pcl_search.h>
+#include <pcl/search/search.h> // for Search
 
 namespace pcl
 {
diff --git a/surface/include/pcl/surface/texture_mapping.h b/surface/include/pcl/surface/texture_mapping.h
index 85250080..4fa2074a 100644
--- a/surface/include/pcl/surface/texture_mapping.h
+++ b/surface/include/pcl/surface/texture_mapping.h
@@ -44,6 +44,7 @@
 #include <pcl/surface/reconstruction.h>
 #include <pcl/common/transforms.h>
 #include <pcl/TextureMesh.h>
+#include <pcl/octree/octree_search.h> // for OctreePointCloudSearch
 
 
 namespace pcl
@@ -256,7 +257,7 @@ namespace pcl
       void
       removeOccludedPoints (const PointCloudPtr &input_cloud,
                             PointCloudPtr &filtered_cloud, const double octree_voxel_size,
-                            std::vector<int> &visible_indices, std::vector<int> &occluded_indices);
+                            pcl::Indices &visible_indices, pcl::Indices &occluded_indices);
 
       /** \brief Remove occluded points from a textureMesh
         * \param[in] tex_mesh input mesh, on witch to perform occlusion detection
diff --git a/surface/src/3rdparty/opennurbs/opennurbs_annotation2.cpp b/surface/src/3rdparty/opennurbs/opennurbs_annotation2.cpp
index e9fac7bc..3c72905c 100644
--- a/surface/src/3rdparty/opennurbs/opennurbs_annotation2.cpp
+++ b/surface/src/3rdparty/opennurbs/opennurbs_annotation2.cpp
@@ -5820,64 +5820,6 @@ void ON_Annotation2Text::SetText(const wchar_t* s)
   memset(&m_rect,0,sizeof(m_rect));
 }
 
-// SDKBREAK Oct 30, 07 - LW
-// This function should not be used any longer
-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*/,
-      const ON_DimStyle& /*dimstyle*/,
-      double /*dimscale*/,
-      const ON_Viewport* /*vp*/,
-      ON_Xform& /*xform*/
-      ) const
-{
-  ON_ERROR("This function should not be used. Use the version that takes a model transform argument.");
-  return false;
-
-  //const int gdi_height_of_I = font.HeightOfI();
-  //const double dimstyle_textheight = dimstyle.TextHeight();
-  //double dimstyle_textgap = dimstyle.TextGap();
-  //const ON::eTextDisplayMode dimstyle_textalignment 
-  //          =  ON::TextDisplayMode(dimstyle.TextAlignment()) ;
-  //const ON_3dVector cameraX = (vp) ? vp->CameraX() : m_plane.xaxis;
-  //const ON_3dVector cameraY = (vp) ? vp->CameraY() : m_plane.yaxis;
-
-  //// SDKBREAK - Oct 4, 07 LW Hack to get correct text gap using 
-  //// multi-line tolerance text since GetTextXform doesn't do that.
-  //if(( dimstyle.ToleranceStyle() == 2 || dimstyle.ToleranceStyle() == 3) &&
-  //  ( Type() == ON::dtDimLinear || Type() == ON::dtDimAligned))
-  //  dimstyle_textgap += dimstyle_textheight * 0.5;
-
-  //return GetTextXform( 
-  //    gdi_text_rect,
-  //    gdi_height_of_I,
-  //    dimstyle_textheight, dimstyle_textgap, dimstyle_textalignment,
-  //    dimscale,
-  //    cameraX, cameraY, 
-  //    xform
-  //    );
-}
-
-// New function added Oct 30, 07 - LW 
-// To use model xform to draw annotation in blocks correctly
-#if 0
-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,
-      const ON_DimStyle& dimstyle,
-      double dimscale,
-      const ON_Viewport* vp,
-      const ON_Xform* model_xform,
-      ON_Xform& xform
-      ) const
-{
-  ON_ERROR("This function should not be used. Use the one below that takes a dimstyle pointer.");
-  return false;
-}
-#endif
-
 bool ON_Annotation2::GetTextXform( 
       ON_RECT gdi_text_rect,
       const ON_Font& font,
@@ -5989,355 +5931,6 @@ static bool GetLeaderEndAndDirection( const ON_Annotation2* pAnn,
   return rc;
 }
 
-// SDKBREAK Oct 30, 07 - LW
-// This function should not be used any longer
-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*/,
-      double /*dimstyle_textheight*/,
-      double /*dimstyle_textgap*/,
-      ON::eTextDisplayMode /*dimstyle_textalignment*/,
-      double /*dimscale*/,
-      ON_3dVector /*cameraX*/,
-      ON_3dVector /*cameraY*/,
-      ON_Xform& /*xform*/
-      ) const
-{
-  ON_ERROR("This function should not be used. Use the version that takes a model transform argument.");
-  return false;
-
-  //const ON_Annotation2* ann = this;
-
-  //const ON::eAnnotationType ann_type = ann->m_type;
-
-  //if ( 0 == gdi_height_of_I )
-  //{
-  //  // Default to height of Ariel 'I'
-  //  gdi_height_of_I = (165*ON_Font::normal_font_height)/256;
-  //}
-
-  //if ( 0.0 == dimscale )
-  //{
-  //  dimscale = 1.0;
-  //}
-
-  //dimstyle_textheight *= dimscale;
-  //dimstyle_textgap *= dimscale;
-
-  //double textheight = ( ON::dtTextBlock == ann_type )
-  //                  ? m_textheight*dimscale
-  //                  : dimstyle_textheight;
-  //if ( 0.0 == textheight )
-  //  textheight = 1.0;
-
-  //ON_3dVector cameraZ = ON_CrossProduct( cameraX, cameraY );
-  //if ( fabs( 1.0 - cameraZ.Length() ) > ON_SQRT_EPSILON )
-  //{
-  //  cameraZ.Unitize();
-  //}
-
-  //// This xform is a scale from Windows gdi coordinates 
-  //// to annotation plane coordinates.
-  //const double gdi_to_plane_scale = textheight/gdi_height_of_I;
-  //ON_Xform gdi_to_plane(1.0);
-  //gdi_to_plane.m_xform[0][0] =  gdi_to_plane_scale;
-  //gdi_to_plane.m_xform[1][1] = -gdi_to_plane_scale;
-
-  //// width and height of text line in Rhino units.
-  //const double text_line_width  = gdi_to_plane_scale*(gdi_text_rect.right - gdi_text_rect.left);
-  ////const double text_line_height = gdi_to_plane_scale*(gdi_text_rect.bottom - gdi_text_rect.top);
-
-  //if ( ON::dtTextBlock == ann_type )
-  //{
-  //  // The orientation of the text is text blocks
-  //  // does not depend on the view or text alignment
-  //  // settings.  The position and orientation of 
-  //  // the text in every other annotation depends on
-  //  // the view and text alignment settings.  
-  //  //
-  //  // It simplifies the code for the rest of the
-  //  // annotation settings to quickly deal with text
-  //  // blocks here.
-  //  ON_Xform plane_to_world(1.0);
-  //  plane_to_world.Rotation(ON_xy_plane,ann->m_plane);
-  //  xform = plane_to_world*gdi_to_plane;
-  //  return true;
-  //}
-
-
-  //// text_position_mode
-  ////   1 = linear, aligned, or anglular dimension
-  ////       (dimension definition determines center point of text box)
-  ////   2 = radial, diameter, leader
-  ////       (dimension definition determined end point of text box)
-  //int position_style = 0;
-  //switch( ann_type )
-  //{
-  //case ON::dtDimAligned:
-  //case ON::dtDimLinear:
-  //case ON::dtDimAngular:
-  //  // dimension definition determines center point of text box
-  //  position_style = 1;
-  //  break;
-
-  //case ON::dtLeader:
-  //case ON::dtDimRadius:
-  //case ON::dtDimDiameter:
-  //case ON::dtDimOrdinate:
-  //  // dimension definition determines end of text box
-  //  position_style = 2;
-  //  break;
-
-  //case ON::dtTextBlock:
-  //case ON::dtNothing:
-  //  break;
-  //}
-
-
-  //// This translation puts the center of the fist line of text at
-  //// (0,0) in the annotation's plane.
-  //if ( ON::dtHorizontal != dimstyle_textalignment || 1 == position_style )
-  //{
-
-  //  gdi_to_plane.m_xform[0][3] = -0.5*text_line_width;
-  //  gdi_to_plane.m_xform[0][3] = -0.5*text_line_width;
-  //}
-  //gdi_to_plane.m_xform[1][3] = -0.5*textheight;
-
-  //if ( ON::dtHorizontal != dimstyle_textalignment )
-  //{
-  //  if ( ((cameraZ*m_plane.zaxis) < -ON_SQRT_EPSILON) )
-  //  {
-  //    // Viewing dimension from the backside
-  //    ON_Xform flip(1.0);
-  //    switch ( position_style )
-  //    {
-  //    case 1: // ON::dtDimLinear, ON::dtDimAligned, ON::dtDimAngular
-  //      flip.m_xform[0][0] = -1.0;
-  //      flip.m_xform[0][3] = gdi_text_rect.left + gdi_text_rect.right;
-  //      break;
-
-  //    case 2: // ON::dtDimDiameter, ON::dtDimRadius, ON::dtLeader
-  //      flip.m_xform[1][1] = -1.0;
-  //      flip.m_xform[1][3] = gdi_text_rect.top + gdi_text_rect.bottom;
-  //      break;
-  //    }
-  //    gdi_to_plane = gdi_to_plane*flip;
-  //  }
-  //}
-
-  //// text_centering_rotation rotates about the "center".  Angular,
-  //// radial, and leader dimensions use this rotation.
-  //ON_2dVector text_centering_rotation(1.0,0.0);
-
-  //// text_centering_translation is a small translation deals with
-  //// text that is above or to the right of the "center" point.  
-  //// It is no larger than dimstyle_gap + 1/2 the size of the
-  //// text's bounding box.
-  //ON_2dVector text_centering_translation(0.0,0.0);
-
-  //double x, y;
-
-  //if ( ON::dtHorizontal != dimstyle_textalignment )
-  //{
-  //  if ( ON::dtDimLinear  == ann_type || ON::dtDimAligned == ann_type )
-  //  {
-  //    if ( ON::dtAboveLine == dimstyle_textalignment )
-  //    {
-  //      text_centering_translation.y =  0.5*textheight+dimstyle_textgap;
-  //    }
-  //    y =  ann->m_plane.yaxis*cameraY;
-  //    x = -ann->m_plane.yaxis*cameraX;
-  //    if ( fabs(y) <= ON_SQRT_EPSILON && fabs(x) > ON_SQRT_EPSILON )
-  //    {
-  //      y = x;
-  //    }
-  //    if ( y < 0.0 )
-  //    {
-  //      text_centering_translation.Reverse();
-  //      text_centering_rotation.Reverse(); // rotate 180 degrees
-  //    }
-  //  }
-  //  else if ( ON::dtDimAngular == ann_type )
-  //  {
-  //    // This transform rotates the text in the annotation plane.
-  //    const ON_AngularDimension2* angular_dim = ON_AngularDimension2::Cast(ann);
-  //    if ( 0 != angular_dim )
-  //    {
-  //      double a = 0.5*angular_dim->m_angle;
-  //      ON_2dVector R(cos(a),sin(a));
-  //      a -= 0.5*ON_PI;
-  //      text_centering_rotation.x = cos(a);
-  //      text_centering_rotation.y = sin(a);
-  //      ON_3dVector V = R.x*m_plane.xaxis + R.y*m_plane.yaxis;
-  //      x = V*cameraX;
-  //      y = V*cameraY;
-  //      if ( fabs(y) <= ON_SQRT_EPSILON && fabs(x) > ON_SQRT_EPSILON )
-  //      {
-  //        y = -x;
-  //      }
-  //      if ( y < 0.0 )
-  //      {
-  //        text_centering_rotation.Reverse(); // add another 180 degrees of rotation
-  //      }
-
-  //      if ( ON::dtAboveLine == dimstyle_textalignment )
-  //      {
-  //        y = 0.5*textheight + dimstyle_textgap;
-  //        text_centering_translation.x = -y*text_centering_rotation.y;
-  //        text_centering_translation.y =  y*text_centering_rotation.x;
-  //      }
-  //    }
-  //  }
-  //  else if (    ON::dtDimDiameter == ann_type 
-  //            || ON::dtDimRadius == ann_type 
-  //            || ON::dtLeader == ann_type 
-  //            || ON::dtDimOrdinate == ann_type)
-  //  {
-  //    ON_2dPoint E(0.0,0.0); // end point
-  //    ON_2dVector R(1.0,0.0); // unit vector from penultimate point to end point
-  //    GetLeaderEndAndDirection( this, E, R );
-
-  //    text_centering_rotation = R;
-
-  //    text_centering_translation = (dimstyle_textgap + 0.5*text_line_width)*text_centering_rotation;
-
-  //    ON_3dVector V = text_centering_rotation.x*m_plane.xaxis + text_centering_rotation.y*m_plane.yaxis;
-  //    x = V*cameraX;
-  //    y = V*cameraY;
-  //    if ( fabs(x) <= ON_SQRT_EPSILON && fabs(y) > ON_SQRT_EPSILON )
-  //    {
-  //      x = y;
-  //    }
-  //    if ( x < 0.0 )
-  //    {
-  //      text_centering_rotation.Reverse(); // rotate 180 degrees
-  //    }
-  //  }
-  //}
-
-  //ON_Xform text_centering_xform(1.0);
-  //text_centering_xform.m_xform[0][0] =  text_centering_rotation.x;
-  //text_centering_xform.m_xform[0][1] = -text_centering_rotation.y;
-  //text_centering_xform.m_xform[1][0] =  text_centering_rotation.y;
-  //text_centering_xform.m_xform[1][1] =  text_centering_rotation.x;
-  //// Since the translation happens after the rotation about (0,0),
-  //// we can just tack it on here.
-  //text_centering_xform.m_xform[0][3] =  text_centering_translation.x;
-  //text_centering_xform.m_xform[1][3] =  text_centering_translation.y;
-
-  //// This transform translates the text in the annotation plane
-  //// It can be a large translation
-  //ON_2dVector text_offset_translation(0.0,0.0); // CRhinoText::Offset() = text->Offset()
-  //switch( ann_type )
-  //{
-  //case ON::dtDimLinear:
-  //case ON::dtDimAligned:
-  //  if ( m_points.Count() >= ON_LinearDimension2::dim_pt_count )
-  //  {
-  //    const ON_LinearDimension2* linear_dim = ON_LinearDimension2::Cast(ann);
-  //    if ( linear_dim )
-  //    {
-  //      text_offset_translation = linear_dim->Dim2dPoint(ON_LinearDimension2::text_pivot_pt);
-  //    }
-  //  }
-  //  break;
-
-  //case ON::dtDimAngular:
-  //  if ( m_points.Count() >= ON_AngularDimension2::dim_pt_count )
-  //  {
-  //    const ON_AngularDimension2* angular_dim = ON_AngularDimension2::Cast(ann);
-  //    if ( angular_dim )
-  //    {
-  //      text_offset_translation = angular_dim->Dim2dPoint(ON_AngularDimension2::text_pivot_pt);
-  //    }
-  //  }
-  //  break;
-
-  //case ON::dtDimDiameter:
-  //case ON::dtDimRadius:
-  //  if ( m_points.Count() >= ON_RadialDimension2::dim_pt_count )
-  //  {
-  //    // No user positioned text on radial dimensions.
-  //    text_offset_translation = m_points[ON_RadialDimension2::tail_pt_index];
-  //  }
-  //  break;
-
-  //case ON::dtLeader:
-  //  if ( m_points.Count() > 0 )
-  //  {
-  //    // No user positioned text on leaders.
-  //    text_offset_translation = *m_points.Last();
-  //  }
-  //  break;
-
-  //case ON::dtDimOrdinate:
-  //  if ( m_points.Count() == 2 )
-  //  {
-  //    // No user positioned text on leaders.
-  //    text_offset_translation = m_points[1];
-  //  }
-  //  break;
-
-  //case ON::dtTextBlock:
-  //case ON::dtNothing:
-  //  break;
-  //}
-
-  //ON_Xform plane_translation(1.0);
-  //plane_translation.m_xform[0][3] = text_offset_translation.x;
-  //plane_translation.m_xform[1][3] = text_offset_translation.y;
-
-  //// this transform maps a point in the annotation plane to world coordinates
-  //ON_Xform plane_to_world(1.0);
-  //plane_to_world.Rotation(ON_xy_plane,ann->m_plane);
-
-  //ON_Xform horizonal_xform(1.0);
-  //if ( ON::dtHorizontal == dimstyle_textalignment )
-  //{
-  //  ON_3dPoint fixed_point = ann->m_plane.PointAt(text_offset_translation.x,text_offset_translation.y);
-  //  horizonal_xform.Rotation( 
-  //      fixed_point,
-  //      ann->m_plane.xaxis,
-  //      ann->m_plane.yaxis,
-  //      ann->m_plane.zaxis,
-  //      fixed_point,
-  //      cameraX,
-  //      cameraY,
-  //      cameraZ
-  //      );
-
-  //  if ( 2 == position_style )
-  //  {
-  //    // leaders, radial, and diameter
-  //    ON_2dPoint E(0.0,0.0); // end point
-  //    ON_2dVector R(1.0,0.0); // unit vector from penultimate point to end point
-  //    GetLeaderEndAndDirection( this, E, R );
-  //    ON_3dVector V = R.x*m_plane.xaxis + R.y*m_plane.yaxis;
-  //    x = V*cameraX;
-  //    y = ( x > -ON_SQRT_EPSILON )
-  //      ? dimstyle_textgap
-  //      : -(dimstyle_textgap + text_line_width);
-  //    V = y*cameraX;
-  //    horizonal_xform.m_xform[0][3] += V.x;
-  //    horizonal_xform.m_xform[1][3] += V.y;
-  //    horizonal_xform.m_xform[2][3] += V.z;
-  //  }
-  //}
-
-  //ON_Xform gdi_to_world;
-  //gdi_to_world = horizonal_xform
-  //              * plane_to_world
-  //              * plane_translation
-  //              * text_centering_xform
-  //              * gdi_to_plane;
-
-  //xform = gdi_to_world;
-
-  //return true;
-}
-
 //static bool do_plane_translation = true;
 //static bool do_text_centering_xform = true;
 //static bool do_text_centering_rotation = true;
diff --git a/surface/src/3rdparty/poisson4/marching_cubes_poisson.cpp b/surface/src/3rdparty/poisson4/marching_cubes_poisson.cpp
index 81648389..b142ce88 100644
--- a/surface/src/3rdparty/poisson4/marching_cubes_poisson.cpp
+++ b/surface/src/3rdparty/poisson4/marching_cubes_poisson.cpp
@@ -25,7 +25,6 @@ CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
 ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 DAMAGE.
 */
-#include <math.h>
 #include <pcl/surface/3rdparty/poisson4/marching_cubes_poisson.h>
 
 ////////////
diff --git a/surface/src/ear_clipping.cpp b/surface/src/ear_clipping.cpp
index c4cef9e6..1bc9e1c1 100644
--- a/surface/src/ear_clipping.cpp
+++ b/surface/src/ear_clipping.cpp
@@ -76,7 +76,7 @@ pcl::EarClipping::triangulate (const Vertices& vertices, PolygonMesh& output)
     return;
   }
 
-  std::vector<std::uint32_t> remaining_vertices (n_vertices);
+  Indices remaining_vertices (n_vertices);
   if (area (vertices.vertices) > 0) // clockwise?
     remaining_vertices = vertices.vertices;
   else
@@ -112,7 +112,7 @@ pcl::EarClipping::triangulate (const Vertices& vertices, PolygonMesh& output)
 
 /////////////////////////////////////////////////////////////////////////////////////////////
 float
-pcl::EarClipping::area (const std::vector<std::uint32_t>& vertices)
+pcl::EarClipping::area (const Indices& vertices)
 {
     //if the polygon is projected onto the xy-plane, the area of the polygon is determined
     //by the trapeze formula of Gauss. However this fails, if the projection is one 'line'.
@@ -146,10 +146,9 @@ pcl::EarClipping::area (const std::vector<std::uint32_t>& vertices)
     return area * 0.5f; 
 }
 
-
 /////////////////////////////////////////////////////////////////////////////////////////////
 bool
-pcl::EarClipping::isEar (int u, int v, int w, const std::vector<std::uint32_t>& vertices)
+pcl::EarClipping::isEar (int u, int v, int w, const Indices& vertices)
 {
   Eigen::Vector3f p_u, p_v, p_w;
   p_u = (*points_)[vertices[u]].getVector3fMap();
diff --git a/surface/src/marching_cubes_hoppe.cpp b/surface/src/marching_cubes_hoppe.cpp
index 9e75e152..9a029837 100644
--- a/surface/src/marching_cubes_hoppe.cpp
+++ b/surface/src/marching_cubes_hoppe.cpp
@@ -37,7 +37,6 @@
 #include <pcl/point_types.h>
 #include <pcl/surface/marching_cubes_hoppe.h>
 #include <pcl/surface/impl/marching_cubes_hoppe.hpp>
-#include <pcl/surface/impl/marching_cubes.hpp>
 
 // Instantiations of specific point types
 PCL_INSTANTIATE(MarchingCubesHoppe, (pcl::PointNormal)(pcl::PointXYZRGBNormal)(pcl::PointXYZINormal))
diff --git a/surface/src/on_nurbs/closing_boundary.cpp b/surface/src/on_nurbs/closing_boundary.cpp
index 8b25eaf2..bb4ef245 100644
--- a/surface/src/on_nurbs/closing_boundary.cpp
+++ b/surface/src/on_nurbs/closing_boundary.cpp
@@ -36,6 +36,7 @@
  */
 
 #include <pcl/surface/on_nurbs/closing_boundary.h>
+#include <Eigen/Geometry> // for cross
 
 using namespace pcl;
 using namespace on_nurbs;
diff --git a/surface/src/on_nurbs/fitting_curve_2d.cpp b/surface/src/on_nurbs/fitting_curve_2d.cpp
index 3dd75999..93aee2e8 100644
--- a/surface/src/on_nurbs/fitting_curve_2d.cpp
+++ b/surface/src/on_nurbs/fitting_curve_2d.cpp
@@ -37,6 +37,7 @@
 
 #include <pcl/surface/on_nurbs/fitting_curve_2d.h>
 #include <stdexcept>
+#include <Eigen/LU> // for inverse
 
 using namespace pcl;
 using namespace on_nurbs;
diff --git a/surface/src/on_nurbs/fitting_curve_2d_apdm.cpp b/surface/src/on_nurbs/fitting_curve_2d_apdm.cpp
index faafb9d8..3c2588d3 100644
--- a/surface/src/on_nurbs/fitting_curve_2d_apdm.cpp
+++ b/surface/src/on_nurbs/fitting_curve_2d_apdm.cpp
@@ -38,6 +38,7 @@
 #include <pcl/surface/on_nurbs/fitting_curve_2d_apdm.h>
 #include <pcl/pcl_macros.h>
 #include <stdexcept>
+#include <Eigen/Geometry> // for cross
 
 using namespace pcl;
 using namespace on_nurbs;
diff --git a/surface/src/on_nurbs/fitting_curve_2d_asdm.cpp b/surface/src/on_nurbs/fitting_curve_2d_asdm.cpp
index b198dc7a..dbdc0419 100644
--- a/surface/src/on_nurbs/fitting_curve_2d_asdm.cpp
+++ b/surface/src/on_nurbs/fitting_curve_2d_asdm.cpp
@@ -37,6 +37,7 @@
 
 #include <pcl/surface/on_nurbs/fitting_curve_2d_asdm.h>
 #include <stdexcept>
+#include <Eigen/Geometry> // for cross
 
 using namespace pcl;
 using namespace on_nurbs;
diff --git a/surface/src/on_nurbs/fitting_curve_2d_atdm.cpp b/surface/src/on_nurbs/fitting_curve_2d_atdm.cpp
index 7f7642ea..8c66856a 100644
--- a/surface/src/on_nurbs/fitting_curve_2d_atdm.cpp
+++ b/surface/src/on_nurbs/fitting_curve_2d_atdm.cpp
@@ -37,6 +37,7 @@
 
 #include <pcl/surface/on_nurbs/fitting_curve_2d_atdm.h>
 #include <stdexcept>
+#include <Eigen/Geometry> // for cross
 
 using namespace pcl;
 using namespace on_nurbs;
diff --git a/surface/src/on_nurbs/fitting_cylinder_pdm.cpp b/surface/src/on_nurbs/fitting_cylinder_pdm.cpp
index 726b32ff..ecb88446 100644
--- a/surface/src/on_nurbs/fitting_cylinder_pdm.cpp
+++ b/surface/src/on_nurbs/fitting_cylinder_pdm.cpp
@@ -38,6 +38,9 @@
 #include <stdexcept>
 #include <pcl/surface/on_nurbs/fitting_cylinder_pdm.h>
 #include <pcl/pcl_macros.h>
+#include <Eigen/Cholesky> // for ldlt
+#include <Eigen/Geometry> // for cross
+#include <Eigen/LU> // for inverse
 
 using namespace pcl;
 using namespace on_nurbs;
diff --git a/surface/src/on_nurbs/fitting_sphere_pdm.cpp b/surface/src/on_nurbs/fitting_sphere_pdm.cpp
index f65ac82c..802a81ad 100644
--- a/surface/src/on_nurbs/fitting_sphere_pdm.cpp
+++ b/surface/src/on_nurbs/fitting_sphere_pdm.cpp
@@ -38,6 +38,8 @@
 #include <stdexcept>
 #include <pcl/surface/on_nurbs/fitting_sphere_pdm.h>
 #include <pcl/pcl_macros.h>
+#include <Eigen/Cholesky> // for ldlt
+#include <Eigen/Geometry> // for cross
 
 using namespace pcl;
 using namespace on_nurbs;
diff --git a/surface/src/on_nurbs/fitting_surface_im.cpp b/surface/src/on_nurbs/fitting_surface_im.cpp
index 05bfb1f8..75721685 100644
--- a/surface/src/on_nurbs/fitting_surface_im.cpp
+++ b/surface/src/on_nurbs/fitting_surface_im.cpp
@@ -38,6 +38,7 @@
 #include <pcl/surface/on_nurbs/fitting_surface_im.h>
 #include <pcl/pcl_macros.h>
 
+#include <Eigen/Cholesky> // for ldlt
 #include <stdexcept>
 
 using namespace pcl;
@@ -54,7 +55,7 @@ FittingSurfaceIM::computeMean () const
   double ds = 1.0 / double (m_indices.size ());
 
   const pcl::PointCloud<pcl::PointXYZRGB> &cloud_ref = *m_cloud;
-  for (const int &index : m_indices)
+  for (const auto &index : m_indices)
   {
     int i = index % cloud_ref.width;
     int j = index / cloud_ref.width;
@@ -78,7 +79,7 @@ FittingSurfaceIM::computeIndexBoundingBox (pcl::PointCloud<pcl::PointXYZRGB>::Pt
   Eigen::Vector4d bb = Eigen::Vector4d (DBL_MAX, 0, DBL_MAX, 0);
   const pcl::PointCloud<pcl::PointXYZRGB> &cloud_ref = *cloud;
 
-  for (const int &index : indices)
+  for (const auto &index : indices)
   {
     int i = index % cloud_ref.width;
     int j = index / cloud_ref.width;
@@ -275,7 +276,7 @@ FittingSurfaceIM::assemble (bool inverse_mapping)
 
   // assemble data points
   const pcl::PointCloud<pcl::PointXYZRGB> &cloud_ref = *m_cloud;
-  for (const int &index : m_indices)
+  for (const auto &index : m_indices)
   {
     int px = index % cloud_ref.width;
     int py = index / cloud_ref.width;
diff --git a/surface/src/on_nurbs/fitting_surface_pdm.cpp b/surface/src/on_nurbs/fitting_surface_pdm.cpp
index 5325c13f..298f99f7 100644
--- a/surface/src/on_nurbs/fitting_surface_pdm.cpp
+++ b/surface/src/on_nurbs/fitting_surface_pdm.cpp
@@ -38,6 +38,9 @@
 #include <pcl/surface/on_nurbs/fitting_surface_pdm.h>
 #include <pcl/pcl_macros.h>
 
+#include <Eigen/Cholesky> // for ldlt
+#include <Eigen/Geometry> // for cross
+#include <Eigen/LU> // for inverse
 #include <stdexcept>
 
 using namespace pcl;
diff --git a/surface/src/on_nurbs/fitting_surface_tdm.cpp b/surface/src/on_nurbs/fitting_surface_tdm.cpp
index 3dd10adc..fd71feb6 100644
--- a/surface/src/on_nurbs/fitting_surface_tdm.cpp
+++ b/surface/src/on_nurbs/fitting_surface_tdm.cpp
@@ -38,6 +38,7 @@
 #include <pcl/surface/on_nurbs/fitting_surface_tdm.h>
 #include <pcl/pcl_macros.h>
 
+#include <Eigen/Geometry> // for cross
 #include <stdexcept>
 
 using namespace pcl;
diff --git a/surface/src/on_nurbs/global_optimization_pdm.cpp b/surface/src/on_nurbs/global_optimization_pdm.cpp
index cd8b9fde..079dc072 100644
--- a/surface/src/on_nurbs/global_optimization_pdm.cpp
+++ b/surface/src/on_nurbs/global_optimization_pdm.cpp
@@ -39,6 +39,7 @@
 #include <pcl/surface/on_nurbs/closing_boundary.h>
 #include <pcl/pcl_macros.h>
 
+#include <Eigen/Geometry> // for cross
 #include <stdexcept>
 
 #undef DEBUG
diff --git a/surface/src/on_nurbs/global_optimization_tdm.cpp b/surface/src/on_nurbs/global_optimization_tdm.cpp
index 0662b5c6..fa9bd216 100644
--- a/surface/src/on_nurbs/global_optimization_tdm.cpp
+++ b/surface/src/on_nurbs/global_optimization_tdm.cpp
@@ -39,6 +39,7 @@
 #include <pcl/surface/on_nurbs/closing_boundary.h>
 #include <pcl/pcl_macros.h>
 
+#include <Eigen/Geometry> // for cross
 #include <stdexcept>
 
 using namespace pcl;
diff --git a/surface/src/on_nurbs/nurbs_solve_eigen.cpp b/surface/src/on_nurbs/nurbs_solve_eigen.cpp
index ff2b4168..0c28bf60 100644
--- a/surface/src/on_nurbs/nurbs_solve_eigen.cpp
+++ b/surface/src/on_nurbs/nurbs_solve_eigen.cpp
@@ -38,6 +38,8 @@
 #include <iostream>
 #include <stdexcept>
 
+#include <Eigen/SVD> // for jacobiSvd
+
 #include <pcl/surface/on_nurbs/nurbs_solve.h>
 
 using namespace pcl;
diff --git a/surface/src/on_nurbs/nurbs_solve_umfpack.cpp b/surface/src/on_nurbs/nurbs_solve_umfpack.cpp
index 1ac922cc..92682ed1 100644
--- a/surface/src/on_nurbs/nurbs_solve_umfpack.cpp
+++ b/surface/src/on_nurbs/nurbs_solve_umfpack.cpp
@@ -39,6 +39,7 @@
 #include <suitesparse/umfpack.h>
 #include <iostream>
 #include <stdio.h>
+#include <time.h> // for clock, clock_t
 #include <stdexcept>
 
 #include <pcl/surface/on_nurbs/nurbs_solve.h>
diff --git a/surface/src/on_nurbs/nurbs_tools.cpp b/surface/src/on_nurbs/nurbs_tools.cpp
index 53c2876e..d2be4c33 100644
--- a/surface/src/on_nurbs/nurbs_tools.cpp
+++ b/surface/src/on_nurbs/nurbs_tools.cpp
@@ -39,6 +39,8 @@
 #include <stdexcept>
 #include <map>
 #include <algorithm>
+#include <Eigen/Eigenvalues> // for SelfAdjointEigenSolver
+#include <Eigen/Geometry> // for cross
 #include <pcl/surface/on_nurbs/nurbs_tools.h>
 
 using namespace pcl;
diff --git a/surface/src/on_nurbs/sequential_fitter.cpp b/surface/src/on_nurbs/sequential_fitter.cpp
index fcd68467..4ce521db 100644
--- a/surface/src/on_nurbs/sequential_fitter.cpp
+++ b/surface/src/on_nurbs/sequential_fitter.cpp
@@ -37,6 +37,8 @@
 
 #include <pcl/surface/on_nurbs/sequential_fitter.h>
 
+#include <Eigen/Geometry> // for cross
+
 using namespace pcl;
 using namespace on_nurbs;
 using namespace Eigen;
@@ -603,12 +605,12 @@ SequentialFitter::grow (float max_dist, float max_angle, unsigned min_length, un
 }
 
 unsigned
-SequentialFitter::PCL2ON (pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pcl_cloud, const std::vector<int> &indices,
+SequentialFitter::PCL2ON (pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pcl_cloud, const pcl::Indices &indices,
                           vector_vec3d &on_cloud)
 {
   std::size_t numPoints = 0;
 
-  for (const int &index : indices)
+  for (const auto &index : indices)
   {
 
     pcl::PointXYZRGB &pt = pcl_cloud->at (index);
diff --git a/surface/src/on_nurbs/sparse_mat.cpp b/surface/src/on_nurbs/sparse_mat.cpp
index 82fdeaf5..3c429b04 100644
--- a/surface/src/on_nurbs/sparse_mat.cpp
+++ b/surface/src/on_nurbs/sparse_mat.cpp
@@ -31,11 +31,12 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  *
- * 
+ *
  *
  */
 
 #include <pcl/surface/on_nurbs/sparse_mat.h>
+#include <cstdio> // for printf
 
 using namespace pcl;
 using namespace on_nurbs;
diff --git a/surface/src/on_nurbs/triangulation.cpp b/surface/src/on_nurbs/triangulation.cpp
index fccd8103..baaa3508 100644
--- a/surface/src/on_nurbs/triangulation.cpp
+++ b/surface/src/on_nurbs/triangulation.cpp
@@ -40,6 +40,8 @@
 #include <pcl/surface/on_nurbs/fitting_curve_2d_apdm.h>
 #include <pcl/conversions.h>
 
+#include <Eigen/Geometry> // for cross
+
 using namespace pcl;
 using namespace on_nurbs;
 
diff --git a/surface/src/simplification_remove_unused_vertices.cpp b/surface/src/simplification_remove_unused_vertices.cpp
index 692e2ce3..fa2d2b4c 100644
--- a/surface/src/simplification_remove_unused_vertices.cpp
+++ b/surface/src/simplification_remove_unused_vertices.cpp
@@ -43,7 +43,7 @@
 #include <iostream>
 
 void
-pcl::surface::SimplificationRemoveUnusedVertices::simplify(const pcl::PolygonMesh& input, pcl::PolygonMesh& output, std::vector<int>& indices)
+pcl::surface::SimplificationRemoveUnusedVertices::simplify(const pcl::PolygonMesh& input, pcl::PolygonMesh& output, pcl::Indices& indices)
 {
   if (input.polygons.empty ())
     return;
diff --git a/surface/src/vtk_smoothing/vtk_mesh_smoothing_laplacian.cpp b/surface/src/vtk_smoothing/vtk_mesh_smoothing_laplacian.cpp
index 5250dda3..d0141a39 100644
--- a/surface/src/vtk_smoothing/vtk_mesh_smoothing_laplacian.cpp
+++ b/surface/src/vtk_smoothing/vtk_mesh_smoothing_laplacian.cpp
@@ -39,7 +39,6 @@
 #include <pcl/surface/vtk_smoothing/vtk_mesh_smoothing_laplacian.h>
 #include <pcl/surface/vtk_smoothing/vtk_utils.h>
 
-#include <vtkVersion.h>
 #include <vtkSmoothPolyDataFilter.h>
 
 
diff --git a/surface/src/vtk_smoothing/vtk_mesh_smoothing_windowed_sinc.cpp b/surface/src/vtk_smoothing/vtk_mesh_smoothing_windowed_sinc.cpp
index a79fac37..ca7d13eb 100644
--- a/surface/src/vtk_smoothing/vtk_mesh_smoothing_windowed_sinc.cpp
+++ b/surface/src/vtk_smoothing/vtk_mesh_smoothing_windowed_sinc.cpp
@@ -39,7 +39,6 @@
 #include <pcl/surface/vtk_smoothing/vtk_mesh_smoothing_windowed_sinc.h>
 #include <pcl/surface/vtk_smoothing/vtk_utils.h>
 
-#include <vtkVersion.h>
 #include <vtkWindowedSincPolyDataFilter.h>
 
 
diff --git a/surface/src/vtk_smoothing/vtk_mesh_subdivision.cpp b/surface/src/vtk_smoothing/vtk_mesh_subdivision.cpp
index 58b8dff9..5ff489a9 100644
--- a/surface/src/vtk_smoothing/vtk_mesh_subdivision.cpp
+++ b/surface/src/vtk_smoothing/vtk_mesh_subdivision.cpp
@@ -39,7 +39,6 @@
 #include <pcl/surface/vtk_smoothing/vtk_mesh_subdivision.h>
 #include <pcl/surface/vtk_smoothing/vtk_utils.h>
 
-#include <vtkVersion.h>
 #include <vtkLinearSubdivisionFilter.h>
 #include <vtkLoopSubdivisionFilter.h>
 #include <vtkButterflySubdivisionFilter.h>
diff --git a/surface/src/vtk_smoothing/vtk_utils.cpp b/surface/src/vtk_smoothing/vtk_utils.cpp
index 892c366c..8c0a9449 100644
--- a/surface/src/vtk_smoothing/vtk_utils.cpp
+++ b/surface/src/vtk_smoothing/vtk_utils.cpp
@@ -42,13 +42,13 @@
 #include <pcl/PolygonMesh.h>
 #include <pcl/conversions.h>
 #include <pcl/point_types.h> // for PointXYZ, PointXYZRGB, RGB
-#include <vtkVersion.h>
 #include <vtkCellArray.h>
 #include <vtkTriangleFilter.h>
 #include <vtkPoints.h>
 #include <vtkPolyData.h>
 #include <vtkPointData.h>
 #include <vtkFloatArray.h>
+#include <vtkUnsignedCharArray.h>
 
 // Support for VTK 7.1 upwards
 #ifdef vtkGenericDataArray_h
@@ -155,7 +155,11 @@ pcl::VTKUtils::vtk2mesh (const vtkSmartPointer<vtkPolyData>& poly_data, pcl::Pol
   }
 
   mesh.polygons.resize (nr_polygons);
+#ifdef VTK_CELL_ARRAY_V2
+  vtkIdType const *cell_points;
+#else
   vtkIdType* cell_points;
+#endif
   vtkIdType nr_cell_points;
   vtkCellArray * mesh_polygons = poly_data->GetPolys ();
   mesh_polygons->InitTraversal ();
diff --git a/test/2d/test_2d.cpp b/test/2d/test_2d.cpp
index b669b589..78fe8602 100644
--- a/test/2d/test_2d.cpp
+++ b/test/2d/test_2d.cpp
@@ -47,7 +47,6 @@
 #include <fstream>
 
 #include <pcl/point_types.h>
-#include <pcl/pcl_base.h>
 #include <pcl/io/pcd_io.h>
 
 char *lena;
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 6c60f68d..cb7f7753 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -23,9 +23,21 @@ if(MSVC)
   set(PCL_CTEST_ARGUMENTS ${PCL_CTEST_ARGUMENTS} -C $<$<CONFIG:Debug>:Debug>$<$<CONFIG:Release>:Release>)
 endif()
 
+# Enables you to disable visualization tests. Used on CI.
+if(PCL_DISABLE_VISUALIZATION_TESTS)
+  list(APPEND EXCLUDE_TESTS visualization)
+endif()
+
 # Enables you to disable GPU tests. Used on CI as it has no access to GPU hardware
 if(PCL_DISABLE_GPU_TESTS)
-  set(PCL_CTEST_ARGUMENTS ${PCL_CTEST_ARGUMENTS} -E gpu)
+  list(APPEND EXCLUDE_TESTS gpu)
+endif()
+
+#Check if there are any tests to exclude
+if(EXCLUDE_TESTS)
+  message(STATUS "Tests excluded: ${EXCLUDE_TESTS}")
+  string(REPLACE ";" "|" EXCLUDE_TESTS_REGEX "${EXCLUDE_TESTS}")
+  set(PCL_CTEST_ARGUMENTS ${PCL_CTEST_ARGUMENTS} -E "(${EXCLUDE_TESTS_REGEX})")
 endif()
 
 add_custom_target(tests "${CMAKE_CTEST_COMMAND}" ${PCL_CTEST_ARGUMENTS} -V -T Test VERBATIM)
@@ -43,6 +55,7 @@ add_subdirectory(gpu)
 add_subdirectory(io)
 add_subdirectory(kdtree)
 add_subdirectory(keypoints)
+add_subdirectory(ml)
 add_subdirectory(people)
 add_subdirectory(octree)
 add_subdirectory(outofcore)
diff --git a/test/common/CMakeLists.txt b/test/common/CMakeLists.txt
index c3c0d1e5..b6872cdf 100644
--- a/test/common/CMakeLists.txt
+++ b/test/common/CMakeLists.txt
@@ -17,6 +17,7 @@ PCL_ADD_TEST(common_test_wrappers test_wrappers FILES test_wrappers.cpp LINK_WIT
 PCL_ADD_TEST(common_test_macros test_macros FILES test_macros.cpp LINK_WITH pcl_gtest pcl_common)
 PCL_ADD_TEST(common_vector_average test_vector_average FILES test_vector_average.cpp LINK_WITH pcl_gtest)
 PCL_ADD_TEST(common_common test_common FILES test_common.cpp LINK_WITH pcl_gtest pcl_common)
+PCL_ADD_TEST(common_pointcloud test_pointcloud FILES test_pointcloud.cpp LINK_WITH pcl_gtest pcl_common)
 PCL_ADD_TEST(common_parse test_parse FILES test_parse.cpp LINK_WITH pcl_gtest pcl_common)
 PCL_ADD_TEST(common_geometry test_geometry FILES test_geometry.cpp LINK_WITH pcl_gtest pcl_common)
 PCL_ADD_TEST(common_copy_point test_copy_point FILES test_copy_point.cpp LINK_WITH pcl_gtest pcl_common)
@@ -34,6 +35,7 @@ PCL_ADD_TEST(common_copy_make_borders test_copy_make_borders FILES test_copy_mak
 PCL_ADD_TEST(common_bearing_angle_image test_bearing_angle_image FILES test_bearing_angle_image.cpp LINK_WITH pcl_gtest pcl_common)
 PCL_ADD_TEST(common_polygon_mesh test_polygon_mesh_concatenate FILES test_polygon_mesh.cpp LINK_WITH pcl_gtest pcl_common)
 PCL_ADD_TEST(common_point_type_conversion test_common_point_type_conversion FILES test_point_type_conversion.cpp LINK_WITH pcl_gtest pcl_common)
+PCL_ADD_TEST(common_point_type_static_member_functions test_common_point_type_static_member_functions FILES test_point_type_static_member_functions.cpp LINK_WITH pcl_gtest pcl_common)
 PCL_ADD_TEST(common_colors test_colors FILES test_colors.cpp LINK_WITH pcl_gtest pcl_common)
 PCL_ADD_TEST(common_type_traits test_type_traits FILES test_type_traits.cpp LINK_WITH pcl_gtest pcl_common)
 
diff --git a/test/common/test_centroid.cpp b/test/common/test_centroid.cpp
index 79b357ae..28c97600 100644
--- a/test/common/test_centroid.cpp
+++ b/test/common/test_centroid.cpp
@@ -37,7 +37,6 @@
  */
 
 #include <pcl/test/gtest.h>
-#include <pcl/common/eigen.h>
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
 #include <pcl/io/pcd_io.h>
diff --git a/test/common/test_common.cpp b/test/common/test_common.cpp
index 24554a2e..5829090b 100644
--- a/test/common/test_common.cpp
+++ b/test/common/test_common.cpp
@@ -37,6 +37,8 @@
  *
  */
 
+#include <Eigen/Eigenvalues> // for SelfAdjointEigenSolver
+
 #include <pcl/test/gtest.h>
 #include <pcl/pcl_tests.h>
 #include <pcl/common/common.h>
@@ -170,120 +172,6 @@ TEST (PCL, Eigen)
 
 }
 
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-TEST (PCL, PointCloud)
-{
-  PointCloud<PointXYZ> cloud;
-  cloud.width = 640;
-  cloud.height = 480;
-
-  EXPECT_TRUE (cloud.isOrganized ());
-
-  cloud.height = 1;
-  EXPECT_FALSE (cloud.isOrganized ());
-
-  cloud.width = 10;
-  for (std::uint32_t i = 0; i < cloud.width*cloud.height; ++i)
-  {
-    float j = static_cast<float> (i);
-    cloud.points.emplace_back(3.0f * j + 0.0f, 3.0f * j + 1.0f, 3.0f * j + 2.0f);
-  }
-
-  Eigen::MatrixXf mat_xyz1 = cloud.getMatrixXfMap ();
-  Eigen::MatrixXf mat_xyz = cloud.getMatrixXfMap (3, 4, 0);
-
-  if (Eigen::MatrixXf::Flags & Eigen::RowMajorBit)
-  {
-    EXPECT_EQ (mat_xyz1.cols (), 4);
-    EXPECT_EQ (mat_xyz1.rows (), cloud.width);
-    EXPECT_EQ (mat_xyz1 (0, 0), 0);
-    EXPECT_EQ (mat_xyz1 (cloud.width - 1, 2), 3 * cloud.width - 1);   // = 29
-
-    EXPECT_EQ (mat_xyz.cols (), 3);
-    EXPECT_EQ (mat_xyz.rows (), cloud.width);
-    EXPECT_EQ (mat_xyz (0, 0), 0);
-    EXPECT_EQ (mat_xyz (cloud.width - 1, 2), 3 * cloud.width - 1);    // = 29
-  }
-  else
-  {
-    EXPECT_EQ (mat_xyz1.cols (), cloud.width);
-    EXPECT_EQ (mat_xyz1.rows (), 4);
-    EXPECT_EQ (mat_xyz1 (0, 0), 0);
-    EXPECT_EQ (mat_xyz1 (2, cloud.width - 1), 3 * cloud.width - 1);   // = 29
-
-    EXPECT_EQ (mat_xyz.cols (), cloud.width);
-    EXPECT_EQ (mat_xyz.rows (), 3);
-    EXPECT_EQ (mat_xyz (0, 0), 0);
-    EXPECT_EQ (mat_xyz (2, cloud.width - 1), 3 * cloud.width - 1);    // = 29
-  }
-
-#ifdef NDEBUG
-  if (Eigen::MatrixXf::Flags & Eigen::RowMajorBit)
-  {
-    Eigen::MatrixXf mat_yz = cloud.getMatrixXfMap (2, 4, 1);
-    EXPECT_EQ (mat_yz.cols (), 2);
-    EXPECT_EQ (mat_yz.rows (), cloud.width);
-    EXPECT_EQ (mat_yz (0, 0), 1);
-    EXPECT_EQ (mat_yz (cloud.width - 1, 1), 3 * cloud.width - 1);
-    std::uint32_t j = 1;
-    for (std::uint32_t i = 1; i < cloud.width*cloud.height; i+=4, j+=3)
-    {
-      Eigen::MatrixXf mat_yz = cloud.getMatrixXfMap (2, 4, i);
-      EXPECT_EQ (mat_yz.cols (), 2);
-      EXPECT_EQ (mat_yz.rows (), cloud.width);
-      EXPECT_EQ (mat_yz (0, 0), j);
-    }
-  }
-  else
-  {
-    Eigen::MatrixXf mat_yz = cloud.getMatrixXfMap (2, 4, 1);
-    EXPECT_EQ (mat_yz.cols (), cloud.width);
-    EXPECT_EQ (mat_yz.rows (), 2);
-    EXPECT_EQ (mat_yz (0, 0), 1);
-    EXPECT_EQ (mat_yz (1, cloud.width - 1), 3 * cloud.width - 1);
-    std::uint32_t j = 1;
-    for (std::uint32_t i = 1; i < cloud.width*cloud.height; i+=4, j+=3)
-    {
-      Eigen::MatrixXf mat_yz = cloud.getMatrixXfMap (2, 4, i);
-      EXPECT_EQ (mat_yz.cols (), cloud.width);
-      EXPECT_EQ (mat_yz.rows (), 2);
-      EXPECT_EQ (mat_yz (0, 0), j);
-    }
-  }
-#endif
-  cloud.clear ();
-  EXPECT_EQ (cloud.width, 0);
-  EXPECT_EQ (cloud.height, 0);
-
-  cloud.width = 640;
-  cloud.height = 480;
-
-  cloud.insert (cloud.end (), PointXYZ (1, 1, 1));
-  EXPECT_FALSE (cloud.isOrganized ());
-  EXPECT_EQ (cloud.width, 1);
-
-  cloud.insert (cloud.end (), 5, PointXYZ (1, 1, 1));
-  EXPECT_FALSE (cloud.isOrganized ());
-  EXPECT_EQ (cloud.width, 6);
-
-  cloud.erase (cloud.end () - 1);
-  EXPECT_FALSE (cloud.isOrganized ());
-  EXPECT_EQ (cloud.width, 5);
-
-  cloud.erase (cloud.begin (), cloud.end ());
-  EXPECT_FALSE (cloud.isOrganized ());
-  EXPECT_EQ (cloud.width, 0);
-
-  cloud.emplace (cloud.end (), 1, 1, 1);
-  EXPECT_FALSE (cloud.isOrganized ());
-  EXPECT_EQ (cloud.width, 1);
-
-  auto& new_point = cloud.emplace_back (1, 1, 1);
-  EXPECT_FALSE (cloud.isOrganized ());
-  EXPECT_EQ (cloud.width, 2);
-  EXPECT_EQ (&new_point, &cloud.back ());
-}
-
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 TEST (PCL, PointTypes)
 {
@@ -529,6 +417,56 @@ TEST (PCL, HasField)
   EXPECT_FALSE ((pcl::traits::has_label<pcl::Normal>::value));
 }
 
+TEST (PCL, GetMinMax3D)
+{
+  pcl::PointCloud<pcl::PointXYZ> cloud;
+  cloud.emplace_back ( 0.0f,      0.0f,  0.0f);
+  cloud.emplace_back (10.0f, -10000.0f,  1.0f);
+  cloud.emplace_back ( 5.0f,      5.0f,  0.0f);
+  cloud.emplace_back (-5.0f,      0.0f, -0.5f);
+
+  pcl::PointXYZ min_pt, max_pt;
+  Eigen::Vector4f min_vec, max_vec;
+
+  pcl::getMinMax3D (cloud, min_pt, max_pt);
+  EXPECT_EQ (min_pt.x, -5.0f);
+  EXPECT_EQ (min_pt.y, -10000.0f);
+  EXPECT_EQ (min_pt.z, -0.5f);
+  EXPECT_EQ (max_pt.x, 10.0f);
+  EXPECT_EQ (max_pt.y, 5.0f);
+  EXPECT_EQ (max_pt.z, 1.0f);
+
+  pcl::getMinMax3D (cloud, min_vec, max_vec);
+  EXPECT_EQ (min_vec.x (), -5.0f);
+  EXPECT_EQ (min_vec.y (), -10000.0f);
+  EXPECT_EQ (min_vec.z (), -0.5f);
+  EXPECT_EQ (max_vec.x (), 10.0f);
+  EXPECT_EQ (max_vec.y (), 5.0f);
+  EXPECT_EQ (max_vec.z (), 1.0f);
+
+  pcl::PointIndices pindices;
+  pindices.indices.push_back (0);
+  pindices.indices.push_back (2);
+  pcl::getMinMax3D (cloud, pindices, min_vec, max_vec);
+  EXPECT_EQ (min_vec.x (), 0.0f);
+  EXPECT_EQ (min_vec.y (), 0.0f);
+  EXPECT_EQ (min_vec.z (), 0.0f);
+  EXPECT_EQ (max_vec.x (), 5.0f);
+  EXPECT_EQ (max_vec.y (), 5.0f);
+  EXPECT_EQ (max_vec.z (), 0.0f);
+
+  pcl::Indices indices;
+  indices.push_back (1);
+  indices.push_back (3);
+  pcl::getMinMax3D (cloud, indices, min_vec, max_vec);
+  EXPECT_EQ (min_vec.x (), -5.0f);
+  EXPECT_EQ (min_vec.y (), -10000.0f);
+  EXPECT_EQ (min_vec.z (), -0.5f);
+  EXPECT_EQ (max_vec.x (), 10.0f);
+  EXPECT_EQ (max_vec.y (), 0.0f);
+  EXPECT_EQ (max_vec.z (), 1.0f);
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 TEST (PCL, GetMaxDistance)
 {
@@ -537,7 +475,7 @@ TEST (PCL, GetMaxDistance)
   const Eigen::Vector4f pivot_pt (Eigen::Vector4f::Zero ());
 
   // populate cloud
-  cloud.points.resize (3);
+  cloud.resize (3);
   cloud[0].data[0] = 4.f; cloud[0].data[1] = 3.f;
   cloud[0].data[2] = 0.f; cloud[0].data[3] = 0.f;
   cloud[1].data[0] = 0.f; cloud[1].data[1] = 0.f;
@@ -558,6 +496,29 @@ TEST (PCL, GetMaxDistance)
   test::EXPECT_EQ_VECTORS (max_exp_pt, max_pt);
 }
 
+TEST (PCL, computeMedian)
+{
+  std::vector<float> vector1{4.0f, 2.0f, 1.0f, 5.0f, 3.0f, 6.0f};
+  const auto median1 = computeMedian (vector1.begin (), vector1.end ());
+  EXPECT_EQ(median1, 3.5f);
+
+  std::vector<double> vector2{1.0, 25.0, 9.0, 4.0, 16.0};
+  const auto median2 = computeMedian (vector2.begin (), vector2.end (), [](const double& x){ return std::sqrt(x); });
+  EXPECT_EQ(median2, 3.0);
+
+  std::vector<double> vector3{1.0, 2.0, 6.0, 5.0, 4.0, 3.0};
+  const auto median3 = computeMedian (vector3.begin (), vector3.end (), [](const double& x){ return x+1.0; });
+  EXPECT_EQ(median3, 4.5);
+
+  std::vector<int> vector4{-1, 1, 2, 9, 15, 16};
+  const auto median4 = computeMedian (vector4.begin (), vector4.end ());
+  EXPECT_EQ(median4, 5);
+
+  std::vector<int> vector5{-1, 1, 2, 9, 15, 16};
+  const auto median5 = computeMedian (vector5.begin (), vector5.end (), [](const int& x){ return static_cast<double>(x); });
+  EXPECT_EQ(median5, 5.5);
+}
+
 /* ---[ */
 int
 main (int argc, char** argv)
diff --git a/test/common/test_io.cpp b/test/common/test_io.cpp
index 781ca8a8..efdc8245 100644
--- a/test/common/test_io.cpp
+++ b/test/common/test_io.cpp
@@ -106,7 +106,7 @@ TEST (PCL, copyPointCloud)
     EXPECT_EQ (cloud_xyz_rgba[i].rgba, cloud_xyz_rgb_normal[i].rgba);
   }
 
-  IndicesAllocator< Eigen::aligned_allocator<int> > indices_aligned;
+  IndicesAllocator< Eigen::aligned_allocator<pcl::index_t> > indices_aligned;
   indices_aligned.push_back (1); indices_aligned.push_back (2); indices_aligned.push_back (3); 
   pcl::copyPointCloud (cloud_xyz_rgba, indices_aligned, cloud_xyz_rgb_normal);
   ASSERT_EQ (int (cloud_xyz_rgb_normal.size ()), 3);
@@ -128,203 +128,6 @@ 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)
-{
-  CloudXYZRGBA cloud_xyz_rgba;
-  cloud_xyz_rgba.push_back (pt_xyz_rgba);
-  cloud_xyz_rgba.push_back (pt_xyz_rgba);
-  cloud_xyz_rgba.push_back (pt_xyz_rgba);
-  cloud_xyz_rgba.push_back (pt_xyz_rgba);
-  cloud_xyz_rgba.push_back (pt_xyz_rgba);
-
-  CloudXYZRGBA cloud_xyz_rgba2;
-  cloud_xyz_rgba2.push_back (pt_xyz_rgba2);
-  cloud_xyz_rgba2.push_back (pt_xyz_rgba2);
-
-  pcl::PCLPointCloud2 cloud1, cloud2, cloud_out, cloud_out2, cloud_out3, cloud_out4;
-  pcl::toPCLPointCloud2 (cloud_xyz_rgba, cloud1);
-  pcl::toPCLPointCloud2 (cloud_xyz_rgba2, cloud2);
-
-  // Regular
-  pcl::concatenatePointCloud (cloud1, cloud2, cloud_out);
-  
-  CloudXYZRGBA cloud_all;
-  pcl::fromPCLPointCloud2 (cloud_out, cloud_all);
-
-  EXPECT_EQ (cloud_all.size (), cloud_xyz_rgba.size () + cloud_xyz_rgba2.size ());
-  for (int i = 0; i < int (cloud_xyz_rgba.size ()); ++i)
-  {
-    EXPECT_XYZ_EQ (cloud_all[i], cloud_xyz_rgba[i]);
-    EXPECT_RGB_EQ (cloud_all[i], cloud_xyz_rgba[i]);
-    EXPECT_EQ (cloud_all[i].rgba, cloud_xyz_rgba[i].rgba);
-  }
-  for (int i = 0; i < int (cloud_xyz_rgba2.size ()); ++i)
-  {
-    EXPECT_XYZ_EQ (cloud_all[cloud_xyz_rgba.size () + i], cloud_xyz_rgba2[i]);
-    EXPECT_RGB_EQ (cloud_all[cloud_xyz_rgba.size () + i], cloud_xyz_rgba2[i]);
-    EXPECT_EQ (cloud_all[cloud_xyz_rgba.size () + i].rgba, cloud_xyz_rgba2[i].rgba);
-  }
-
-  // RGB != RGBA
-  CloudXYZRGB cloud_xyz_rgb;
-  cloud_xyz_rgb.push_back (pt_xyz_rgb);
-  cloud_xyz_rgb.push_back (pt_xyz_rgb);
-
-  pcl::toPCLPointCloud2 (cloud_xyz_rgb, cloud2);
-  pcl::concatenatePointCloud (cloud1, cloud2, cloud_out2);
-  
-  pcl::fromPCLPointCloud2 (cloud_out2, cloud_all);
-
-  EXPECT_EQ (cloud_all.size (), cloud_xyz_rgba.size () + cloud_xyz_rgba2.size ());
-  for (int i = 0; i < int (cloud_xyz_rgba.size ()); ++i)
-  {
-    EXPECT_XYZ_EQ (cloud_all[i], cloud_xyz_rgba[i]);
-    EXPECT_RGB_EQ (cloud_all[i], cloud_xyz_rgba[i]);
-    EXPECT_EQ (cloud_all[i].rgba, cloud_xyz_rgba[i].rgba);
-  }
-  for (int i = 0; i < int (cloud_xyz_rgb.size ()); ++i)
-  {
-    EXPECT_XYZ_EQ (cloud_all[cloud_xyz_rgba.size () + i], cloud_xyz_rgb[i]);
-    EXPECT_RGB_EQ (cloud_all[cloud_xyz_rgba.size () + i], cloud_xyz_rgb[i]);
-    EXPECT_EQ (cloud_all[cloud_xyz_rgba.size () + i].rgba, cloud_xyz_rgb[i].rgba);
-  }
-
-  // _ vs regular
-  int rgb_idx = pcl::getFieldIndex (cloud1, "rgba");
-  cloud1.fields[rgb_idx].name = "_";
-  pcl::concatenatePointCloud (cloud1, cloud2, cloud_out3);
-  
-  pcl::fromPCLPointCloud2 (cloud_out3, cloud_all);
-
-  EXPECT_EQ (cloud_all.size (), cloud_xyz_rgba.size () + cloud_xyz_rgba2.size ());
-  for (int i = 0; i < int (cloud_xyz_rgba.size ()); ++i)
-  {
-    EXPECT_XYZ_EQ (cloud_all[i], cloud_xyz_rgba[i]);
-    // Data doesn't get modified
-    EXPECT_RGB_EQ (cloud_all[i], cloud_xyz_rgba[i]);
-    EXPECT_EQ (cloud_all[i].rgba, cloud_xyz_rgba[i].rgba);
-  }
-  for (int i = 0; i < int (cloud_xyz_rgb.size ()); ++i)
-    EXPECT_XYZ_EQ (cloud_all[cloud_xyz_rgba.size () + i], cloud_xyz_rgb[i]);
-
-  cloud1.fields[rgb_idx].name = "rgba";
-  // regular vs _
-  rgb_idx = pcl::getFieldIndex (cloud2, "rgb");
-  cloud2.fields[rgb_idx].name = "_";
-  pcl::concatenatePointCloud (cloud1, cloud2, cloud_out4);
-
-  pcl::fromPCLPointCloud2 (cloud_out4, cloud_all);
-
-  EXPECT_EQ (cloud_all.size (), cloud_xyz_rgba.size () + cloud_xyz_rgba2.size ());
-  for (int i = 0; i < int (cloud_xyz_rgba.size ()); ++i)
-  {
-    EXPECT_XYZ_EQ (cloud_all[i], cloud_xyz_rgba[i]);
-    // Data doesn't get modified
-    EXPECT_RGB_EQ (cloud_all[i], cloud_xyz_rgba[i]);
-    EXPECT_EQ (cloud_all[i].rgba, cloud_xyz_rgba[i].rgba);
-  }
-  for (int i = 0; i < int (cloud_xyz_rgb.size ()); ++i)
-  {
-    EXPECT_XYZ_EQ (cloud_all[cloud_xyz_rgba.size () + i], cloud_xyz_rgb[i]);
-    EXPECT_FLOAT_EQ (cloud_all[cloud_xyz_rgba.size () + i].r, 0);
-    EXPECT_FLOAT_EQ (cloud_all[cloud_xyz_rgba.size () + i].g, 0);
-    EXPECT_FLOAT_EQ (cloud_all[cloud_xyz_rgba.size () + i].b, 0);
-    EXPECT_EQ (cloud_all[cloud_xyz_rgba.size () + i].rgba, 0);
-  }
-
-  // _ vs _
-  rgb_idx = pcl::getFieldIndex (cloud1, "rgba");
-  cloud1.fields[rgb_idx].name = "_";
-  pcl::toPCLPointCloud2 (cloud_xyz_rgb, cloud2);
-  rgb_idx = pcl::getFieldIndex (cloud2, "rgb");
-  cloud2.fields[rgb_idx].name = "_";
-
-  pcl::concatenatePointCloud (cloud1, cloud2, cloud_out3);
-  
-  pcl::fromPCLPointCloud2 (cloud_out3, cloud_all);
-
-  EXPECT_EQ (cloud_all.size (), cloud_xyz_rgba.size () + cloud_xyz_rgba2.size ());
-  for (int i = 0; i < int (cloud_xyz_rgba.size ()); ++i)
-  {
-    EXPECT_XYZ_EQ (cloud_all[i], cloud_xyz_rgba[i]);
-    // Data doesn't get modified
-    EXPECT_RGB_EQ (cloud_all[i], cloud_xyz_rgba[i]);
-    EXPECT_EQ (cloud_all[i].rgba, cloud_xyz_rgba[i].rgba);
-  }
-  for (int i = 0; i < int (cloud_xyz_rgb.size ()); ++i)
-  {
-    EXPECT_XYZ_EQ (cloud_all[cloud_xyz_rgba.size () + i], cloud_xyz_rgb[i]);
-    EXPECT_FLOAT_EQ (cloud_all[cloud_xyz_rgba.size () + i].r, 0);
-    EXPECT_FLOAT_EQ (cloud_all[cloud_xyz_rgba.size () + i].g, 0);
-    EXPECT_FLOAT_EQ (cloud_all[cloud_xyz_rgba.size () + i].b, 0);
-    EXPECT_EQ (cloud_all[cloud_xyz_rgba.size () + i].rgba, 0);
-  }
-
-  cloud1.fields[rgb_idx].name = "rgba";
-  // _ vs regular
-  rgb_idx = pcl::getFieldIndex (cloud1, "rgba");
-
-  cloud1.fields[rgb_idx].name = "_";
-  pcl::toPCLPointCloud2 (cloud_xyz_rgb, cloud2);
-  pcl::concatenatePointCloud (cloud2, cloud1, cloud_out3);
-  
-  pcl::fromPCLPointCloud2 (cloud_out3, cloud_all);
-
-  EXPECT_EQ (cloud_all.size (), cloud_xyz_rgba.size () + cloud_xyz_rgba2.size ());
-  for (int i = 0; i < int (cloud_xyz_rgb.size ()); ++i)
-  {
-    EXPECT_XYZ_EQ (cloud_all[i], cloud_xyz_rgb[i]);
-    // Data doesn't get modified
-    EXPECT_RGB_EQ (cloud_all[i], cloud_xyz_rgb[i]);
-    EXPECT_EQ (cloud_all[i].rgba, cloud_xyz_rgb[i].rgba);
-  }
-  for (int i = 0; i < int (cloud_xyz_rgba.size ()); ++i)
-  {
-    EXPECT_XYZ_EQ (cloud_all[cloud_xyz_rgb.size () + i], cloud_xyz_rgba[i]);
-    EXPECT_FLOAT_EQ (cloud_all[cloud_xyz_rgb.size () + i].r, 0);
-    EXPECT_FLOAT_EQ (cloud_all[cloud_xyz_rgb.size () + i].g, 0);
-    EXPECT_FLOAT_EQ (cloud_all[cloud_xyz_rgb.size () + i].b, 0);
-    EXPECT_EQ (cloud_all[cloud_xyz_rgb.size () + i].rgba, 0);
-  }
-
-  cloud1.fields[rgb_idx].name = "rgba";
-  // regular vs _
-  rgb_idx = pcl::getFieldIndex (cloud2, "rgb");
-  cloud2.fields[rgb_idx].name = "_";
-  pcl::concatenatePointCloud (cloud2, cloud1, cloud_out4);
-
-  pcl::fromPCLPointCloud2 (cloud_out4, cloud_all);
-
-  EXPECT_EQ (cloud_all.size (), cloud_xyz_rgba.size () + cloud_xyz_rgba2.size ());
-  for (int i = 0; i < int (cloud_xyz_rgb.size ()); ++i)
-  {
-    EXPECT_XYZ_EQ (cloud_all[i], cloud_xyz_rgb[i]);
-    // Data doesn't get modified
-    EXPECT_RGB_EQ (cloud_all[i], cloud_xyz_rgb[i]);
-    EXPECT_EQ (cloud_all[i].rgba, cloud_xyz_rgb[i].rgba);
-  }
-  for (int i = 0; i < int (cloud_xyz_rgba.size ()); ++i)
-  {
-    EXPECT_XYZ_EQ (cloud_all[cloud_xyz_rgb.size () + i], cloud_xyz_rgba[i]);
-    EXPECT_FLOAT_EQ (cloud_all[cloud_xyz_rgb.size () + i].r, 0);
-    EXPECT_FLOAT_EQ (cloud_all[cloud_xyz_rgb.size () + i].g, 0);
-    EXPECT_FLOAT_EQ (cloud_all[cloud_xyz_rgb.size () + i].b, 0);
-    EXPECT_EQ (cloud_all[cloud_xyz_rgb.size () + i].rgba, 0);
-  }
-}
-#pragma GCC diagnostic pop
-#ifdef _MSC_VER
-#pragma warning(pop)
-#endif
-
 ///////////////////////////////////////////////////////////////////////////////////////////
 TEST (PCL, concatenatePointCloud2)
 {
diff --git a/test/common/test_plane_intersection.cpp b/test/common/test_plane_intersection.cpp
index 06db46ac..cb0ee8c5 100644
--- a/test/common/test_plane_intersection.cpp
+++ b/test/common/test_plane_intersection.cpp
@@ -35,7 +35,6 @@
  */
 
 #include <pcl/test/gtest.h>
-#include <pcl/common/common.h>
 #include <pcl/common/intersections.h>
 #include <pcl/pcl_tests.h>
 
diff --git a/test/common/test_point_type_static_member_functions.cpp b/test/common/test_point_type_static_member_functions.cpp
new file mode 100644
index 00000000..36142495
--- /dev/null
+++ b/test/common/test_point_type_static_member_functions.cpp
@@ -0,0 +1,36 @@
+/*
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2014-, Open Perception Inc.
+ *
+ *  All rights reserved
+ */
+
+#include <pcl/test/gtest.h>
+#include <pcl/pcl_macros.h>
+#include <pcl/pcl_tests.h>
+#include <pcl/point_types.h>
+#include <pcl/type_traits.h>
+
+using namespace pcl;
+using namespace pcl::test;
+
+template <typename T> class PointTypeStaticMemberFunctionsTest : public testing::Test { };
+using FeaturePointTypes = ::testing::Types<BOOST_PP_SEQ_ENUM (PCL_DESCRIPTOR_FEATURE_POINT_TYPES), Histogram<1>>;
+TYPED_TEST_SUITE (PointTypeStaticMemberFunctionsTest, FeaturePointTypes);
+TYPED_TEST (PointTypeStaticMemberFunctionsTest, DescriptorSizeTests)
+{
+  static_assert
+  (
+    TypeParam::descriptorSize() == pcl::detail::traits::descriptorSize_v<TypeParam>,
+    "incorrect descriptorSize"
+  );
+}
+
+int
+main (int argc, char** argv)
+{
+  testing::InitGoogleTest (&argc, argv);
+  return (RUN_ALL_TESTS ());
+}
diff --git a/test/common/test_pointcloud.cpp b/test/common/test_pointcloud.cpp
new file mode 100644
index 00000000..2e43620b
--- /dev/null
+++ b/test/common/test_pointcloud.cpp
@@ -0,0 +1,405 @@
+/*
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2021-, Open Perception
+ *
+ *  All rights reserved
+ */
+
+#include <pcl/test/gtest.h>
+#include <pcl/pcl_tests.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+
+using namespace pcl;
+
+//////////////////////////////////////////////
+struct pointCloudTest : public testing::Test {
+  protected:
+    PointCloud<PointXYZ> cloud;
+};
+
+TEST_F (pointCloudTest, is_organized)
+{
+  cloud.width = 640;
+  cloud.height = 480;
+  EXPECT_TRUE (cloud.isOrganized ());
+}
+
+TEST_F (pointCloudTest, not_organized)
+{
+  cloud.width = 640;
+  cloud.height = 1;
+  EXPECT_FALSE (cloud.isOrganized ());
+}
+
+TEST_F (pointCloudTest, getMatrixXfMap)
+{
+  cloud.height = 1;
+  cloud.width = 10;
+  for (std::uint32_t i = 0; i < cloud.width*cloud.height; ++i)
+  {
+    float j = static_cast<float> (i);
+    cloud.emplace_back(3.0f * j + 0.0f, 3.0f * j + 1.0f, 3.0f * j + 2.0f);
+  }
+
+  Eigen::MatrixXf mat_xyz1 = cloud.getMatrixXfMap ();
+  Eigen::MatrixXf mat_xyz = cloud.getMatrixXfMap (3, 4, 0);
+
+  if (Eigen::MatrixXf::Flags & Eigen::RowMajorBit)
+  {
+    EXPECT_EQ (mat_xyz1.cols (), 4);
+    EXPECT_EQ (mat_xyz1.rows (), cloud.width);
+    EXPECT_EQ (mat_xyz1 (0, 0), 0);
+    EXPECT_EQ (mat_xyz1 (cloud.width - 1, 2), 3 * cloud.width - 1);   // = 29
+
+    EXPECT_EQ (mat_xyz.cols (), 3);
+    EXPECT_EQ (mat_xyz.rows (), cloud.width);
+    EXPECT_EQ (mat_xyz (0, 0), 0);
+    EXPECT_EQ (mat_xyz (cloud.width - 1, 2), 3 * cloud.width - 1);    // = 29
+  }
+  else
+  {
+    EXPECT_EQ (mat_xyz1.cols (), cloud.width);
+    EXPECT_EQ (mat_xyz1.rows (), 4);
+    EXPECT_EQ (mat_xyz1 (0, 0), 0);
+    EXPECT_EQ (mat_xyz1 (2, cloud.width - 1), 3 * cloud.width - 1);   // = 29
+
+    EXPECT_EQ (mat_xyz.cols (), cloud.width);
+    EXPECT_EQ (mat_xyz.rows (), 3);
+    EXPECT_EQ (mat_xyz (0, 0), 0);
+    EXPECT_EQ (mat_xyz (2, cloud.width - 1), 3 * cloud.width - 1);    // = 29
+  }
+
+#ifdef NDEBUG
+  if (Eigen::MatrixXf::Flags & Eigen::RowMajorBit)
+  {
+    Eigen::MatrixXf mat_yz = cloud.getMatrixXfMap (2, 4, 1);
+    EXPECT_EQ (mat_yz.cols (), 2);
+    EXPECT_EQ (mat_yz.rows (), cloud.width);
+    EXPECT_EQ (mat_yz (0, 0), 1);
+    EXPECT_EQ (mat_yz (cloud.width - 1, 1), 3 * cloud.width - 1);
+    std::uint32_t j = 1;
+    for (std::uint32_t i = 1; i < cloud.width*cloud.height; i+=4, j+=3)
+    {
+      Eigen::MatrixXf mat_yz = cloud.getMatrixXfMap (2, 4, i);
+      EXPECT_EQ (mat_yz.cols (), 2);
+      EXPECT_EQ (mat_yz.rows (), cloud.width);
+      EXPECT_EQ (mat_yz (0, 0), j);
+    }
+  }
+  else
+  {
+    Eigen::MatrixXf mat_yz = cloud.getMatrixXfMap (2, 4, 1);
+    EXPECT_EQ (mat_yz.cols (), cloud.width);
+    EXPECT_EQ (mat_yz.rows (), 2);
+    EXPECT_EQ (mat_yz (0, 0), 1);
+    EXPECT_EQ (mat_yz (1, cloud.width - 1), 3 * cloud.width - 1);
+    std::uint32_t j = 1;
+    for (std::uint32_t i = 1; i < cloud.width*cloud.height; i+=4, j+=3)
+    {
+      Eigen::MatrixXf mat_yz = cloud.getMatrixXfMap (2, 4, i);
+      EXPECT_EQ (mat_yz.cols (), cloud.width);
+      EXPECT_EQ (mat_yz.rows (), 2);
+      EXPECT_EQ (mat_yz (0, 0), j);
+    }
+  }
+#endif
+}
+
+TEST_F (pointCloudTest, clear)
+{
+  cloud.insert (cloud.end (), PointXYZ (1, 1, 1));
+  EXPECT_EQ (cloud.size(), 1);
+  cloud.clear ();
+  EXPECT_EQ (cloud.width, 0);
+  EXPECT_EQ (cloud.height, 0);
+}
+
+TEST_F (pointCloudTest, insert)
+{
+  cloud.insert (cloud.end (), PointXYZ (1, 1, 1));
+  EXPECT_FALSE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.width, 1);
+}
+
+TEST_F (pointCloudTest, insert_with_height)
+{
+  cloud.insert (cloud.end (), 5, PointXYZ (1, 1, 1));
+  EXPECT_FALSE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.width, 5);
+}
+
+TEST_F (pointCloudTest, erase_at_position)
+{
+  cloud.insert (cloud.end (), 5, PointXYZ (1, 1, 1));
+  cloud.erase (cloud.end () - 1);
+  EXPECT_FALSE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.width, 4);
+}
+
+TEST_F (pointCloudTest, erase_with_iterator)
+{
+  cloud.insert (cloud.end (), 5, PointXYZ (1, 1, 1));
+  cloud.erase (cloud.begin (), cloud.end ());
+  EXPECT_FALSE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.width, 0);
+}
+
+TEST_F (pointCloudTest, emplace)
+{
+  cloud.emplace (cloud.end (), 1, 1, 1);
+  EXPECT_FALSE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.width, 1);
+}
+
+TEST_F (pointCloudTest, emplace_back)
+{
+  auto& new_point = cloud.emplace_back (1, 1, 1);
+  EXPECT_FALSE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.width, 1);
+  EXPECT_EQ (&new_point, &cloud.back ());
+}
+
+TEST_F (pointCloudTest, resize_with_count_elements)
+{
+  cloud.resize (640*360);
+  EXPECT_FALSE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.width, 640*360);
+}
+
+TEST_F (pointCloudTest, resize_with_new_width_and_height)
+{
+  cloud.resize (640, 480);
+  EXPECT_TRUE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.width, 640);
+  EXPECT_EQ (cloud.height, 480);
+}
+
+TEST_F (pointCloudTest, resize_with_initialized_count_elements)
+{
+  cloud.resize (640*360, PointXYZ (1, 1, 1));
+  EXPECT_FALSE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.width, 640*360);
+}
+
+TEST_F (pointCloudTest, resize_with_initialized_count_and_new_width_and_height)
+{
+  cloud.resize (640, 480, PointXYZ (1, 1, 1));
+  EXPECT_TRUE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.width, 640);
+}
+
+TEST_F (pointCloudTest, assign_with_copies)
+{
+  cloud.assign (640*360, PointXYZ (1, 1, 1));
+  EXPECT_FALSE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.width, 640*360);
+}
+
+TEST_F (pointCloudTest, assign_with_new_width_and_height_copies)
+{
+  cloud.assign(640, 480, PointXYZ (1, 1, 1));
+  EXPECT_TRUE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.width, 640);
+}
+
+TEST_F (pointCloudTest, assign_with_copies_in_range)
+{
+  std::vector<PointXYZ> pointVec;
+  pointVec.resize (640*360, PointXYZ (2, 3, 4));
+  cloud.assign (pointVec.begin(), pointVec.end());
+  EXPECT_FALSE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.width, 640*360);
+}
+
+TEST_F (pointCloudTest, assign_with_copies_in_range_and_new_width)
+{
+  std::vector<PointXYZ> pointVec;
+  pointVec.resize (640*360, PointXYZ (2, 3, 4));
+  cloud.assign (pointVec.begin(), pointVec.end(), 640);
+  EXPECT_TRUE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.width, 640);
+}
+
+TEST_F (pointCloudTest, assign_mismatch_size_and_width_height)
+{
+  std::vector<PointXYZ> pointVec;
+  pointVec.resize (640*480, PointXYZ (7, 7, 7));
+  cloud.assign (pointVec.begin(), pointVec.end(), 460);
+  EXPECT_FALSE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.width, 640*480);
+}
+
+TEST_F (pointCloudTest, assign_initializer_list)
+{
+  cloud.assign ({PointXYZ (3, 4, 5), PointXYZ (3, 4, 5), PointXYZ (3, 4, 5)});
+  EXPECT_FALSE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.width, 3);
+}
+
+TEST_F (pointCloudTest, assign_initializer_list_with_new_width)
+{
+  cloud.assign ({PointXYZ (3, 4, 5), PointXYZ (3, 4, 5), PointXYZ (3, 4, 5), PointXYZ (3, 4, 5)}, 2);
+  EXPECT_TRUE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.width, 2);
+}
+
+TEST_F (pointCloudTest, assign_initializer_list_with_unorganized_cloud)
+{
+  cloud.assign ({PointXYZ (3, 4, 5), PointXYZ (3, 4, 5), PointXYZ (3, 4, 5)}, 6);
+  EXPECT_FALSE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.width, 3);
+}
+
+TEST_F (pointCloudTest, push_back_to_unorganized_cloud)
+{
+  cloud.push_back (PointXYZ (3, 4, 5));
+  EXPECT_FALSE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.width, 1);
+}
+
+TEST_F (pointCloudTest, push_back_to_organized_cloud)
+{
+  cloud.resize (80, 80, PointXYZ (1, 1, 1));
+  EXPECT_TRUE (cloud.isOrganized ());
+  cloud.push_back (PointXYZ (3, 4, 5));
+  EXPECT_EQ (cloud.width, (80*80) + 1);
+}
+
+/////////////////////////////////////////////////
+struct organizedPointCloudTest : public pointCloudTest {
+  protected:
+    void SetUp() override
+    {
+      cloud.resize (640, 480, PointXYZ (1, 1, 1));
+    }
+};
+
+TEST_F (organizedPointCloudTest, transient_push_back)
+{
+  cloud.transient_push_back (PointXYZ(2, 2, 2));
+  EXPECT_TRUE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.width, 640);
+  EXPECT_EQ (cloud.size(), (640*480) + 1);
+}
+
+TEST_F (organizedPointCloudTest, transient_emplace_back)
+{
+  auto& new_pointXYZ = cloud.transient_emplace_back (3, 3, 3);
+  EXPECT_TRUE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.width, 640);
+  EXPECT_EQ (cloud.size(), (640*480) + 1);
+  EXPECT_EQ (&new_pointXYZ, &cloud.back ());
+}
+
+TEST_F (organizedPointCloudTest, transient_insert_one_element)
+{
+  cloud.transient_insert (cloud.end (), PointXYZ (1, 1, 1));
+  EXPECT_TRUE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.size(), (640*480) + 1);
+  EXPECT_EQ (cloud.width, 640);
+}
+
+TEST_F (organizedPointCloudTest, transient_insert_with_n_elements)
+{
+  cloud.transient_insert (cloud.end (), 10, PointXYZ (1, 1, 1));
+  EXPECT_TRUE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.size(), (640*480) + 10);
+  EXPECT_EQ (cloud.width, 640);
+}
+
+TEST_F (organizedPointCloudTest, transient_emplace)
+{
+  cloud.transient_emplace (cloud.end (), 4, 4, 4);
+  EXPECT_TRUE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.width, 640);
+  EXPECT_EQ (cloud.size(), (640*480) + 1);
+}
+
+TEST_F (organizedPointCloudTest, transient_erase_at_position)
+{
+  cloud.transient_erase (cloud.end () - 1);
+  EXPECT_TRUE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.width, 640);
+  EXPECT_EQ (cloud.size(), (640*480) - 1);
+}
+
+TEST_F (organizedPointCloudTest, transient_erase_with_iterator)
+{
+  cloud.transient_erase (cloud.begin (), cloud.end ());
+  EXPECT_TRUE (cloud.isOrganized ());
+  EXPECT_EQ (cloud.width, 640);
+  EXPECT_EQ (cloud.size(), 0);
+}
+
+TEST_F (organizedPointCloudTest, unorganized_concatenate)
+{
+  PointCloud<PointXYZ> new_unorganized_cloud;
+  PointCloud<PointXYZ>::concatenate (new_unorganized_cloud, cloud);
+  EXPECT_FALSE (new_unorganized_cloud.isOrganized ());
+  EXPECT_EQ (new_unorganized_cloud.width, 640*480);
+}
+
+TEST_F (organizedPointCloudTest, unorganized_concatenate_with_argument_return)
+{
+  PointCloud<PointXYZ> new_unorganized_cloud;
+  PointCloud<PointXYZ>::concatenate (new_unorganized_cloud, cloud);
+  PointCloud<PointXYZ> unorganized_cloud_out;
+  PointCloud<PointXYZ>::concatenate (new_unorganized_cloud, cloud, unorganized_cloud_out);
+  EXPECT_FALSE (unorganized_cloud_out.isOrganized ());
+  EXPECT_EQ (unorganized_cloud_out.width, 640*480*2);
+}
+
+TEST_F (organizedPointCloudTest, unorganized_concatenate_with_assignment_return)
+{
+  PointCloud<PointXYZ> unorganized_cloud;
+  PointCloud<PointXYZ>::concatenate (unorganized_cloud, cloud);
+  PointCloud<PointXYZ> unorganized_cloud_out = cloud + unorganized_cloud;
+  EXPECT_FALSE (unorganized_cloud_out.isOrganized ());
+  EXPECT_EQ (unorganized_cloud_out.width, 640*480*2);
+}
+
+TEST_F (organizedPointCloudTest, unorganized_concatenate_with_plus_operator)
+{
+  PointCloud<PointXYZ> unorganized_cloud;
+  unorganized_cloud += cloud;
+  EXPECT_FALSE (unorganized_cloud.isOrganized ());
+  EXPECT_EQ (unorganized_cloud.width, 640*480);
+}
+
+TEST_F (organizedPointCloudTest, at_with_throw)
+{
+  PointCloud<PointXYZ> unorganized_cloud;
+  unorganized_cloud += cloud;
+  EXPECT_THROW({unorganized_cloud.at (5, 5);}, UnorganizedPointCloudException);
+}
+
+TEST_F (organizedPointCloudTest, at_no_throw)
+{
+  const auto& point_at = cloud.at (cloud.width - 1, cloud.height - 1);
+  EXPECT_EQ(&point_at, &cloud.back());
+}
+
+TEST_F (organizedPointCloudTest, organized_concatenate)
+{
+  PointCloud<PointXYZ> organized_cloud1 = cloud;
+  PointCloud<PointXYZ> organized_cloud2 = cloud;
+  EXPECT_TRUE (organized_cloud1.isOrganized ());
+  EXPECT_TRUE (organized_cloud2.isOrganized ());
+  PointCloud<PointXYZ> organized_cloud_out = organized_cloud1 + organized_cloud2;
+  std::size_t total_size = organized_cloud1.size() + organized_cloud2.size();
+  EXPECT_FALSE (organized_cloud_out.isOrganized ());
+  EXPECT_EQ(total_size, 614400);
+  EXPECT_EQ (organized_cloud_out.width, total_size);
+}
+
+/* ---[ */
+int
+main (int argc, char** argv)
+{
+  testing::InitGoogleTest (&argc, argv);
+  return (RUN_ALL_TESTS ());
+}
+/* ]--- */
diff --git a/test/common/test_polygon_mesh.cpp b/test/common/test_polygon_mesh.cpp
index 6c0d56e8..d03038ab 100644
--- a/test/common/test_polygon_mesh.cpp
+++ b/test/common/test_polygon_mesh.cpp
@@ -72,7 +72,7 @@ TEST(PolygonMesh, concatenate_cloud)
     cloud_template.height = 480;
     for (std::uint32_t i = 0; i < size; ++i)
     {
-        cloud_template.points.emplace_back(3.0f * static_cast<float>(i) + 0, 
+        cloud_template.emplace_back(3.0f * static_cast<float>(i) + 0, 
                                            3.0f * static_cast<float> (i) + 1,
                                            3.0f * static_cast<float> (i) + 2);
     }
@@ -91,11 +91,13 @@ TEST(PolygonMesh, concatenate_cloud)
 
 TEST(PolygonMesh, concatenate_vertices)
 {
+    const std::size_t size = 15;
+
     PolygonMesh test, dummy;
-    test.cloud.width = 10;
-    test.cloud.height = 5;
+    // The algorithm works regardless of the organization.
+    test.cloud.width = dummy.cloud.width = size;
+    test.cloud.height = dummy.cloud.height = 1;
 
-    const std::size_t size = 15;
     for (std::size_t i = 0; i < size; ++i)
     {
         dummy.polygons.emplace_back();
@@ -111,18 +113,16 @@ TEST(PolygonMesh, concatenate_vertices)
     EXPECT_EQ(2 * dummy.polygons.size(), test.polygons.size());
 
     const auto cloud_size = test.cloud.width * test.cloud.height;
-    for (std::size_t i = 0; i < dummy.polygons.size(); ++i)
-    {
-        EXPECT_EQ(dummy.polygons[i].vertices.size(), test.polygons[i].vertices.size());
-        EXPECT_EQ(dummy.polygons[i].vertices.size(),
-                  test.polygons[i + dummy.polygons.size()].vertices.size());
-        for (std::size_t j = 0; j < size; ++j)
-        {
-            EXPECT_EQ(dummy.polygons[i].vertices[j],
-                      test.polygons[i].vertices[j]);
-            EXPECT_EQ(dummy.polygons[i].vertices[j] + cloud_size,
-                      test.polygons[i + dummy.polygons.size()].vertices[j]);
-        }
+    for (const auto& polygon : test.polygons)
+      for (const auto& vertex : polygon.vertices)
+        EXPECT_LT(vertex, cloud_size);
+
+    pcl::Indices vertices(size);
+    for (std::size_t i = 0; i < size; ++i) {
+        vertices = dummy.polygons[i].vertices;
+        EXPECT_EQ_VECTORS(vertices, test.polygons[i].vertices);
+        for (auto& vertex : vertices) { vertex += size; }
+        EXPECT_EQ_VECTORS(vertices, test.polygons[i + size].vertices);
     }
 }
 
diff --git a/test/common/test_transforms.cpp b/test/common/test_transforms.cpp
index ff893ab5..2c2a23a3 100644
--- a/test/common/test_transforms.cpp
+++ b/test/common/test_transforms.cpp
@@ -279,6 +279,34 @@ TEST (PCL, Matrix4Affine3Transform)
   EXPECT_NEAR (pt.z, ct[0].z, 1e-4);
 }
 
+TEST (PCL, OrganizedTransform)
+{
+  const Eigen::Matrix4f transform=Eigen::Matrix4f::Identity();
+  // test if organized point cloud is still organized after transformPointCloud
+  pcl::PointCloud<PointXYZ> cloud_a, cloud_b, cloud_c;
+  cloud_a.resize (12);
+  cloud_a.width=4;
+  cloud_a.height=3;
+  pcl::transformPointCloud (cloud_a, cloud_b, transform, true);
+  EXPECT_EQ (cloud_a.width , cloud_b.width );
+  EXPECT_EQ (cloud_a.height, cloud_b.height);
+  pcl::transformPointCloud (cloud_a, cloud_c, transform, false);
+  EXPECT_EQ (cloud_a.width , cloud_c.width );
+  EXPECT_EQ (cloud_a.height, cloud_c.height);
+
+  // test if organized point cloud is still organized after transformPointCloudWithNormals
+  pcl::PointCloud<PointNormal> cloud_d, cloud_e, cloud_f;
+  cloud_d.resize (10);
+  cloud_d.width=2;
+  cloud_d.height=5;
+  pcl::transformPointCloudWithNormals (cloud_d, cloud_e, transform, true);
+  EXPECT_EQ (cloud_d.width , cloud_e.width );
+  EXPECT_EQ (cloud_d.height, cloud_e.height);
+  pcl::transformPointCloudWithNormals (cloud_d, cloud_f, transform, false);
+  EXPECT_EQ (cloud_d.width , cloud_f.width );
+  EXPECT_EQ (cloud_d.height, cloud_f.height);
+}
+
 /* ---[ */
 int
 main (int argc, char** argv)
diff --git a/test/common/test_wrappers.cpp b/test/common/test_wrappers.cpp
index 0d96a431..76a2a7ff 100644
--- a/test/common/test_wrappers.cpp
+++ b/test/common/test_wrappers.cpp
@@ -50,7 +50,7 @@ const std::size_t size = 10 * 480;
 
 TEST (PointCloud, size)
 {
-  EXPECT_EQ(cloud.points.size (), cloud.size ());
+  EXPECT_EQ(cloud.size (), cloud.size ());
 }
 
 TEST (PointCloud, sq_brackets_wrapper)
@@ -63,19 +63,19 @@ TEST (PointCloud, sq_brackets_wrapper)
 TEST (PointCloud, at)
 {
   for (std::uint32_t i = 0; i < size; ++i)
-    EXPECT_EQ_VECTORS (cloud.points.at (i).getVector3fMap (),
+    EXPECT_EQ_VECTORS (cloud.at (i).getVector3fMap (),
                        cloud.at (i).getVector3fMap ());
 }
 
 TEST (PointCloud, front)
 {
-  EXPECT_EQ_VECTORS (cloud.points.front ().getVector3fMap (),
+  EXPECT_EQ_VECTORS (cloud.front ().getVector3fMap (),
                      cloud.front ().getVector3fMap ());
 }
 
 TEST (PointCloud, back)
 {
-  EXPECT_EQ_VECTORS (cloud.points.back ().getVector3fMap (),
+  EXPECT_EQ_VECTORS (cloud.back ().getVector3fMap (),
                      cloud.back ().getVector3fMap ());
 }
 
@@ -105,11 +105,11 @@ TEST (PointCloud, constructor_with_allocation_valued)
 TEST (PointCloud, iterators)
 {
   EXPECT_EQ_VECTORS (cloud.begin ()->getVector3fMap (),
-                     cloud.points.begin ()->getVector3fMap ());
+                     cloud.begin ()->getVector3fMap ());
   EXPECT_EQ_VECTORS ((--cloud.end ())->getVector3fMap (),
-                     (--cloud.points.end ())->getVector3fMap ());
+                     (--cloud.end ())->getVector3fMap ());
   PointCloud<PointXYZ>::const_iterator pit = cloud.begin ();
-  PointCloud<PointXYZ>::VectorType::const_iterator pit2 = cloud.points.begin ();
+  PointCloud<PointXYZ>::VectorType::const_iterator pit2 = cloud.begin ();
   for (; pit < cloud.end (); ++pit2, ++pit)
     EXPECT_EQ_VECTORS (pit->getVector3fMap (), pit2->getVector3fMap ());
 }
@@ -137,7 +137,7 @@ main (int argc, char** argv)
   cloud.width = 10;
   cloud.height = 480;
   for (std::uint32_t i = 0; i < size; ++i)
-    cloud.points.emplace_back(3.0f * static_cast<float>(i) + 0, 3.0f * static_cast<float> (i) + 1, 3.0f * static_cast<float> (i) + 2);
+    cloud.emplace_back(3.0f * static_cast<float>(i) + 0, 3.0f * static_cast<float> (i) + 1, 3.0f * static_cast<float> (i) + 2);
 
   testing::InitGoogleTest (&argc, argv);
   return (RUN_ALL_TESTS ());
diff --git a/test/features/test_base_feature.cpp b/test/features/test_base_feature.cpp
index a81c9dee..426a7803 100644
--- a/test/features/test_base_feature.cpp
+++ b/test/features/test_base_feature.cpp
@@ -49,7 +49,7 @@ using namespace pcl::io;
 using KdTreePtr = search::KdTree<PointXYZ>::Ptr;
 
 PointCloud<PointXYZ> cloud;
-std::vector<int> indices;
+pcl::Indices indices;
 KdTreePtr tree;
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/test/features/test_board_estimation.cpp b/test/features/test_board_estimation.cpp
index ffed2a5e..cd4243c0 100644
--- a/test/features/test_board_estimation.cpp
+++ b/test/features/test_board_estimation.cpp
@@ -51,7 +51,7 @@ using namespace pcl::io;
 using KdTreePtr = search::KdTree<PointXYZ>::Ptr;
 
 PointCloud<PointXYZ> cloud;
-std::vector<int> indices;
+pcl::Indices indices;
 KdTreePtr tree;
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/test/features/test_boundary_estimation.cpp b/test/features/test_boundary_estimation.cpp
index 2d3aa0fd..ea23214a 100644
--- a/test/features/test_boundary_estimation.cpp
+++ b/test/features/test_boundary_estimation.cpp
@@ -49,7 +49,7 @@ using namespace pcl::io;
 using KdTreePtr = search::KdTree<PointXYZ>::Ptr;
 
 PointCloud<PointXYZ> cloud;
-std::vector<int> indices;
+pcl::Indices indices;
 KdTreePtr tree;
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/test/features/test_curvatures_estimation.cpp b/test/features/test_curvatures_estimation.cpp
index f08b5faa..11e858f3 100644
--- a/test/features/test_curvatures_estimation.cpp
+++ b/test/features/test_curvatures_estimation.cpp
@@ -49,7 +49,7 @@ using namespace pcl::io;
 using KdTreePtr = search::KdTree<PointXYZ>::Ptr;
 
 PointCloud<PointXYZ> cloud;
-std::vector<int> indices;
+pcl::Indices indices;
 KdTreePtr tree;
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/test/features/test_cvfh_estimation.cpp b/test/features/test_cvfh_estimation.cpp
index 6c7e78dd..d5955e48 100644
--- a/test/features/test_cvfh_estimation.cpp
+++ b/test/features/test_cvfh_estimation.cpp
@@ -51,7 +51,7 @@ using KdTreePtr = search::KdTree<PointXYZ>::Ptr;
 using CloudPtr = PointCloud<PointXYZ>::Ptr;
 
 PointCloud<PointXYZ> cloud;
-std::vector<int> indices;
+pcl::Indices indices;
 KdTreePtr tree;
 
 CloudPtr cloud_milk;
diff --git a/test/features/test_flare_estimation.cpp b/test/features/test_flare_estimation.cpp
index e9cc6b89..7e9746ca 100644
--- a/test/features/test_flare_estimation.cpp
+++ b/test/features/test_flare_estimation.cpp
@@ -164,7 +164,7 @@ main (int argc, char** argv)
 
   sampled_cloud.reset (new pcl::PointCloud<pcl::PointXYZ> ());
 
-  std::vector<int> sampled_indices;
+  pcl::Indices sampled_indices;
   for (float sa = 0.0f; sa < (float)cloud->size (); sa += sampling_incr)
     sampled_indices.push_back (static_cast<int> (sa));
   copyPointCloud (*cloud, sampled_indices, *sampled_cloud);
diff --git a/test/features/test_gradient_estimation.cpp b/test/features/test_gradient_estimation.cpp
index b56a021e..7127c13b 100644
--- a/test/features/test_gradient_estimation.cpp
+++ b/test/features/test_gradient_estimation.cpp
@@ -43,7 +43,6 @@
 #include <pcl/features/intensity_gradient.h>
 
 using namespace pcl;
-using namespace pcl::io;
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 TEST (PCL, IntensityGradientEstimation)
@@ -62,7 +61,7 @@ TEST (PCL, IntensityGradientEstimation)
       p.z = 0.1f * powf (x, 2.0f) + 0.5f * y + 1.0f;
       p.intensity = 0.1f * powf (x, 3.0f) + 0.2f * powf (y, 2.0f) + 1.0f * p.z + 20000.0f;
 
-      cloud_xyzi.points.push_back (p);
+      cloud_xyzi.push_back (p);
     }
   }
   cloud_xyzi.width = cloud_xyzi.size ();
diff --git a/test/features/test_grsd_estimation.cpp b/test/features/test_grsd_estimation.cpp
index 438cdd2b..0e3ac29f 100644
--- a/test/features/test_grsd_estimation.cpp
+++ b/test/features/test_grsd_estimation.cpp
@@ -49,7 +49,6 @@ using namespace pcl::io;
 
 search::KdTree<PointXYZ>::Ptr tree (new search::KdTree<PointXYZ> ());
 PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ> ());
-std::vector<int> indices;
 
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 TEST (PCL, GRSDEstimation)
diff --git a/test/features/test_ii_normals.cpp b/test/features/test_ii_normals.cpp
index 1008d54a..220f7792 100644
--- a/test/features/test_ii_normals.cpp
+++ b/test/features/test_ii_normals.cpp
@@ -494,7 +494,7 @@ TEST (PCL, IINormalEstimationSimple3DGradientUnorganized)
 {
   PointCloud<Normal> output;
   cloud.height = 1;
-  cloud.points.resize (cloud.height * cloud.width);
+  cloud.resize (cloud.height * cloud.width);
   ne.setInputCloud (cloud.makeShared ());
   ne.setRectSize (3, 3);
   ne.setNormalEstimationMethod (ne.SIMPLE_3D_GRADIENT);
@@ -511,7 +511,7 @@ main (int argc, char** argv)
 {
   cloud.width = 640;
   cloud.height = 480;
-  cloud.points.resize (cloud.width * cloud.height);
+  cloud.resize (cloud.width * cloud.height);
   cloud.is_dense = true;
   for (std::size_t v = 0; v < cloud.height; ++v)
   {
diff --git a/test/features/test_invariants_estimation.cpp b/test/features/test_invariants_estimation.cpp
index 9973c550..3ffbd9c0 100644
--- a/test/features/test_invariants_estimation.cpp
+++ b/test/features/test_invariants_estimation.cpp
@@ -48,7 +48,7 @@ using namespace pcl::io;
 using KdTreePtr = search::KdTree<PointXYZ>::Ptr;
 
 PointCloud<PointXYZ> cloud;
-std::vector<int> indices;
+pcl::Indices indices;
 KdTreePtr tree;
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/test/features/test_normal_estimation.cpp b/test/features/test_normal_estimation.cpp
index 34119960..29fbbc9e 100644
--- a/test/features/test_normal_estimation.cpp
+++ b/test/features/test_normal_estimation.cpp
@@ -51,7 +51,7 @@ using namespace pcl::io;
 using KdTreePtr = search::KdTree<PointXYZ>::Ptr;
 
 PointCloud<PointXYZ> cloud;
-std::vector<int> indices;
+pcl::Indices indices;
 KdTreePtr tree;
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -192,7 +192,7 @@ template<typename PointT>
 class DummySearch : public pcl::search::Search<PointT>
 {
   public:
-    virtual int nearestKSearch (const PointT &point, int k, std::vector<int> &k_indices,
+    virtual int nearestKSearch (const PointT &point, int k, pcl::Indices &k_indices,
                                 std::vector<float> &k_sqr_distances ) const
     {
       pcl::utils::ignore(point);
@@ -203,7 +203,7 @@ class DummySearch : public pcl::search::Search<PointT>
 	  return k;
     }
 
-    virtual int radiusSearch (const PointT& point, double radius, std::vector<int>& k_indices,
+    virtual int radiusSearch (const PointT& point, double radius, pcl::Indices& k_indices,
                               std::vector<float>& k_sqr_distances, unsigned int max_nn = 0 ) const
     {
       pcl::utils::ignore(point, radius, k_indices, k_sqr_distances);
diff --git a/test/features/test_pfh_estimation.cpp b/test/features/test_pfh_estimation.cpp
index 080d9acc..3b4967a2 100644
--- a/test/features/test_pfh_estimation.cpp
+++ b/test/features/test_pfh_estimation.cpp
@@ -56,7 +56,7 @@ using KdTreePtr = pcl::search::KdTree<PointT>::Ptr;
 using pcl::PointCloud;
 
 static PointCloud<PointT>::Ptr cloud (new PointCloud<PointT> ());
-static std::vector<int> indices;
+static pcl::Indices indices;
 static KdTreePtr tree;
 
 ///////////////////////////////////////////////////////////////////////////////////
diff --git a/test/features/test_rift_estimation.cpp b/test/features/test_rift_estimation.cpp
index e3510104..4a0eba25 100644
--- a/test/features/test_rift_estimation.cpp
+++ b/test/features/test_rift_estimation.cpp
@@ -61,7 +61,7 @@ TEST (PCL, RIFTEstimation)
       p.intensity = std::exp ((-powf (x - 3.0f, 2.0f) + powf (y + 2.0f, 2.0f)) / (2.0f * 25.0f)) + std::exp ((-powf (x + 5.0f, 2.0f) + powf (y - 5.0f, 2.0f))
                                                                                  / (2.0f * 4.0f));
 
-      cloud_xyzi.points.push_back (p);
+      cloud_xyzi.push_back (p);
     }
   }
   cloud_xyzi.width = cloud_xyzi.size ();
@@ -71,7 +71,7 @@ TEST (PCL, RIFTEstimation)
   gradient.height = 1;
   gradient.width = cloud_xyzi.size ();
   gradient.is_dense = true;
-  gradient.points.resize (gradient.width);
+  gradient.resize (gradient.width);
   for (std::size_t i = 0; i < cloud_xyzi.size (); ++i)
   {
     const PointXYZI &p = cloud_xyzi[i];
diff --git a/test/features/test_rsd_estimation.cpp b/test/features/test_rsd_estimation.cpp
index 81fe9dd9..b30be4f6 100644
--- a/test/features/test_rsd_estimation.cpp
+++ b/test/features/test_rsd_estimation.cpp
@@ -49,7 +49,6 @@ using namespace pcl::io;
 
 search::KdTree<PointXYZ>::Ptr tree (new search::KdTree<PointXYZ> ());
 PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ> ());
-std::vector<int> indices;
 
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 TEST (PCL, RSDEstimation)
diff --git a/test/features/test_shot_estimation.cpp b/test/features/test_shot_estimation.cpp
index 4e2022cb..1ccc9d67 100644
--- a/test/features/test_shot_estimation.cpp
+++ b/test/features/test_shot_estimation.cpp
@@ -53,12 +53,12 @@ using namespace pcl::io;
 using KdTreePtr = search::KdTree<PointXYZ>::Ptr;
 
 PointCloud<PointXYZ> cloud;
-std::vector<int> indices;
+pcl::Indices indices;
 KdTreePtr tree;
 
 ///////////////////////////////////////////////////////////////////////////////////
 template<typename PointT> void
-shotCopyPointCloud (const PointCloud<PointT> &cloud_in, const std::vector<int> &indices,
+shotCopyPointCloud (const PointCloud<PointT> &cloud_in, const pcl::Indices &indices,
                     PointCloud<PointT> &cloud_out)
 {
   pcl::copyPointCloud<PointT>(cloud_in, indices, cloud_out);
diff --git a/test/features/test_shot_lrf_estimation.cpp b/test/features/test_shot_lrf_estimation.cpp
index 0c231a34..4c5f5e99 100644
--- a/test/features/test_shot_lrf_estimation.cpp
+++ b/test/features/test_shot_lrf_estimation.cpp
@@ -50,7 +50,7 @@ using namespace pcl::io;
 using KdTreePtr = search::KdTree<PointXYZ>::Ptr;
 
 PointCloud<PointXYZ> cloud;
-std::vector<int> indices;
+pcl::Indices indices;
 KdTreePtr tree;
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/test/features/test_spin_estimation.cpp b/test/features/test_spin_estimation.cpp
index 328cf98c..ec540575 100644
--- a/test/features/test_spin_estimation.cpp
+++ b/test/features/test_spin_estimation.cpp
@@ -50,7 +50,7 @@ using namespace pcl::io;
 using KdTreePtr = search::KdTree<PointXYZ>::Ptr;
 
 PointCloud<PointXYZ> cloud;
-std::vector<int> indices;
+pcl::Indices indices;
 KdTreePtr tree;
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -250,7 +250,7 @@ TEST (PCL, IntensitySpinEstimation)
       p.intensity = std::exp (-(powf (x - 3.0f, 2.0f) + powf (y + 2.0f, 2.0f)) / (2.0f * 25.0f)) + std::exp (-(powf (x + 5.0f, 2.0f) + powf (y - 5.0f, 2.0f))
                                                                                  / (2.0f * 4.0f));
 
-      cloud_xyzi.points.push_back (p);
+      cloud_xyzi.push_back (p);
     }
   }
   cloud_xyzi.width = cloud_xyzi.size ();
diff --git a/test/filters/CMakeLists.txt b/test/filters/CMakeLists.txt
index df60939a..1cbb3249 100644
--- a/test/filters/CMakeLists.txt
+++ b/test/filters/CMakeLists.txt
@@ -34,8 +34,12 @@ PCL_ADD_TEST(filters_uniform_sampling test_uniform_sampling
              LINK_WITH pcl_gtest pcl_common pcl_filters)
 
 PCL_ADD_TEST(filters_convolution test_convolution
-        FILES test_convolution.cpp
-        LINK_WITH pcl_gtest pcl_filters)
+             FILES test_convolution.cpp
+             LINK_WITH pcl_gtest pcl_filters)
+
+PCL_ADD_TEST(filters_crop_hull test_crop_hull
+             FILES test_crop_hull.cpp
+             LINK_WITH pcl_gtest pcl_filters)
 
 if(BUILD_io)
   PCL_ADD_TEST(filters_bilateral test_filters_bilateral
diff --git a/test/filters/test_clipper.cpp b/test/filters/test_clipper.cpp
index 3d9f6ccf..46f82e48 100644
--- a/test/filters/test_clipper.cpp
+++ b/test/filters/test_clipper.cpp
@@ -47,7 +47,6 @@
 #include <pcl/filters/crop_box.h>
 #include <pcl/filters/extract_indices.h>
 
-#include <pcl/common/transforms.h>
 #include <pcl/common/eigen.h>
 
 using namespace pcl;
@@ -160,7 +159,7 @@ TEST (CropBox, Filters)
   cropBoxFilter.setMax (max_pt);
 
   // Indices
-  std::vector<int> indices;
+  pcl::Indices indices;
   cropBoxFilter.filter (indices);
 
   // Cloud
@@ -323,7 +322,7 @@ TEST (CropBox, Filters)
 
   // Should contain all
   EXPECT_EQ (int (indices.size ()), 7);
-  EXPECT_VECTOR_CONTAINS_ALL (std::vector<int>({1, 2, 3, 5, 6, 7, 8}), indices);
+  EXPECT_VECTOR_CONTAINS_ALL (pcl::Indices({1, 2, 3, 5, 6, 7, 8}), indices);
   EXPECT_EQ (int (cloud_out.size ()), 7);
   EXPECT_EQ (int (cloud_out.width), 7);
   EXPECT_EQ (int (cloud_out.height), 1);
@@ -347,12 +346,12 @@ TEST (CropBox, Filters)
   cropBoxFilter.filter (cloud_out);
 
   EXPECT_EQ (int (indices.size ()), 3);
-  EXPECT_VECTOR_CONTAINS_ALL (std::vector<int>({1, 2, 7}), indices);
+  EXPECT_VECTOR_CONTAINS_ALL (pcl::Indices({1, 2, 7}), indices);
   EXPECT_EQ (int (cloud_out.size ()), 3);
 
   removed_indices = cropBoxFilter.getRemovedIndices ();
   EXPECT_EQ (int (removed_indices->size ()), 4);
-  EXPECT_VECTOR_CONTAINS_ALL (std::vector<int>({3, 5, 6, 8}), *removed_indices);
+  EXPECT_VECTOR_CONTAINS_ALL (pcl::Indices({3, 5, 6, 8}), *removed_indices);
 
   // Test setNegative
   cropBoxFilter.setNegative (true);
@@ -361,7 +360,7 @@ TEST (CropBox, Filters)
 
   cropBoxFilter.filter (indices);
   EXPECT_EQ (int (indices.size ()), 4);
-  EXPECT_VECTOR_CONTAINS_ALL (std::vector<int>({3, 5, 6, 8}), indices);
+  EXPECT_VECTOR_CONTAINS_ALL (pcl::Indices({3, 5, 6, 8}), indices);
 
   cropBoxFilter.setNegative (false);
   cropBoxFilter.filter (cloud_out);
@@ -378,7 +377,7 @@ TEST (CropBox, Filters)
 
   removed_indices = cropBoxFilter.getRemovedIndices ();
   EXPECT_EQ (int (removed_indices->size ()), 7);
-  EXPECT_VECTOR_DOES_NOT_CONTAIN (std::vector<int>({0, 4}), *removed_indices);
+  EXPECT_VECTOR_DOES_NOT_CONTAIN (pcl::Indices({0, 4}), *removed_indices);
   // Test setNegative
   cropBoxFilter.setNegative (true);
   cropBoxFilter.filter (cloud_out_negative);
@@ -386,7 +385,7 @@ TEST (CropBox, Filters)
 
   cropBoxFilter.filter (indices);
   EXPECT_EQ (int (indices.size ()), 7);
-  EXPECT_VECTOR_DOES_NOT_CONTAIN(std::vector<int>({0, 4}), indices);
+  EXPECT_VECTOR_DOES_NOT_CONTAIN(pcl::Indices({0, 4}), indices);
 
   cropBoxFilter.setNegative (false);
   cropBoxFilter.filter (cloud_out);
@@ -397,14 +396,14 @@ TEST (CropBox, Filters)
   cropBoxFilter.filter (cloud_out);
 
   EXPECT_EQ (int (indices.size ()), 1);
-  EXPECT_VECTOR_CONTAINS_ALL (std::vector<int>({7}), indices);
+  EXPECT_VECTOR_CONTAINS_ALL (pcl::Indices({7}), indices);
   EXPECT_EQ (int (cloud_out.size ()), 1);
   EXPECT_EQ (int (cloud_out.width), 1);
   EXPECT_EQ (int (cloud_out.height), 1);
 
   removed_indices = cropBoxFilter.getRemovedIndices ();
   EXPECT_EQ (int (removed_indices->size ()), 6);
-  EXPECT_VECTOR_CONTAINS_ALL (std::vector<int>({1, 2, 3, 5, 6, 8}), *removed_indices);
+  EXPECT_VECTOR_CONTAINS_ALL (pcl::Indices({1, 2, 3, 5, 6, 8}), *removed_indices);
 
   // Test setNegative
   cropBoxFilter.setNegative (true);
@@ -413,7 +412,7 @@ TEST (CropBox, Filters)
 
   cropBoxFilter.filter (indices);
   EXPECT_EQ (int (indices.size ()), 6);
-  EXPECT_VECTOR_CONTAINS_ALL (std::vector<int>({1, 2, 3, 5, 6, 8}), indices);
+  EXPECT_VECTOR_CONTAINS_ALL (pcl::Indices({1, 2, 3, 5, 6, 8}), indices);
 
   cropBoxFilter.setNegative (false);
   cropBoxFilter.filter (cloud_out);
@@ -424,14 +423,14 @@ TEST (CropBox, Filters)
   cropBoxFilter.filter (cloud_out);
 
   EXPECT_EQ (int (indices.size ()), 1);
-  EXPECT_VECTOR_CONTAINS_ALL (indices, std::vector<int>({7}));
+  EXPECT_VECTOR_CONTAINS_ALL (indices, pcl::Indices({7}));
   EXPECT_EQ (int (cloud_out.size ()), 1);
   EXPECT_EQ (int (cloud_out.width), 1);
   EXPECT_EQ (int (cloud_out.height), 1);
 
   removed_indices = cropBoxFilter.getRemovedIndices ();
   EXPECT_EQ (int (removed_indices->size ()), 6);
-  EXPECT_VECTOR_CONTAINS_ALL (std::vector<int>( {1, 2, 3, 5, 6, 8}), *removed_indices);
+  EXPECT_VECTOR_CONTAINS_ALL (pcl::Indices( {1, 2, 3, 5, 6, 8}), *removed_indices);
 
   // Test setNegative
   cropBoxFilter.setNegative (true);
@@ -440,7 +439,7 @@ TEST (CropBox, Filters)
 
   cropBoxFilter.filter (indices);
   EXPECT_EQ (int (indices.size ()), 6);
-  EXPECT_VECTOR_CONTAINS_ALL (std::vector<int>({1, 2, 3, 5, 6, 8}), indices);
+  EXPECT_VECTOR_CONTAINS_ALL (pcl::Indices({1, 2, 3, 5, 6, 8}), indices);
 
   cropBoxFilter.setNegative (false);
   cropBoxFilter.filter (cloud_out);
@@ -457,7 +456,7 @@ TEST (CropBox, Filters)
 
   removed_indices = cropBoxFilter.getRemovedIndices ();
   EXPECT_EQ (int (removed_indices->size ()), 7);
-  EXPECT_VECTOR_DOES_NOT_CONTAIN (std::vector<int>({0, 4}), *removed_indices);
+  EXPECT_VECTOR_DOES_NOT_CONTAIN (pcl::Indices({0, 4}), *removed_indices);
 
   // Test setNegative
   cropBoxFilter.setNegative (true);
@@ -466,7 +465,7 @@ TEST (CropBox, Filters)
 
   cropBoxFilter.filter (indices);
   EXPECT_EQ (int (indices.size ()), 7);
-  EXPECT_VECTOR_DOES_NOT_CONTAIN (std::vector<int>({0, 4}), indices);
+  EXPECT_VECTOR_DOES_NOT_CONTAIN (pcl::Indices({0, 4}), indices);
 
   // PCLPointCloud2 without indices
   // -------------------------------------------------------------------------
@@ -484,7 +483,7 @@ TEST (CropBox, Filters)
   cropBoxFilter2.setMax (max_pt);
 
   // Indices
-  std::vector<int> indices2;
+  pcl::Indices indices2;
   cropBoxFilter2.filter (indices2);
 
   // Cloud
@@ -627,7 +626,7 @@ TEST (CropBox, Filters)
 
     // Should contain all
     EXPECT_EQ (int (indices2.size ()), 7);
-    EXPECT_VECTOR_CONTAINS_ALL (std::vector<int>({1, 2, 3, 5, 6, 7, 8}), indices2);
+    EXPECT_VECTOR_CONTAINS_ALL (pcl::Indices({1, 2, 3, 5, 6, 7, 8}), indices2);
     EXPECT_EQ (int (cloud_out2.width), 7);
     EXPECT_EQ (int (cloud_out2.height), 1);
 
@@ -650,12 +649,12 @@ TEST (CropBox, Filters)
     cropBoxFilter2.filter (cloud_out2);
 
     EXPECT_EQ (int (indices2.size ()), 3);
-    EXPECT_VECTOR_CONTAINS_ALL (std::vector<int>({1, 2, 7}), indices2);
+    EXPECT_VECTOR_CONTAINS_ALL (pcl::Indices({1, 2, 7}), indices2);
     EXPECT_EQ (int (cloud_out2.width*cloud_out2.height), 3);
 
     removed_indices2 = cropBoxFilter2.getRemovedIndices ();
     EXPECT_EQ (int (removed_indices2->size ()), 4);
-    EXPECT_VECTOR_CONTAINS_ALL (std::vector<int>({3, 5, 6, 8}), *removed_indices2);
+    EXPECT_VECTOR_CONTAINS_ALL (pcl::Indices({3, 5, 6, 8}), *removed_indices2);
 
     // Test setNegative
     cropBoxFilter2.setNegative (true);
@@ -664,7 +663,7 @@ TEST (CropBox, Filters)
 
     cropBoxFilter2.filter (indices2);
     EXPECT_EQ (int (indices2.size ()), 4);
-    EXPECT_VECTOR_CONTAINS_ALL (std::vector<int>({3, 5, 6, 8}), indices2);
+    EXPECT_VECTOR_CONTAINS_ALL (pcl::Indices({3, 5, 6, 8}), indices2);
 
     cropBoxFilter2.setNegative (false);
     cropBoxFilter2.filter (cloud_out2);
@@ -681,7 +680,7 @@ TEST (CropBox, Filters)
     removed_indices2 = cropBoxFilter2.getRemovedIndices ();
     EXPECT_EQ (int (removed_indices2->size ()), 7);
 
-    EXPECT_VECTOR_DOES_NOT_CONTAIN (std::vector<int>({0, 4}), *removed_indices2);
+    EXPECT_VECTOR_DOES_NOT_CONTAIN (pcl::Indices({0, 4}), *removed_indices2);
     // Test setNegative
     cropBoxFilter2.setNegative (true);
     cropBoxFilter2.filter (cloud_out2_negative);
@@ -689,7 +688,7 @@ TEST (CropBox, Filters)
 
     cropBoxFilter2.filter (indices2);
     EXPECT_EQ (int (indices2.size ()), 7);
-    EXPECT_VECTOR_DOES_NOT_CONTAIN (std::vector<int>({0, 4}), indices2);
+    EXPECT_VECTOR_DOES_NOT_CONTAIN (pcl::Indices({0, 4}), indices2);
 
     cropBoxFilter2.setNegative (false);
     cropBoxFilter2.filter (cloud_out2);
@@ -700,13 +699,13 @@ TEST (CropBox, Filters)
     cropBoxFilter2.filter (cloud_out2);
 
     EXPECT_EQ (int (indices2.size ()), 1);
-    EXPECT_VECTOR_CONTAINS_ALL (std::vector<int>({7}), indices2);
+    EXPECT_VECTOR_CONTAINS_ALL (pcl::Indices({7}), indices2);
     EXPECT_EQ (int (cloud_out2.width), 1);
     EXPECT_EQ (int (cloud_out2.height), 1);
 
     removed_indices2 = cropBoxFilter2.getRemovedIndices ();
     EXPECT_EQ (int (removed_indices2->size ()), 6);
-    EXPECT_VECTOR_CONTAINS_ALL (std::vector<int>({1, 2, 3, 5, 6, 8}), *removed_indices2);
+    EXPECT_VECTOR_CONTAINS_ALL (pcl::Indices({1, 2, 3, 5, 6, 8}), *removed_indices2);
 
     // Test setNegative
     cropBoxFilter2.setNegative (true);
@@ -715,7 +714,7 @@ TEST (CropBox, Filters)
 
     cropBoxFilter2.filter (indices2);
     EXPECT_EQ (int (indices2.size ()), 6);
-    EXPECT_VECTOR_CONTAINS_ALL (std::vector<int>({1, 2, 3, 5, 6, 8}), indices2);
+    EXPECT_VECTOR_CONTAINS_ALL (pcl::Indices({1, 2, 3, 5, 6, 8}), indices2);
 
     cropBoxFilter2.setNegative (false);
     cropBoxFilter2.filter (cloud_out2);
@@ -726,13 +725,13 @@ TEST (CropBox, Filters)
     cropBoxFilter2.filter (cloud_out2);
 
     EXPECT_EQ (int (indices2.size ()), 1);
-    EXPECT_VECTOR_CONTAINS_ALL (std::vector<int>({7}), indices2);
+    EXPECT_VECTOR_CONTAINS_ALL (pcl::Indices({7}), indices2);
     EXPECT_EQ (int (cloud_out2.width), 1);
     EXPECT_EQ (int (cloud_out2.height), 1);
 
     removed_indices = cropBoxFilter.getRemovedIndices ();
     EXPECT_EQ (int (removed_indices2->size ()), 6);
-    EXPECT_VECTOR_CONTAINS_ALL (std::vector<int>( {1, 2, 3, 5, 6, 8}), *removed_indices2);
+    EXPECT_VECTOR_CONTAINS_ALL (pcl::Indices({1, 2, 3, 5, 6, 8}), *removed_indices2);
 
     // Test setNegative
     cropBoxFilter2.setNegative (true);
@@ -741,7 +740,7 @@ TEST (CropBox, Filters)
 
     cropBoxFilter2.filter (indices2);
     EXPECT_EQ (int (indices2.size ()), 6);
-    EXPECT_VECTOR_CONTAINS_ALL (std::vector<int>({1, 2, 3, 5, 6, 8}), indices2);
+    EXPECT_VECTOR_CONTAINS_ALL (pcl::Indices({1, 2, 3, 5, 6, 8}), indices2);
 
     cropBoxFilter2.setNegative (false);
     cropBoxFilter2.filter (cloud_out2);
@@ -757,7 +756,7 @@ TEST (CropBox, Filters)
 
     removed_indices2 = cropBoxFilter2.getRemovedIndices ();
     EXPECT_EQ (int (removed_indices2->size ()), 7);
-    EXPECT_VECTOR_DOES_NOT_CONTAIN (std::vector<int>({0, 4}), *removed_indices2);
+    EXPECT_VECTOR_DOES_NOT_CONTAIN (pcl::Indices({0, 4}), *removed_indices2);
 
     // Test setNegative
     cropBoxFilter2.setNegative (true);
@@ -766,7 +765,7 @@ TEST (CropBox, Filters)
 
     cropBoxFilter2.filter (indices2);
     EXPECT_EQ (int (indices2.size ()), 7);
-    EXPECT_VECTOR_DOES_NOT_CONTAIN (std::vector<int>({0, 4}), indices2);
+    EXPECT_VECTOR_DOES_NOT_CONTAIN (pcl::Indices({0, 4}), indices2);
 
 }
 
diff --git a/test/filters/test_crop_hull.cpp b/test/filters/test_crop_hull.cpp
new file mode 100644
index 00000000..d46838a3
--- /dev/null
+++ b/test/filters/test_crop_hull.cpp
@@ -0,0 +1,376 @@
+/*
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ */
+
+#include <random>
+#include <algorithm>
+#include <array>
+#include <tuple>
+
+#include <pcl/test/gtest.h>
+#include <pcl/pcl_tests.h>
+
+#include <pcl/point_types.h>
+#include <pcl/common/common.h>
+#include <pcl/filters/crop_hull.h>
+
+
+namespace
+{
+
+
+struct TestData
+{
+  TestData(pcl::Indices const & insideIndices, pcl::PointCloud<pcl::PointXYZ>::ConstPtr input_cloud)
+    : input_cloud_(input_cloud),
+      inside_mask_(input_cloud_->size(), false),
+      inside_indices_(insideIndices),
+      inside_cloud_(new pcl::PointCloud<pcl::PointXYZ>),
+      outside_cloud_(new pcl::PointCloud<pcl::PointXYZ>)
+  {
+    pcl::copyPointCloud(*input_cloud_, inside_indices_, *inside_cloud_);
+    for (pcl::index_t idx : inside_indices_) {
+      inside_mask_[idx] = true;
+    }
+    for (size_t i = 0; i < input_cloud_->size(); ++i) {
+      if (!inside_mask_[i]) {
+        outside_indices_.push_back(i);
+      }
+    }
+    pcl::copyPointCloud(*input_cloud_, outside_indices_, *outside_cloud_);
+  }
+
+  pcl::PointCloud<pcl::PointXYZ>::ConstPtr input_cloud_;
+  std::vector<bool> inside_mask_;
+  pcl::Indices inside_indices_, outside_indices_;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr inside_cloud_, outside_cloud_;
+};
+
+
+std::vector<TestData>
+createTestDataSuite(
+    std::function<pcl::PointXYZ()> inside_point_generator,
+    std::function<pcl::PointXYZ()> outside_point_generator)
+{
+  std::vector<TestData> test_data_suite;
+  size_t const chunk_size = 1000;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr inside_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr outside_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr mixed_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::Indices inside_indices_for_inside_cloud;
+  pcl::Indices inside_indices_for_outside_cloud; // empty indices, cause outside_cloud don't contains any inside point
+  pcl::Indices inside_indices_for_mixed_cloud;
+  for (size_t i = 0; i < chunk_size; ++i)
+  {
+    inside_indices_for_inside_cloud.push_back(i);
+    inside_cloud->push_back(inside_point_generator());
+    outside_cloud->push_back(outside_point_generator());
+    if (i % 2) {
+      inside_indices_for_mixed_cloud.push_back(i);
+      mixed_cloud->push_back(inside_point_generator());
+    }
+    else {
+      mixed_cloud->push_back(outside_point_generator());
+    }
+  }
+  test_data_suite.emplace_back(std::move(inside_indices_for_inside_cloud), inside_cloud);
+  test_data_suite.emplace_back(std::move(inside_indices_for_outside_cloud), outside_cloud);
+  test_data_suite.emplace_back(std::move(inside_indices_for_mixed_cloud), mixed_cloud);
+  return test_data_suite;
+}
+
+
+template <class TupleType>
+class PCLCropHullTestFixture : public ::testing::Test
+{
+  public:
+    using CropHullTestTraits = typename std::tuple_element<0, TupleType>::type;
+    using RandomGeneratorType =  typename std::tuple_element<1, TupleType>::type;
+
+    PCLCropHullTestFixture()
+    {
+      baseOffsetList_.emplace_back(0, 0, 0);
+      baseOffsetList_.emplace_back(5, 1, 10);
+      baseOffsetList_.emplace_back(1, 5, 10);
+      baseOffsetList_.emplace_back(1, 10, 5);
+      baseOffsetList_.emplace_back(10, 1, 5);
+      baseOffsetList_.emplace_back(10, 5, 1);
+    }
+  protected:
+
+    void
+    SetUp () override
+    {
+      data_.clear();
+      pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+      for (pcl::PointXYZ const & baseOffset : baseOffsetList_)
+      {
+        pcl::copyPointCloud(*CropHullTestTraits::getHullCloud(), *input_cloud);
+        for (pcl::PointXYZ & p : *input_cloud) {
+          p.getVector3fMap() += baseOffset.getVector3fMap();
+        }
+        auto inside_point_generator = [this, &baseOffset] () {
+          pcl::PointXYZ p(rg_(), rg_(), rg_());
+          p.getVector3fMap() += baseOffset.getVector3fMap();
+          return p;
+        };
+        auto outside_point_generator = [this, &baseOffset] () {
+          std::array<float, 3> pt;
+          std::generate(pt.begin(), pt.end(),
+              [this] {
+                float v = rg_();
+                return v + std::copysign(0.51, v);
+              });
+          pcl::PointXYZ p(pt[0], pt[1], pt[2]);
+          p.getVector3fMap() += baseOffset.getVector3fMap();
+          return p;
+        };
+        pcl::CropHull<pcl::PointXYZ> crop_hull_filter = createDefaultCropHull(input_cloud);
+        std::vector<TestData> test_data_suite = createTestDataSuite(inside_point_generator, outside_point_generator);
+        data_.emplace_back(crop_hull_filter, test_data_suite);
+      }
+    }
+
+    std::vector<std::pair<pcl::CropHull<pcl::PointXYZ>, std::vector<TestData>>> data_;
+
+  private:
+    pcl::CropHull<pcl::PointXYZ>
+    createDefaultCropHull (pcl::PointCloud<pcl::PointXYZ>::ConstPtr input_cloud) const
+    {
+      //pcl::CropHull<pcl::PointXYZ> crop_hull_filter(true);
+      pcl::CropHull<pcl::PointXYZ> crop_hull_filter;
+      crop_hull_filter.setHullCloud(input_cloud->makeShared());
+      crop_hull_filter.setHullIndices(CropHullTestTraits::getHullPolygons());
+      crop_hull_filter.setDim(CropHullTestTraits::getDim());
+      return crop_hull_filter;
+    }
+
+    RandomGeneratorType rg_;
+    pcl::PointCloud<pcl::PointXYZ> baseOffsetList_;
+};
+
+
+struct CropHullTestTraits2d
+{
+  static pcl::PointCloud<pcl::PointXYZ>::ConstPtr
+  getHullCloud();
+
+  static std::vector<pcl::Vertices>
+  getHullPolygons();
+
+  static int
+  getDim();
+};
+
+
+struct CropHullTestTraits3d
+{
+  static pcl::PointCloud<pcl::PointXYZ>::ConstPtr
+  getHullCloud();
+
+  static std::vector<pcl::Vertices>
+  getHullPolygons();
+
+  static int
+  getDim();
+};
+
+
+template <size_t seed> struct RandomGenerator
+{
+  public:
+    RandomGenerator()
+      : gen_(seed), rd_(-0.5f, 0.5f)
+    {}
+
+    float operator()() {
+      return rd_(gen_);
+    }
+
+  private:
+    std::mt19937 gen_;
+    std::uniform_real_distribution<float> rd_;
+};
+
+
+static std::vector<std::vector<pcl::index_t>> cube_elements = {
+  {0, 2, 1}, // l
+  {1, 2, 3}, // l
+  {3, 2, 6}, // f
+  {6, 2, 4}, // bt
+  {4, 2, 0}, // bt
+  {3, 7, 1}, // t
+  {1, 7, 5}, // t
+  {5, 7, 4}, // r
+  {4, 7, 6}, // r
+  {6, 7, 3}, // f
+  {5, 1, 4}, // back
+  {4, 1, 0}  // back
+};
+
+
+pcl::PointCloud<pcl::PointXYZ>::ConstPtr
+CropHullTestTraits2d::getHullCloud ()
+{
+  static pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+  if (input_cloud->empty()) {
+    for (const float i: {-0.5f, 0.5f})
+      for (const float j: {-0.5f, .5f})
+        for (const float k: {0.f, -0.1f})
+          input_cloud->emplace_back(i, j, k);
+  }
+  return input_cloud;
+}
+
+std::vector<pcl::Vertices>
+CropHullTestTraits2d::getHullPolygons ()
+{
+  std::vector<pcl::Vertices> polygons(12);
+  for (size_t i = 0; i < 12; ++i) {
+    polygons[i].vertices = cube_elements[i];
+  }
+  return polygons;
+}
+
+int
+CropHullTestTraits2d::getDim ()
+{
+  return 2;
+}
+
+
+pcl::PointCloud<pcl::PointXYZ>::ConstPtr
+CropHullTestTraits3d::getHullCloud ()
+{
+  static pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+  if (input_cloud->empty()) {
+    for (const float i: {-0.5f, 0.5f})
+      for (const float j: {-0.5f, 0.5f})
+        for (const float k: {-0.5f, 0.5f})
+          input_cloud->emplace_back(i, j, k);
+  }
+  return input_cloud;
+}
+
+std::vector<pcl::Vertices>
+CropHullTestTraits3d::getHullPolygons ()
+{
+  std::vector<pcl::Vertices> polygons(12);
+  for (size_t i = 0; i < 12; ++i) {
+    polygons[i].vertices = cube_elements[i];
+  }
+  return polygons;
+}
+
+int
+CropHullTestTraits3d::getDim ()
+{
+  return 3;
+}
+
+} // end of anonymous namespace
+using CropHullTestTraits2dList = std::tuple<
+  std::tuple<CropHullTestTraits2d, RandomGenerator<0>>,
+  std::tuple<CropHullTestTraits2d, RandomGenerator<123>>,
+  std::tuple<CropHullTestTraits2d, RandomGenerator<456>>
+  >;
+using CropHullTestTraits3dList = std::tuple<
+  std::tuple<CropHullTestTraits3d, RandomGenerator<0>>,
+  std::tuple<CropHullTestTraits3d, RandomGenerator<123>>,
+  std::tuple<CropHullTestTraits3d, RandomGenerator<456>>
+  >;
+using CropHullTestTypes = ::testing::Types<
+  std::tuple_element<0, CropHullTestTraits2dList>::type,
+  std::tuple_element<1, CropHullTestTraits2dList>::type,
+  std::tuple_element<2, CropHullTestTraits2dList>::type,
+  std::tuple_element<0, CropHullTestTraits3dList>::type,
+  std::tuple_element<1, CropHullTestTraits3dList>::type,
+  std::tuple_element<2, CropHullTestTraits3dList>::type
+  >;
+TYPED_TEST_SUITE(PCLCropHullTestFixture, CropHullTestTypes);
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// since test input cloud has same distribution for all dimensions, this test also check problem from issue #3960 //
+TYPED_TEST (PCLCropHullTestFixture, simple_test)
+{
+  for (auto & entry : this->data_)
+  {
+    auto & crop_hull_filter = entry.first;
+    for (TestData const & test_data : entry.second)
+    {
+      crop_hull_filter.setInputCloud(test_data.input_cloud_);
+      pcl::Indices filtered_indices;
+      crop_hull_filter.filter(filtered_indices);
+      pcl::test::EXPECT_EQ_VECTORS(test_data.inside_indices_, filtered_indices);
+    }
+  }
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+TYPED_TEST (PCLCropHullTestFixture, test_cloud_filtering)
+{
+  for (auto & entry : this->data_)
+  {
+    auto & crop_hull_filter = entry.first;
+    for (TestData const & test_data : entry.second)
+    {
+      crop_hull_filter.setInputCloud(test_data.input_cloud_);
+      pcl::PointCloud<pcl::PointXYZ> filteredCloud;
+      crop_hull_filter.filter(filteredCloud);
+      ASSERT_EQ (test_data.inside_cloud_->size(), filteredCloud.size());
+      pcl::index_t cloud_size = test_data.inside_cloud_->size();
+      for (pcl::index_t i = 0; i < cloud_size; ++i)
+      {
+        EXPECT_XYZ_NEAR(test_data.inside_cloud_->at(i), filteredCloud[i], 1e-5);
+      }
+    }
+  }
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// this test will pass only for 2d case //
+template <class T>
+struct PCLCropHullTestFixture2dCrutch : PCLCropHullTestFixture<T>
+{};
+using CropHullTestTraits2dTypes = ::testing::Types<
+  std::tuple_element<0, CropHullTestTraits2dList>::type,
+  std::tuple_element<1, CropHullTestTraits2dList>::type,
+  std::tuple_element<2, CropHullTestTraits2dList>::type
+  >;
+TYPED_TEST_SUITE(PCLCropHullTestFixture2dCrutch, CropHullTestTraits2dTypes);
+
+TYPED_TEST (PCLCropHullTestFixture2dCrutch, test_crop_inside)
+{
+  for (auto & entry : this->data_)
+  {
+    auto & crop_hull_filter = entry.first;
+    for (TestData const & test_data : entry.second)
+    {
+      crop_hull_filter.setInputCloud(test_data.input_cloud_);
+      crop_hull_filter.setCropOutside(false);
+      pcl::Indices filtered_indices;
+      crop_hull_filter.filter(filtered_indices);
+      pcl::test::EXPECT_EQ_VECTORS(test_data.outside_indices_, filtered_indices);
+    }
+  }
+}
+
+
+
+/* ---[ */
+int
+main (int argc, char** argv)
+{
+  // Testing
+  testing::InitGoogleTest (&argc, argv);
+  return (RUN_ALL_TESTS ());
+}
+/* ]--- */
diff --git a/test/filters/test_filters.cpp b/test/filters/test_filters.cpp
index a0222f5a..cae013c4 100644
--- a/test/filters/test_filters.cpp
+++ b/test/filters/test_filters.cpp
@@ -69,7 +69,6 @@ using namespace Eigen;
 
 PCLPointCloud2::Ptr cloud_blob (new PCLPointCloud2);
 PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
-std::vector<int> indices_;
 
 PointCloud<PointXYZRGB>::Ptr cloud_organized (new PointCloud<PointXYZRGB>);
 
@@ -238,7 +237,7 @@ TEST (ExtractIndices, Filters)
   eie.filter (result);
   EXPECT_EQ (result.size (), 0);
 
-  empty.points.resize (10);
+  empty.resize (10);
   empty.width = 10; empty.height = 1;
   eie.setInputCloud (empty.makeShared ());
   for (int i = 0; i < 10; ++i)
@@ -253,7 +252,7 @@ TEST (ExtractIndices, Filters)
 
   /*
   PointCloud<PointXYZ> sc, scf;
-  sc.points.resize (5); sc.width = 5; sc.height = 1; sc.is_dense = true;
+  sc.resize (5); sc.width = 5; sc.height = 1; sc.is_dense = true;
   for (int i = 0; i < 5; i++)
   {
     sc[i].x = sc[i].z = 0;
@@ -897,7 +896,7 @@ TEST (VoxelGrid_RGB, Filters)
     pt.r = col_r[i];
     pt.g = col_g[i];
     pt.b = col_b[i];
-    cloud_rgb_.points.push_back (pt);
+    cloud_rgb_.push_back (pt);
   }
 
   toPCLPointCloud2 (cloud_rgb_, cloud_rgb_blob_);
@@ -991,7 +990,7 @@ TEST (VoxelGrid_RGBA, Filters)
     pt.y = 0.0f;
     pt.z = 0.0f;
     pt.rgba = *reinterpret_cast<std::uint32_t*> (&rgba);
-    cloud_rgba_.points.push_back (pt);
+    cloud_rgba_.push_back (pt);
   }
 
   toPCLPointCloud2 (cloud_rgba_, cloud_rgba_blob_);
@@ -1188,7 +1187,7 @@ TEST (VoxelGrid_XYZNormal, Filters)
       for (unsigned xIdx = 0; xIdx < 2; ++xIdx, ++idx)
       {
         PointNormal& voxel = ground_truth [xIdx][yIdx][zIdx];
-        PointNormal& point = output.points [idx];
+        PointNormal& point = output[idx];
         // check for point equalities
         EXPECT_EQ (voxel.x, point.x);
         EXPECT_EQ (voxel.y, point.y);
@@ -1224,7 +1223,7 @@ TEST (VoxelGrid_XYZNormal, Filters)
       for (unsigned xIdx = 0; xIdx < 2; ++xIdx, ++idx)
       {
         PointNormal& voxel = ground_truth [xIdx][yIdx][zIdx];
-        PointNormal& point = output.points [idx];
+        PointNormal& point = output[idx];
         // check for point equalities
         EXPECT_EQ (voxel.x, point.x);
         EXPECT_EQ (voxel.y, point.y);
@@ -1326,6 +1325,107 @@ TEST (VoxelGridCovariance, Filters)
   EXPECT_NEAR (leaves[2]->getMean ()[2], 0.0508024, 1e-4);
 }
 
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST (VoxelGridMinPoints, Filters)
+{
+  // Setup input with 4 clusters, single point at 0,0,0 and 1,1,1 with 5 point cluster around 0.11,0.11,0.11 and 6 point cluster around 0.31,0.31,0.31
+  PointCloud<PointXYZRGB>::Ptr input(new PointCloud<PointXYZRGB>());
+
+  input->emplace_back(0.0f, 0.0f, 0.0f);
+  std::vector<float> offsets {0.001, 0.002, 0.003, -0.001, -0.002, -0.003};
+  for (unsigned int i=0; i<5; ++i) {
+    input->emplace_back(0.11f+offsets[i], 0.11f+offsets[i], 0.11f+offsets[i],200,0,0);
+    input->emplace_back(0.31f+offsets[i], 0.31f+offsets[i], 0.31f+offsets[i],0,127,127);
+  }
+  input->emplace_back(0.31f+offsets[5], 0.31f+offsets[5], 0.31f+offsets[5],0,127,127);
+  input->emplace_back(1.0f, 1.0f, 1.0f);
+
+  // Test the PointCloud<PointT> VoxelGrid filter method
+  PointCloud<PointXYZRGB> outputMin4;
+  VoxelGrid<PointXYZRGB> grid;
+  // Run filter on input with MinimumPoints threshold at 4
+  grid.setLeafSize (0.02f, 0.02f, 0.02f);
+  grid.setInputCloud (input);
+  grid.setMinimumPointsNumberPerVoxel(4);
+  grid.setDownsampleAllData(true);
+  grid.filter (outputMin4);
+
+  // Verify 2 clusters (0.11 and 0.31) passed threshold and verify their location and color
+  EXPECT_EQ (outputMin4.size (), 2);
+  // Offset noise applied by offsets vec are 1e-3 magnitude, so check within 1e-2 
+  EXPECT_NEAR (outputMin4[0].x, input->at(1).x, 1e-2);
+  EXPECT_NEAR (outputMin4[0].y, input->at(1).y, 1e-2);
+  EXPECT_NEAR (outputMin4[0].z, input->at(1).z, 1e-2);
+  EXPECT_NEAR (outputMin4[0].r, input->at(1).r, 1);
+
+  EXPECT_NEAR (outputMin4[1].x, input->at(2).x, 1e-2);
+  EXPECT_NEAR (outputMin4[1].y, input->at(2).y, 1e-2);
+  EXPECT_NEAR (outputMin4[1].z, input->at(2).z, 1e-2);
+  EXPECT_NEAR (outputMin4[1].g, input->at(2).g, 1);
+  EXPECT_NEAR (outputMin4[1].b, input->at(2).b, 1);
+
+  // Run filter again on input with MinimumPoints threshold at 6
+  PointCloud<PointXYZRGB> outputMin6;
+  grid.setMinimumPointsNumberPerVoxel(6);
+  grid.setDownsampleAllData(false);
+  grid.filter (outputMin6);
+
+  // Verify 1 cluster (0.31) passed threshold and verify location
+  EXPECT_EQ (outputMin6.size (), 1);
+
+  EXPECT_NEAR (outputMin6[0].x, input->at(2).x, 1e-2);
+  EXPECT_NEAR (outputMin6[0].y, input->at(2).y, 1e-2);
+  EXPECT_NEAR (outputMin6[0].z, input->at(2).z, 1e-2);
+
+  // Test the pcl::PCLPointCloud2 VoxelGrid filter method
+  PCLPointCloud2 output_pc2;
+  VoxelGrid<PCLPointCloud2> grid_pc2;
+  PCLPointCloud2::Ptr input_pc2(new PCLPointCloud2());
+
+  // Use same input as above converted to PCLPointCloud2
+  toPCLPointCloud2(*input, *input_pc2);
+
+  // Run filter on input with MinimumPoints threshold at 4
+  grid_pc2.setLeafSize (0.02f, 0.02f, 0.02f);
+  grid_pc2.setInputCloud (input_pc2);
+  grid_pc2.setMinimumPointsNumberPerVoxel(4);
+  grid_pc2.setDownsampleAllData(true);
+  grid_pc2.filter (output_pc2);
+
+  // Convert back to PointXYZRGB for easier data access
+  PointCloud<pcl::PointXYZRGB>::Ptr out_pc( new pcl::PointCloud<pcl::PointXYZRGB> );
+  pcl::fromPCLPointCloud2( output_pc2, *out_pc );
+
+  // Verify 2 clusters (0.11 and 0.31) passed threshold and verify their location and color
+  // PCLPointCloud2 output should be same as PointCloudXYZRGB, account for floating point rounding error with 1e-4
+  EXPECT_EQ (out_pc->points.size (), outputMin4.size());
+
+  EXPECT_NEAR (out_pc->at(0).x, outputMin4[0].x, 1e-4);
+  EXPECT_NEAR (out_pc->at(0).y, outputMin4[0].y, 1e-4);
+  EXPECT_NEAR (out_pc->at(0).z, outputMin4[0].z, 1e-4);
+  EXPECT_NEAR (out_pc->at(0).r, outputMin4[0].r, 1);
+
+  EXPECT_NEAR (out_pc->at(1).x, outputMin4[1].x, 1e-4);
+  EXPECT_NEAR (out_pc->at(1).y, outputMin4[1].y, 1e-4);
+  EXPECT_NEAR (out_pc->at(1).z, outputMin4[1].z, 1e-4);
+  EXPECT_NEAR (out_pc->at(1).g, outputMin4[1].g, 1);
+  EXPECT_NEAR (out_pc->at(1).b, outputMin4[1].b, 1);
+
+  // Run filter again on input with MinimumPoints threshold at 6
+  grid_pc2.setMinimumPointsNumberPerVoxel(6);
+  grid_pc2.setDownsampleAllData(false);
+  grid_pc2.filter (output_pc2);
+
+  // Convert back to PointXYZRGB for easier data access
+  pcl::fromPCLPointCloud2( output_pc2, *out_pc );
+
+  // Verify 1 cluster (0.31) passed threshold and verify location
+  EXPECT_EQ (out_pc->points.size (), outputMin6.size());
+
+  EXPECT_NEAR (out_pc->at(0).x, outputMin6[0].x, 1e-4);
+  EXPECT_NEAR (out_pc->at(0).y, outputMin6[0].y, 1e-4);
+  EXPECT_NEAR (out_pc->at(0).z, outputMin6[0].z, 1e-4);
+}
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 TEST (ProjectInliers, Filters)
 {
@@ -1344,7 +1444,7 @@ TEST (ProjectInliers, Filters)
   proj.setModelCoefficients (coefficients);
   proj.filter (output);
 
-  for (const auto &point : output.points)
+  for (const auto &point : output)
     EXPECT_NEAR (point.z, 0.0, 1e-4);
 
   // Test the pcl::PCLPointCloud2 method
@@ -1359,7 +1459,7 @@ TEST (ProjectInliers, Filters)
 
   fromPCLPointCloud2 (output_blob, output);
 
-  for (const auto &point : output.points)
+  for (const auto &point : output)
     EXPECT_NEAR (point.z, 0.0, 1e-4);
 }
 
@@ -1589,7 +1689,7 @@ TEST (ConditionalRemoval, Filters)
   condrem.filter (output);
 
   int num_not_nan = 0;
-  for (const auto &point : output.points)
+  for (const auto &point : output)
   {
     if (std::isfinite (point.x) &&
         std::isfinite (point.y) &&
@@ -1626,7 +1726,7 @@ TEST (ConditionalRemoval, Filters)
   condrem_.filter (output);
 
   num_not_nan = 0;
-  for (const auto &point : output.points)
+  for (const auto &point : output)
   {
     if (std::isfinite (point.x) &&
         std::isfinite (point.y) &&
@@ -1694,7 +1794,7 @@ TEST (ConditionalRemovalSetIndices, Filters)
   EXPECT_EQ ((*cloud)[cloud->size () - 1].z, output[output.size () - 1].z);
 
   int num_not_nan = 0;
-  for (const auto &point : output.points)
+  for (const auto &point : output)
   {
     if (std::isfinite (point.x) &&
         std::isfinite (point.y) &&
@@ -1744,7 +1844,7 @@ TEST (ConditionalRemovalSetIndices, Filters)
   EXPECT_EQ ((*cloud)[cloud->size () - 1].z, output[output.size () - 1].z);
 
   num_not_nan = 0;
-  for (const auto &point : output.points)
+  for (const auto &point : output)
   {
     if (std::isfinite (point.x) &&
         std::isfinite (point.y) &&
@@ -1775,7 +1875,7 @@ TEST (SamplingSurfaceNormal, Filters)
       pt.x = i;
       pt.y = j;
       pt.z = 1;
-      incloud->points.push_back (pt);
+      incloud->push_back (pt);
     }
   }
   incloud->height = 1;
@@ -1787,7 +1887,7 @@ TEST (SamplingSurfaceNormal, Filters)
   ssn_filter.filter (outcloud);
 
   // All the sampled points should have normals along the direction of Z axis
-  for (const auto &point : outcloud.points)
+  for (const auto &point : outcloud)
   {
     EXPECT_NEAR (point.normal[0], 0, 1e-3);
     EXPECT_NEAR (point.normal[1], 0, 1e-3);
@@ -1810,7 +1910,7 @@ TEST (ShadowPoints, Filters)
 
   // Adding a shadow point
   PointXYZ pt (.0f, .0f, .1f);
-  input->points.push_back (pt);
+  input->push_back (pt);
 
   input->height = 1;
   input->width = input->size ();
@@ -1875,7 +1975,7 @@ TEST (FrustumCulling, Filters)
         pt.x = float (i);
         pt.y = float (j);
         pt.z = float (k);
-        input->points.push_back (pt);
+        input->push_back (pt);
       }
     }
   }
@@ -2152,7 +2252,7 @@ TEST (NormalRefinement, Filters)
 
   // Input without NaN
   pcl::PointCloud<pcl::PointXYZRGB> cloud_organized_nonan;
-  std::vector<int> dummy;
+  pcl::Indices dummy;
   pcl::removeNaNFromPointCloud<pcl::PointXYZRGB> (*cloud_organized, cloud_organized_nonan, dummy);
 
   // Viewpoint
@@ -2162,7 +2262,7 @@ TEST (NormalRefinement, Filters)
 
   // Search parameters
   const int k = 5;
-  std::vector<std::vector<int> > k_indices;
+  std::vector<pcl::Indices> k_indices;
   std::vector<std::vector<float> > k_sqr_distances;
 
   // Estimated and refined normal containers
@@ -2176,7 +2276,7 @@ TEST (NormalRefinement, Filters)
   // Search for neighbors
   pcl::search::KdTree<pcl::PointXYZRGB> kdtree;
   kdtree.setInputCloud (cloud_organized_nonan.makeShared ());
-  kdtree.nearestKSearch (cloud_organized_nonan, std::vector<int> (), k, k_indices, k_sqr_distances);
+  kdtree.nearestKSearch (cloud_organized_nonan, pcl::Indices (), k, k_indices, k_sqr_distances);
 
   /*
    * Estimate normals
@@ -2226,7 +2326,7 @@ TEST (NormalRefinement, Filters)
   seg.segment (*inliers, *coefficients);
 
   // Read out SAC model
-  const std::vector<int>& idx_table = inliers->indices;
+  const auto& idx_table = inliers->indices;
   float a = coefficients->values[0];
   float b = coefficients->values[1];
   float c = coefficients->values[2];
@@ -2256,7 +2356,7 @@ TEST (NormalRefinement, Filters)
   int num_nans = 0;
 
   // Loop
-  for (const int &idx : idx_table)
+  for (const auto &idx : idx_table)
   {
     float tmp;
 
@@ -2335,10 +2435,6 @@ main (int argc, char** argv)
   loadPCDFile (file_name, *cloud_blob);
   fromPCLPointCloud2 (*cloud_blob, *cloud);
 
-  indices_.resize (cloud->size ());
-  for (index_t i = 0; i < static_cast<index_t>(indices_.size ()); ++i)
-    indices_[i] = i;
-
 
   loadPCDFile (argv[2], *cloud_organized);
 
diff --git a/test/filters/test_functor_filter.cpp b/test/filters/test_functor_filter.cpp
index 49ec7f50..e4a11907 100644
--- a/test/filters/test_functor_filter.cpp
+++ b/test/filters/test_functor_filter.cpp
@@ -18,21 +18,21 @@ using namespace pcl::experimental;
 TEST(FunctorFilterTrait, CheckCompatibility)
 {
   const auto copy_all = [](PointCloud<PointXYZ>, index_t) { return 0; };
-  EXPECT_TRUE((is_functor_for_filter_v<PointXYZ, decltype(copy_all)>));
+  EXPECT_TRUE((is_function_object_for_filter_v<PointXYZ, decltype(copy_all)>));
 
   const auto ref_all = [](PointCloud<PointXYZ>&, index_t&) { return 0; };
-  EXPECT_FALSE((is_functor_for_filter_v<PointXYZ, decltype(ref_all)>));
+  EXPECT_FALSE((is_function_object_for_filter_v<PointXYZ, decltype(ref_all)>));
 
   const auto ref_cloud = [](PointCloud<PointXYZ>&, index_t) { return 0; };
-  EXPECT_FALSE((is_functor_for_filter_v<PointXYZ, decltype(ref_cloud)>));
+  EXPECT_FALSE((is_function_object_for_filter_v<PointXYZ, decltype(ref_cloud)>));
 
   const auto const_ref_cloud = [](const PointCloud<PointXYZ>&, index_t) { return 0; };
-  EXPECT_TRUE((is_functor_for_filter_v<PointXYZ, decltype(const_ref_cloud)>));
+  EXPECT_TRUE((is_function_object_for_filter_v<PointXYZ, decltype(const_ref_cloud)>));
 
   const auto const_ref_all = [](const PointCloud<PointXYZ>&, const index_t&) {
     return 0;
   };
-  EXPECT_TRUE((is_functor_for_filter_v<PointXYZ, decltype(const_ref_all)>));
+  EXPECT_TRUE((is_function_object_for_filter_v<PointXYZ, decltype(const_ref_all)>));
 }
 
 struct FunctorFilterRandom : public testing::TestWithParam<std::uint32_t> {
@@ -63,7 +63,7 @@ TEST_P(FunctorFilterRandom, functioning)
   };
 
   for (const auto& keep_removed : {true, false}) {
-    FunctorFilter<PointXYZ, decltype(lambda)> filter{lambda, keep_removed};
+    advanced::FunctorFilter<PointXYZ, decltype(lambda)> filter{lambda, keep_removed};
     filter.setInputCloud(cloud);
     const auto removed_size = filter.getRemovedIndices()->size();
 
@@ -165,7 +165,7 @@ using types = ::testing::Types<LambdaT,
 } // namespace type_test
 
 template <typename T>
-struct FunctorFilterFunctor : public ::testing::Test {
+struct FunctorFilterFunctionObject : public ::testing::Test {
   void
   SetUp() override
   {
@@ -173,20 +173,20 @@ struct FunctorFilterFunctor : public ::testing::Test {
   }
   PointCloud<PointXYZ> cloud;
 };
-TYPED_TEST_SUITE_P(FunctorFilterFunctor);
+TYPED_TEST_SUITE_P(FunctorFilterFunctionObject);
 
-TYPED_TEST_P(FunctorFilterFunctor, type_check)
+TYPED_TEST_P(FunctorFilterFunctionObject, type_check)
 {
   using FunctorT = TypeParam;
   const auto& functor = type_test::Helper<FunctorT>::value;
 
-  FunctorFilter<PointXYZ, FunctorT> filter_lambda{functor};
-  EXPECT_EQ(filter_lambda.getFunctor()(this->cloud, 0), 0);
-  EXPECT_EQ(filter_lambda.getFunctor()(this->cloud, 1), 1);
+  advanced::FunctorFilter<PointXYZ, FunctorT> filter_lambda{functor};
+  EXPECT_EQ(filter_lambda.getFunctionObject()(this->cloud, 0), 0);
+  EXPECT_EQ(filter_lambda.getFunctionObject()(this->cloud, 1), 1);
 }
 
-REGISTER_TYPED_TEST_SUITE_P(FunctorFilterFunctor, type_check);
-INSTANTIATE_TYPED_TEST_SUITE_P(pcl, FunctorFilterFunctor, type_test::types);
+REGISTER_TYPED_TEST_SUITE_P(FunctorFilterFunctionObject, type_check);
+INSTANTIATE_TYPED_TEST_SUITE_P(pcl, FunctorFilterFunctionObject, type_test::types);
 
 int
 main(int argc, char** argv)
diff --git a/test/filters/test_model_outlier_removal.cpp b/test/filters/test_model_outlier_removal.cpp
index 1fc8cd50..8a0f2c11 100644
--- a/test/filters/test_model_outlier_removal.cpp
+++ b/test/filters/test_model_outlier_removal.cpp
@@ -54,7 +54,7 @@ PointCloud<PointXYZ>::Ptr cloud_in (new PointCloud<PointXYZ>);
 TEST (ModelOutlierRemoval, Model_Outlier_Filter)
 {
   PointCloud<PointXYZ>::Ptr cloud_filter_out (new PointCloud<PointXYZ>);
-  std::vector<int> ransac_inliers;
+  pcl::Indices ransac_inliers;
   float thresh = 0.01;
   //run ransac
   Eigen::VectorXf model_coefficients;
diff --git a/test/filters/test_sampling.cpp b/test/filters/test_sampling.cpp
index f6267076..d7e10701 100644
--- a/test/filters/test_sampling.cpp
+++ b/test/filters/test_sampling.cpp
@@ -69,7 +69,7 @@ TEST (CovarianceSampling, Filters)
   // Conditioning number should be loosely close to the expected number. Adopting 10% of the reference value
   EXPECT_NEAR (113.29773, cond_num_walls, 10.);
 
-  IndicesPtr walls_indices (new std::vector<int> ());
+  IndicesPtr walls_indices (new pcl::Indices ());
   covariance_sampling.filter (*walls_indices);
   covariance_sampling.setIndices (walls_indices);
   double cond_num_walls_sampled = covariance_sampling.computeConditionNumber ();
@@ -89,7 +89,7 @@ TEST (CovarianceSampling, Filters)
   // Conditioning number should be loosely close to the expected number. Adopting 10% of the reference value
   EXPECT_NEAR (cond_num_turtle, 20661.7663, 2e4);
 
-  IndicesPtr turtle_indices (new std::vector<int> ());
+  IndicesPtr turtle_indices (new pcl::Indices ());
   covariance_sampling.filter (*turtle_indices);
   covariance_sampling.setIndices (turtle_indices);
   double cond_num_turtle_sampled = covariance_sampling.computeConditionNumber ();
@@ -154,7 +154,7 @@ TEST (RandomSample, Filters)
   sample.setSample (10);
 
   // Indices
-  std::vector<int> indices;
+  pcl::Indices indices;
   sample.filter (indices);
 
   EXPECT_EQ (int (indices.size ()), 10);
@@ -218,7 +218,7 @@ TEST (RandomSample, Filters)
   sample2.setSample (10);
 
   // Indices
-  std::vector<int> indices2;
+  pcl::Indices indices2;
   sample2.filter (indices2);
 
   EXPECT_EQ (int (indices2.size ()), 10);
@@ -268,7 +268,7 @@ main (int argc, char** argv)
   ne.compute (*cloud_walls_normals);
   copyPointCloud (*cloud_walls, *cloud_walls_normals);
 
-  std::vector<int> aux_indices;
+  pcl::Indices aux_indices;
   removeNaNFromPointCloud (*cloud_walls_normals, *cloud_walls_normals, aux_indices);
   removeNaNNormalsFromPointCloud (*cloud_walls_normals, *cloud_walls_normals, aux_indices);
 
diff --git a/test/filters/test_uniform_sampling.cpp b/test/filters/test_uniform_sampling.cpp
index f09a95c3..f9410a9e 100644
--- a/test/filters/test_uniform_sampling.cpp
+++ b/test/filters/test_uniform_sampling.cpp
@@ -71,7 +71,7 @@ TEST(UniformSampling, extractRemovedIndices)
   ASSERT_EQ(output.size(), 1000);
   EXPECT_EQ(removed_indices->size(), xyz->size() - 1000);
   std::set<int> removed_indices_set(removed_indices->begin(), removed_indices->end());
-  ASSERT_TRUE(removed_indices_set.size() == removed_indices->size());
+  ASSERT_EQ(removed_indices_set.size(), removed_indices->size());
 }
 
 int
diff --git a/test/fuzz/build.sh b/test/fuzz/build.sh
new file mode 100644
index 00000000..747809f7
--- /dev/null
+++ b/test/fuzz/build.sh
@@ -0,0 +1,65 @@
+#!/bin/bash -eu
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+#  Point Cloud Library (PCL) - www.pointclouds.org
+#  Copyright (c) 2020-, Open Perception
+#
+#  All rights reserved
+#
+
+# This script is run inside OSS-fuzz's docker image
+# and builds PCL's fuzzers to run continuously 
+# through OSS-fuzz.
+# In the OSS-fuzz  docker image PCL is located at $SRC/pcl.
+
+# The Dockerfile that builds PCL can be found here:
+# (url pending)
+
+# Build PCL
+cd pcl
+mkdir build && cd build
+pwd
+cmake -DWITH_OPENGL=FALSE \
+      -DWITH_PCAP=FALSE \
+      -DWITH_VTK=FALSE \
+      -DPCL_SHARED_LIBS:BOOL=OFF \
+      -DWITH_LIBUSB=FALSE \
+      -DWITH_QT=FALSE \
+      -DBUILD_features=OFF \
+      -DBUILD_filters=OFF \
+      -DBUILD_geometry=OFF \
+      -DBUILD_kdtree=OFF \
+      -DBUILD_keypoints=OFF \
+      -DBUILD_ml=OFF \
+      -DBUILD_outofcore=OFF \
+      -DBUILD_people=OFF \
+      -DBUILD_recognition=OFF \
+      -DBUILD_registration=OFF \
+      -DBUILD_sample_consensus=OFF \
+      -DBUILD_search=OFF \
+      -DBUILD_segmentation=OFF \
+      -DBUILD_stereo=OFF \
+      -DBUILD_surface=OFF \
+      -DBUILD_tracking=OFF \
+      -DBUILD_visualization=OFF \
+      ..
+
+make -j$(nproc)
+
+# Build fuzzers
+cd ../test/fuzz
+
+$CXX $CXXFLAGS -DPCLAPI_EXPORTS \
+        -I/src/pcl/build/include -I/src/pcl/common/include \
+        -I/src/pcl/dssdk/include \
+        -I/src/pcl/io/include -isystem /usr/include/eigen3 \
+        -O2 -g -DNDEBUG -fPIC -std=c++14 \
+        -o ply_reader_fuzzer.o -c ply_reader_fuzzer.cpp
+
+$CXX $CXXFLAGS $LIB_FUZZING_ENGINE ply_reader_fuzzer.o \
+        ../../build/lib/libpcl_io.a ../../build/lib/libpcl_io_ply.a \
+        ../../build/lib/libpcl_common.a \
+        /usr/local/lib/libboost_filesystem.a -o $OUT/ply_reader_fuzzer -lm
+
+zip $OUT/ply_reader_fuzzer_seed_corpus.zip $SRC/pcl/test/cube.ply
diff --git a/test/fuzz/ply_reader_fuzzer.cpp b/test/fuzz/ply_reader_fuzzer.cpp
new file mode 100644
index 00000000..dddd5d85
--- /dev/null
+++ b/test/fuzz/ply_reader_fuzzer.cpp
@@ -0,0 +1,23 @@
+#include <pcl/io/ply_io.h>
+#include <pcl/conversions.h>
+#include <pcl/PolygonMesh.h>
+#include <pcl/PCLPointCloud2.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+extern "C" int LLVMFuzzerTestOneInput(const uint8_t *data, size_t size) {
+    pcl::PCLPointCloud2 cloud_blob;
+    pcl::PLYReader reader;
+    char filename[256];
+    sprintf(filename, "/tmp/libfuzzer.%d", getpid());
+
+    FILE *fp = fopen(filename, "wb");
+    if (!fp)
+        return 0;
+    fwrite(data, size, 1, fp);
+    fclose(fp);
+
+    reader.read (filename, cloud_blob); 
+    unlink(filename);
+    return 0;
+}
diff --git a/test/geometry/test_mesh_conversion.cpp b/test/geometry/test_mesh_conversion.cpp
index c1390056..21fd8883 100644
--- a/test/geometry/test_mesh_conversion.cpp
+++ b/test/geometry/test_mesh_conversion.cpp
@@ -52,6 +52,9 @@
 
 #include <type_traits>
 
+using pcl::index_t;
+using pcl::Indices;
+
 ////////////////////////////////////////////////////////////////////////////////
 
 template <class MeshTraitsT>
@@ -89,7 +92,7 @@ class TestMeshConversion : public ::testing::Test
       }
 
       // Faces
-      std::vector <std::uint32_t> face;
+      Indices face;
 
       face.push_back (0);
       face.push_back (1);
@@ -130,8 +133,8 @@ class TestMeshConversion : public ::testing::Test
     }
 
     pcl::PointCloud <pcl::PointXYZRGBNormal> vertices_;
-    std::vector <std::vector <std::uint32_t> >    non_manifold_faces_;
-    std::vector <std::vector <std::uint32_t> >    manifold_faces_;
+    std::vector <Indices>    non_manifold_faces_;
+    std::vector <Indices>    manifold_faces_;
 
   public:
     PCL_MAKE_ALIGNED_OPERATOR_NEW
@@ -163,7 +166,7 @@ TYPED_TEST (TestMeshConversion, HalfEdgeMeshToFaceVertexMesh)
   using VertexIndex = typename Mesh::VertexIndex;
   using VertexIndices = typename Mesh::VertexIndices;
 
-  const std::vector <std::vector <std::uint32_t> > faces =
+  const std::vector <Indices> faces =
       Mesh::IsManifold::value ? this->manifold_faces_ :
                                 this->non_manifold_faces_;
 
@@ -179,7 +182,7 @@ TYPED_TEST (TestMeshConversion, HalfEdgeMeshToFaceVertexMesh)
   for (std::size_t i=0; i<faces.size (); ++i)
   {
     vi.clear ();
-    for (const unsigned int &j : faces [i])
+    for (const auto &j : faces [i])
     {
       vi.push_back (VertexIndex (static_cast <int> (j)));
     }
@@ -268,13 +271,13 @@ TYPED_TEST (TestMeshConversion, FaceVertexMeshToHalfEdgeMesh)
   }
 
   // Check the faces
-  const std::vector <std::vector <std::uint32_t> > expected_faces =
+  const std::vector <Indices> expected_faces =
       Mesh::IsManifold::value ? this->manifold_faces_ :
                                 this->non_manifold_faces_;
 
   ASSERT_EQ (expected_faces.size (), half_edge_mesh.sizeFaces ());
 
-  std::vector <std::uint32_t> converted_face;
+  Indices converted_face;
   for (std::size_t i=0; i<half_edge_mesh.sizeFaces (); ++i)
   {
     VAFC       circ     = half_edge_mesh.getVertexAroundFaceCirculator (FaceIndex (i));
@@ -282,7 +285,7 @@ TYPED_TEST (TestMeshConversion, FaceVertexMeshToHalfEdgeMesh)
     converted_face.clear ();
     do
     {
-      converted_face.push_back (static_cast <std::uint32_t> (circ.getTargetIndex ().get ()));
+      converted_face.push_back (static_cast <index_t> (circ.getTargetIndex ().get ()));
     } while (++circ != circ_end);
 
     EXPECT_TRUE (isCircularPermutation (expected_faces [i], converted_face)) << "Face number " << i;
diff --git a/test/geometry/test_quad_mesh.cpp b/test/geometry/test_quad_mesh.cpp
index 31fad4e4..0a958f6e 100644
--- a/test/geometry/test_quad_mesh.cpp
+++ b/test/geometry/test_quad_mesh.cpp
@@ -345,9 +345,7 @@ TYPED_TEST (TestQuadMesh, NineQuads)
   ASSERT_EQ (9, faces.size ());
   for (std::size_t i=0; i<order_vec.size (); ++i)
   {
-    std::stringstream ss;
-    ss << "Configuration " << i;
-    SCOPED_TRACE (ss.str ());
+    SCOPED_TRACE ("Configuration " + std::to_string(i));
 
     const std::vector <int> order = order_vec [i];
 
diff --git a/test/gpu/octree/perfomance.cpp b/test/gpu/octree/perfomance.cpp
index 245e8695..0d72a553 100644
--- a/test/gpu/octree/perfomance.cpp
+++ b/test/gpu/octree/perfomance.cpp
@@ -100,8 +100,8 @@ TEST(PCL_OctreeGPU, performance)
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_host(new pcl::PointCloud<pcl::PointXYZ>);	
     cloud_host->width = data.points.size();
     cloud_host->height = 1;
-    cloud_host->points.resize (cloud_host->width * cloud_host->height);    
-    std::transform(data.points.begin(), data.points.end(), cloud_host->points.begin(), DataGenerator::ConvPoint<pcl::PointXYZ>());
+    cloud_host->resize (cloud_host->width * cloud_host->height);    
+    std::transform(data.points.cbegin(), data.points.cend(), cloud_host->begin(), DataGenerator::ConvPoint<pcl::PointXYZ>());
 
     float host_octree_resolution = 25.f;    
     
@@ -133,7 +133,7 @@ TEST(PCL_OctreeGPU, performance)
     cv::Octree octree_opencv;
     const static int opencv_octree_points_per_leaf = 32;    
     std::vector<cv::Point3f> opencv_points(data.size());
-    std::transform(data.points.begin(), data.points.end(), opencv_points.begin(), DataGenerator::ConvPoint<cv::Point3f>());
+    std::transform(data.cbegin(), data.cend(), opencv_points.begin(), DataGenerator::ConvPoint<cv::Point3f>());
         
     {        
         ScopeTime t("opencv-build");	        
@@ -145,17 +145,18 @@ TEST(PCL_OctreeGPU, performance)
 
     const int max_answers = 500;
     float dist;
-    int inds;
 
     //host buffers
-    std::vector<int> indeces;
+    std::vector<int> indices;
+    pcl::Indices indices_host;
     std::vector<float> pointRadiusSquaredDistance;
 #ifdef HAVE_OPENCV  
     std::vector<cv::Point3f> opencv_results;
 #endif
 
     //reserve
-    indeces.reserve(data.data_size);
+    indices.reserve(data.data_size);
+    indices_host.reserve(data.data_size);
     pointRadiusSquaredDistance.reserve(data.data_size);
 #ifdef HAVE_OPENCV
     opencv_results.reserve(data.data_size);
@@ -178,14 +179,14 @@ TEST(PCL_OctreeGPU, performance)
     {
         ScopeTime up("gpu-radius-search-{host}-all");	
         for(std::size_t i = 0; i < data.tests_num; ++i)
-            octree_device.radiusSearchHost(data.queries[i], data.radiuses[i], indeces, max_answers);                        
+            octree_device.radiusSearchHost(data.queries[i], data.radiuses[i], indices, max_answers);
     }
 
     {                
         ScopeTime up("host-radius-search-all");	
         for(std::size_t i = 0; i < data.tests_num; ++i)
             octree_host.radiusSearch(pcl::PointXYZ(data.queries[i].x, data.queries[i].y, data.queries[i].z), 
-                data.radiuses[i], indeces, pointRadiusSquaredDistance, max_answers);                        
+                data.radiuses[i], indices_host, pointRadiusSquaredDistance, max_answers);
     }
      
     {
@@ -204,14 +205,14 @@ TEST(PCL_OctreeGPU, performance)
     {
         ScopeTime up("gpu-radius-search-{host}-all");
         for(std::size_t i = 0; i < data.tests_num; ++i)
-            octree_device.radiusSearchHost(data.queries[i], data.shared_radius, indeces, max_answers);                        
+            octree_device.radiusSearchHost(data.queries[i], data.shared_radius, indices, max_answers);
     }
 
     {                
         ScopeTime up("host-radius-search-all");	
         for(std::size_t i = 0; i < data.tests_num; ++i)
             octree_host.radiusSearch(pcl::PointXYZ(data.queries[i].x, data.queries[i].y, data.queries[i].z), 
-                data.radiuses[i], indeces, pointRadiusSquaredDistance, max_answers);                        
+                data.radiuses[i], indices_host, pointRadiusSquaredDistance, max_answers);
     }
      
     {
@@ -223,17 +224,20 @@ TEST(PCL_OctreeGPU, performance)
     std::cout << "======  Approx nearest search =====" << std::endl;
 
     {
-        ScopeTime up("gpu-approx-nearest-batch-all");	        
-        octree_device.approxNearestSearch(queries_device, result_device);                        
+        ScopeTime up("gpu-approx-nearest-batch-all");	       
+        pcl::gpu::Octree::ResultSqrDists sqr_distance;
+        octree_device.approxNearestSearch(queries_device, result_device, sqr_distance);
     }
 
     {        
+        int inds;
         ScopeTime up("gpu-approx-nearest-search-{host}-all");
         for(std::size_t i = 0; i < data.tests_num; ++i)
             octree_device.approxNearestSearchHost(data.queries[i], inds, dist);                        
     }
 
     {                
+        pcl::index_t inds;
         ScopeTime up("host-approx-nearest-search-all");	
         for(std::size_t i = 0; i < data.tests_num; ++i)
             octree_host.approxNearestSearch(data.queries[i], inds, dist);
@@ -248,7 +252,7 @@ TEST(PCL_OctreeGPU, performance)
     {                
         ScopeTime up("host-knn-search-all");	
         for(std::size_t i = 0; i < data.tests_num; ++i)
-            octree_host.nearestKSearch(data.queries[i], k, indeces, pointRadiusSquaredDistance);
+            octree_host.nearestKSearch(data.queries[i], k, indices, pointRadiusSquaredDistance);
     }*/
 }
 
diff --git a/test/gpu/octree/test_approx_nearest.cpp b/test/gpu/octree/test_approx_nearest.cpp
index 4af13542..f6f4c0a5 100644
--- a/test/gpu/octree/test_approx_nearest.cpp
+++ b/test/gpu/octree/test_approx_nearest.cpp
@@ -1,157 +1,137 @@
 /*
- * Software License Agreement (BSD License)
+ * SPDX-License-Identifier: BSD-3-Clause
  *
- *  Copyright (c) 2011, Willow Garage, Inc.
- *  All rights reserved.
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2020-, Open Perception
  *
- *  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.
- *
- *  Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ *  All rights reserved
  */
 
-#if defined _MSC_VER
-    #pragma warning (disable : 4996 4530)
-#endif
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/gpu/octree/octree.hpp>
+#include <pcl/octree/octree_search.h>
+#include <pcl/point_cloud.h>
+#include <pcl/common/distances.h>
 
 #include <gtest/gtest.h>
 
-#include <iostream>
-#include <fstream>
 #include <algorithm>
+#include <fstream>
+#include <iostream>
+#include <array> // for std::array
 
-#if defined _MSC_VER
-    #pragma warning (disable: 4521)
-#endif
-#include <pcl/point_cloud.h>
-#include <pcl/octree/octree_search.h>
-#if defined _MSC_VER
-    #pragma warning (default: 4521)
-#endif
-
-#include <pcl/gpu/octree/octree.hpp>
-#include <pcl/gpu/containers/device_array.h>
-
-#include "data_source.hpp"
-
-using namespace pcl::gpu;
-
-//TEST(PCL_OctreeGPU, DISABLED_approxNearesSearch)
 TEST(PCL_OctreeGPU, approxNearesSearch)
-{   
-    DataGenerator data;
-    data.data_size = 871000;
-    data.tests_num = 10000;
-    data.cube_size = 1024.f;
-    data.max_radius    = data.cube_size/30.f;
-    data.shared_radius = data.cube_size/30.f;
-    data.printParams();
-
-    const float host_octree_resolution = 25.f;
-
-    //generate
-    data();
-        
-    //prepare device cloud
-    pcl::gpu::Octree::PointCloud cloud_device;
-    cloud_device.upload(data.points);
-
-
-    //prepare host cloud
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_host(new pcl::PointCloud<pcl::PointXYZ>);	
-    cloud_host->width = data.points.size();
-    cloud_host->height = 1;
-    cloud_host->points.resize (cloud_host->width * cloud_host->height);    
-    std::transform(data.points.begin(), data.points.end(), cloud_host->points.begin(), DataGenerator::ConvPoint<pcl::PointXYZ>());
-
-    //gpu build 
-    pcl::gpu::Octree octree_device;                
-    octree_device.setCloud(cloud_device);	    
-    octree_device.build();
-    
-    //build host octree
-    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree_host(host_octree_resolution);
-    octree_host.setInputCloud (cloud_host);    
-    octree_host.addPointsFromInputCloud();
-           
-    //upload queries
-    pcl::gpu::Octree::Queries queries_device;
-    queries_device.upload(data.queries);
-    
-        
-    //prepare output buffers on device
-    pcl::gpu::NeighborIndices result_device(data.tests_num, 1);
-    std::vector<int> result_host_pcl(data.tests_num);
-    std::vector<int> result_host_gpu(data.tests_num);
-    std::vector<float> dists_pcl(data.tests_num);
-    std::vector<float> dists_gpu(data.tests_num);
-    
-    //search GPU shared
-    octree_device.approxNearestSearch(queries_device, result_device);
-
-    std::vector<int> downloaded;
-    result_device.data.download(downloaded);
-                
-    for(std::size_t i = 0; i < data.tests_num; ++i)
-    {
-        octree_host.approxNearestSearch(data.queries[i], result_host_pcl[i], dists_pcl[i]);
-        octree_device.approxNearestSearchHost(data.queries[i], result_host_gpu[i], dists_gpu[i]);
-    }
-
-    ASSERT_EQ ( ( downloaded == result_host_gpu ), true );
-
-    int count_gpu_better = 0;
-    int count_pcl_better = 0;
-    float diff_pcl_better = 0;
-    for(std::size_t i = 0; i < data.tests_num; ++i)
-    {
-        float diff = dists_pcl[i] - dists_gpu[i];
-        bool gpu_better = diff > 0;
-
-        ++(gpu_better ? count_gpu_better : count_pcl_better);
-
-        if (!gpu_better)
-            diff_pcl_better +=std::abs(diff);
-    }
-
-    diff_pcl_better /=count_pcl_better;
-
-    std::cout << "count_gpu_better: " << count_gpu_better << std::endl;
-    std::cout << "count_pcl_better: " << count_pcl_better << std::endl;
-    std::cout << "avg_diff_pcl_better: " << diff_pcl_better << std::endl;    
+{
 
+  /*
+  the test points create an octree with bounds (-1, -1, -1) and (1, 1, 1).
+  point q, represents a query point
+  ------------------------------------
+  |                |                 |
+  |                |                 |
+  |                |                 |
+  |                |                 |
+  |                |                 |
+  |----------------------------------|
+  | x     | q      |                 |
+  |       |        |                 |
+  |-------|--------|                 |
+  |       | y      |                 |
+  |       |        |                 |
+  ------------------------------------
+  the final two point are positioned such that point 'x' is farther from query point 'q'
+  than 'y', but the voxel containing 'x' is closer to  'q' than the voxel containing 'y'
+  */
+
+  const std::array<pcl::PointXYZ, 10> coords{
+      pcl::PointXYZ{-1.f, -1.f, -1.f},
+      pcl::PointXYZ{-1.f, -1.f, 1.f},
+      pcl::PointXYZ{-1.f, 1.f, -1.f},
+      pcl::PointXYZ{-1.f, 1.f, 1.f},
+      pcl::PointXYZ{1.f, -1.f, -1.f},
+      pcl::PointXYZ{1.f, -1.f, 1.f},
+      pcl::PointXYZ{1.f, 1.f, -1.f},
+      pcl::PointXYZ{1.f, 1.f, 1.f},
+      pcl::PointXYZ{-0.9f, -0.2f, -0.75f},
+      pcl::PointXYZ{-0.4f, -0.6f, -0.75f},
+  };
+
+  // While the GPU implementation has a fixed depth of 10 levels, octree depth in the
+  // CPU implementation can vary based on the leaf size set by the user, which can
+  // affect the results. Therefore results would only tally if depths match.
+  //generate custom pointcloud
+  constexpr pcl::index_t point_size = 1000 * coords.size();
+  auto cloud = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(point_size, 1);
+
+  // copy chunks of 10 points at the same time
+  for (auto it = cloud->begin(); it != cloud->cend(); it += coords.size())
+    std::copy(coords.cbegin(), coords.cend(), it);
+
+  const std::vector<pcl::PointXYZ> queries = {
+      {-0.4, -0.2, -0.75}, // should be different across CPU and GPU if different
+                           // traversal methods are used
+      {-0.6, -0.2, -0.75}, // should be same across CPU and GPU
+      {1.1, 1.1, 1.1},     // out of range query
+  };
+
+  // prepare device cloud
+  pcl::gpu::Octree::PointCloud cloud_device;
+  cloud_device.upload(cloud->points);
+
+  // gpu build
+  pcl::gpu::Octree octree_device;
+  octree_device.setCloud(cloud_device);
+  octree_device.build();
+
+  // build host octree
+  constexpr float host_octree_resolution = 0.05;
+  pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree_host(
+      host_octree_resolution);
+  octree_host.setInputCloud(cloud);
+  octree_host.addPointsFromInputCloud();
+
+  // upload queries
+  pcl::gpu::Octree::Queries queries_device;
+  queries_device.upload(queries);
+
+  // prepare output buffers on device
+  pcl::gpu::NeighborIndices result_device(queries.size(), 1);
+  pcl::Indices result_host_pcl(queries.size());
+  std::vector<int> result_host_gpu(queries.size());
+  std::vector<float> dists_pcl(queries.size());
+  std::vector<float> dists_gpu(queries.size());
+  pcl::gpu::Octree::ResultSqrDists dists_device;
+
+  // search GPU shared
+  octree_device.approxNearestSearch(queries_device, result_device, dists_device);
+  std::vector<int> downloaded;
+  std::vector<float> dists_device_downloaded;
+  result_device.data.download(downloaded);
+  dists_device.download(dists_device_downloaded);
+
+  for (size_t i = 0; i < queries.size(); ++i) {
+    octree_host.approxNearestSearch(queries[i], result_host_pcl[i], dists_pcl[i]);
+    octree_device.approxNearestSearchHost(queries[i], result_host_gpu[i], dists_gpu[i]);
+  }
+
+  ASSERT_EQ(downloaded, result_host_gpu);
+
+  const std::array<float, 3> expected_sqr_dists {pcl::squaredEuclideanDistance(coords[8], queries[0]),
+                                                  pcl::squaredEuclideanDistance(coords[8], queries[1]),
+                                                  pcl::squaredEuclideanDistance(coords[7], queries[2]) };
+
+  for (size_t i = 0; i < queries.size(); ++i) {
+    ASSERT_EQ(dists_pcl[i], dists_gpu[i]);
+    ASSERT_NEAR(dists_gpu[i], dists_device_downloaded[i], 0.001);
+    ASSERT_NEAR(dists_device_downloaded[i], expected_sqr_dists[i], 0.001);
+  }
 }
 
 /* ---[ */
 int
-main (int argc, char** argv)
+main(int argc, char** argv)
 {
-  testing::InitGoogleTest (&argc, argv);
-  return (RUN_ALL_TESTS ());
+  testing::InitGoogleTest(&argc, argv);
+  return (RUN_ALL_TESTS());
 }
 /* ]--- */
-
diff --git a/test/gpu/octree/test_host_radius_search.cpp b/test/gpu/octree/test_host_radius_search.cpp
index 15a05f3d..a16c6d59 100644
--- a/test/gpu/octree/test_host_radius_search.cpp
+++ b/test/gpu/octree/test_host_radius_search.cpp
@@ -44,6 +44,7 @@
     #pragma warning (disable: 4521)
 #endif
     
+#include <pcl/pcl_tests.h> // for EXPECT_EQ_VECTORS
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl/octree/octree_search.h>
@@ -84,7 +85,7 @@ TEST(PCL_OctreeGPU, hostRadiusSearch)
     cloud_host->width = data.points.size();
     cloud_host->height = 1;
     cloud_host->points.resize (cloud_host->width * cloud_host->height);
-    std::transform(data.points.begin(), data.points.end(),  cloud_host->points.begin(), DataGenerator::ConvPoint<pcl::PointXYZ>());
+    std::transform(data.points.cbegin(), data.points.cend(),  cloud_host->begin(), DataGenerator::ConvPoint<pcl::PointXYZ>());
     
     // build device octree
     pcl::gpu::Octree octree_device;                
@@ -115,14 +116,14 @@ TEST(PCL_OctreeGPU, hostRadiusSearch)
         
         //search host
         std::vector<float> dists;
-        std::vector<int> results_host;                
+        pcl::Indices results_host;
         octree_host.radiusSearch(pcl::PointXYZ(data.queries[i].x, data.queries[i].y, data.queries[i].z), data.radiuses[i], results_host, dists);                        
         
         std::sort(results_host_gpu.begin(), results_host_gpu.end());
         std::sort(results_host.begin(), results_host.end());
 
-        ASSERT_EQ ( (results_host_gpu == results_host     ), true );
-        ASSERT_EQ ( (results_host_gpu == data.bfresutls[i]), true );                
+        pcl::test::EXPECT_EQ_VECTORS (results_host_gpu, results_host);
+        pcl::test::EXPECT_EQ_VECTORS (results_host_gpu, data.bfresutls[i]);
         sizes.push_back(results_host.size());      
     }    
 
diff --git a/test/gpu/octree/test_knn_search.cpp b/test/gpu/octree/test_knn_search.cpp
index 5e11bdc9..c21ba65c 100644
--- a/test/gpu/octree/test_knn_search.cpp
+++ b/test/gpu/octree/test_knn_search.cpp
@@ -96,7 +96,7 @@ TEST(PCL_OctreeGPU, exactNeighbourSearch)
     cloud_host->width = data.points.size();
     cloud_host->height = 1;
     cloud_host->points.resize (cloud_host->width * cloud_host->height);    
-    std::transform(data.points.begin(), data.points.end(), cloud_host->points.begin(), DataGenerator::ConvPoint<pcl::PointXYZ>());
+    std::transform(data.points.cbegin(), data.points.cend(), cloud_host->begin(), DataGenerator::ConvPoint<pcl::PointXYZ>());
 
     //gpu build 
     pcl::gpu::Octree octree_device;                
@@ -111,28 +111,32 @@ TEST(PCL_OctreeGPU, exactNeighbourSearch)
     //upload queries
     pcl::gpu::Octree::Queries queries_device;
     queries_device.upload(data.queries);
-            
-    //prepare output buffers on device
-    pcl::gpu::NeighborIndices result_device(data.tests_num, k);    
 
     //prepare output buffers on host
-    std::vector<std::vector<  int> > result_host(data.tests_num);   
+    std::vector<pcl::Indices > result_host(data.tests_num);
     std::vector<std::vector<float> >  dists_host(data.tests_num);
     for(std::size_t i = 0; i < data.tests_num; ++i)
     {
         result_host[i].reserve(k);
         dists_host[i].reserve(k);
     }
-        
+
+    //prepare output buffers on device
+    pcl::gpu::NeighborIndices result_device;
+    pcl::gpu::Octree::ResultSqrDists result_sqr_distances;
+
     //search GPU shared
     {
         pcl::ScopeTime time("1nn-gpu");
-        octree_device.nearestKSearchBatch(queries_device, k, result_device);
+        octree_device.nearestKSearchBatch(queries_device, k, result_device, result_sqr_distances);
     }
 
-    std::vector<int> downloaded, downloaded_cur;
+    std::vector<int> downloaded;
     result_device.data.download(downloaded);
-                 
+
+    std::vector<float> downloaded_sqr_dists;
+    result_sqr_distances.download(downloaded_sqr_dists);
+
     {
         pcl::ScopeTime time("1nn-cpu");
         for(std::size_t i = 0; i < data.tests_num; ++i)
@@ -143,14 +147,18 @@ TEST(PCL_OctreeGPU, exactNeighbourSearch)
     for(std::size_t i = 0; i < data.tests_num; ++i)    
     {           
         //std::cout << i << std::endl;
-        std::vector<int>&   results_host_cur = result_host[i];
+        const auto&   results_host_cur = result_host[i];
         std::vector<float>&   dists_host_cur = dists_host[i];
                 
         int beg = i * k;
         int end = beg + k;
 
-        downloaded_cur.assign(downloaded.begin() + beg, downloaded.begin() + end);
-        
+        const auto beg_dist2 = downloaded_sqr_dists.cbegin() + beg;
+        const auto end_dist2 = downloaded_sqr_dists.cbegin() + end;
+
+        const std::vector<int> downloaded_cur (downloaded.cbegin() + beg, downloaded.cbegin() + end);
+        const std::vector<float> downloaded_sqr_dists_cur (beg_dist2, end_dist2);
+
         std::vector<PriorityPair> pairs_host;
         std::vector<PriorityPair> pairs_gpu;
         for(int n = 0; n < k; ++n)
@@ -162,9 +170,7 @@ TEST(PCL_OctreeGPU, exactNeighbourSearch)
 
             PriorityPair gpu;
             gpu.index = downloaded_cur[n];
-
-            float dist = (data.queries[i].getVector3fMap() - data.points[gpu.index].getVector3fMap()).norm();
-            gpu.dist2 = dist * dist;
+            gpu.dist2 = downloaded_sqr_dists_cur[n];
             pairs_gpu.push_back(gpu);
         }
         
diff --git a/test/io/CMakeLists.txt b/test/io/CMakeLists.txt
index 4443fb9b..303ce711 100644
--- a/test/io/CMakeLists.txt
+++ b/test/io/CMakeLists.txt
@@ -52,4 +52,5 @@ PCL_ADD_TEST(buffers test_buffers
 
 PCL_ADD_TEST(io_octree_compression test_octree_compression
         FILES test_octree_compression.cpp
-        LINK_WITH pcl_gtest pcl_common pcl_io pcl_octree)
+        LINK_WITH pcl_gtest pcl_common pcl_io pcl_octree
+        ARGUMENTS "${PCL_SOURCE_DIR}/test/milk_color.pcd")
diff --git a/test/io/test_grabbers.cpp b/test/io/test_grabbers.cpp
index 4394e944..a7c794aa 100644
--- a/test/io/test_grabbers.cpp
+++ b/test/io/test_grabbers.cpp
@@ -7,6 +7,8 @@
 #include <string>
 #include <thread>
 #include <vector>
+#include <boost/filesystem.hpp> // for directory_iterator, extension
+#include <boost/algorithm/string/case_conv.hpp> // for to_upper_copy
 
 using namespace std::chrono_literals;
 
diff --git a/test/io/test_io.cpp b/test/io/test_io.cpp
index 07d9c609..5e46e856 100644
--- a/test/io/test_io.cpp
+++ b/test/io/test_io.cpp
@@ -121,7 +121,7 @@ TEST (PCL, ComplexPCDFileASCII)
   int fpfh_idx = pcl::getFieldIndex (blob, "fpfh");
   EXPECT_EQ (fpfh_idx, 0);
   float val[33];
-  for (std::size_t i = 0; i < blob.fields[fpfh_idx].count; ++i)
+  for (uindex_t i = 0; i < blob.fields[fpfh_idx].count; ++i)
     memcpy (&val[i], &blob.data[0 * blob.point_step + blob.fields[fpfh_idx + 0].offset + i * sizeof (float)], sizeof (float));
 
   EXPECT_EQ (val[0], 0); 
@@ -271,8 +271,8 @@ TEST (PCL, ConcatenatePoints)
   cloud_a.width  = 5;
   cloud_b.width  = 3;
   cloud_a.height = cloud_b.height = 1;
-  cloud_a.points.resize (cloud_a.width * cloud_a.height);
-  cloud_b.points.resize (cloud_b.width * cloud_b.height);
+  cloud_a.resize (cloud_a.width * cloud_a.height);
+  cloud_b.resize (cloud_b.width * cloud_b.height);
 
   for (auto &point : cloud_a.points)
   {
@@ -319,8 +319,8 @@ TEST (PCL, ConcatenateFields)
   // Fill in the cloud data
   cloud_a.width  = cloud_b.width  = 5;
   cloud_a.height = cloud_b.height = 1;
-  cloud_a.points.resize (cloud_a.width * cloud_a.height);
-  cloud_b.points.resize (cloud_b.width * cloud_b.height);
+  cloud_a.resize (cloud_a.width * cloud_a.height);
+  cloud_b.resize (cloud_b.width * cloud_b.height);
 
   for (auto& point: cloud_a)
   {
@@ -360,7 +360,7 @@ TEST (PCL, IO)
 
   cloud.width  = 640;
   cloud.height = 480;
-  cloud.points.resize (cloud.width * cloud.height);
+  cloud.resize (cloud.width * cloud.height);
   cloud.is_dense = true;
 
   srand (static_cast<unsigned int> (time (nullptr)));
@@ -593,7 +593,7 @@ TEST (PCL, IO)
   EXPECT_FLOAT_EQ (cloud[nr_p - 1].z, last.z);    // test for fromPCLPointCloud2 ()
   EXPECT_FLOAT_EQ (float (cloud[nr_p - 1].intensity), float (last.intensity)); // test for fromPCLPointCloud2 ()
 
-  std::vector<int> indices (cloud.width * cloud.height / 2);
+  pcl::Indices indices (cloud.width * cloud.height / 2);
   for (int i = 0; i < static_cast<int> (indices.size ()); ++i) indices[i] = i;
   // Save as ASCII
   try
@@ -670,7 +670,7 @@ TEST (PCL, PCDReaderWriter)
 
   cloud.width  = 640;
   cloud.height = 480;
-  cloud.points.resize (cloud.width * cloud.height);
+  cloud.resize (cloud.width * cloud.height);
   cloud.is_dense = true;
 
   srand (static_cast<unsigned int> (time (nullptr)));
@@ -735,7 +735,7 @@ TEST (PCL, PCDReaderWriter)
 TEST (PCL, PCDReaderWriterASCIIColorPrecision)
 {
   PointCloud<PointXYZRGB> cloud;
-  cloud.points.reserve (256 / 4 * 256 / 4 * 256 / 4 * 256 / 16);
+  cloud.reserve (256 / 4 * 256 / 4 * 256 / 4 * 256 / 16);
   for (std::size_t r_i = 0; r_i < 256; r_i += 5)
     for (std::size_t g_i = 0; g_i < 256; g_i += 5)
       for (std::size_t b_i = 0; b_i < 256; b_i += 5)
@@ -785,7 +785,7 @@ TEST (PCL, ASCIIRead)
 
   cloud.width  = 300;
   cloud.height = 1;
-  cloud.points.resize (cloud.width * cloud.height);
+  cloud.resize (cloud.width * cloud.height);
   cloud.is_dense = true;
 
 
@@ -937,7 +937,7 @@ TEST (PCL, ExtendedIO)
 {
   PointCloud<PointXYZFPFH33> cloud;
   cloud.width = 2; cloud.height = 1;
-  cloud.points.resize (2);
+  cloud.resize (2);
 
   cloud[0].x = cloud[0].y = cloud[0].z = 1;
   cloud[1].x = cloud[1].y = cloud[1].z = 2;
@@ -948,7 +948,7 @@ TEST (PCL, ExtendedIO)
   }
 
   savePCDFile ("v.pcd", cloud);
-  cloud.points.clear ();
+  cloud.clear ();
   loadPCDFile ("v.pcd", cloud);
 
   EXPECT_EQ (cloud.width, 2);
@@ -972,7 +972,7 @@ TEST (PCL, ExtendedIO)
 TEST (PCL, EigenConversions)
 {
   PointCloud<PointXYZ> cloud;
-  cloud.points.resize (5);
+  cloud.resize (5);
 
   for (std::size_t i = 0; i < cloud.size (); ++i)
     cloud[i].x = cloud[i].y = cloud[i].z = static_cast<float> (i);
@@ -1024,8 +1024,8 @@ TEST (PCL, CopyPointCloud)
   // Fill in the cloud data
   cloud_a.width  = cloud_b.width  = 3;
   cloud_a.height = cloud_b.height = 1;
-  cloud_a.points.resize (cloud_a.width * cloud_a.height);
-  cloud_b.points.resize (cloud_b.width * cloud_b.height);
+  cloud_a.resize (cloud_a.width * cloud_a.height);
+  cloud_b.resize (cloud_b.width * cloud_b.height);
 
   for (std::size_t i = 0; i < cloud_a.size (); ++i)
   {
@@ -1063,7 +1063,7 @@ TEST (PCL, LZF)
   PointCloud<PointXYZ> cloud, cloud2;
   cloud.width  = 640;
   cloud.height = 480;
-  cloud.points.resize (cloud.width * cloud.height);
+  cloud.resize (cloud.width * cloud.height);
   cloud.is_dense = true;
 
   srand (static_cast<unsigned int> (time (nullptr)));
@@ -1121,7 +1121,7 @@ TEST (PCL, LZFExtended)
   PointCloud<PointXYZRGBNormal> cloud, cloud2;
   cloud.width  = 640;
   cloud.height = 480;
-  cloud.points.resize (cloud.width * cloud.height);
+  cloud.resize (cloud.width * cloud.height);
   cloud.is_dense = true;
 
   srand (static_cast<unsigned int> (time (nullptr)));
@@ -1174,7 +1174,7 @@ TEST (PCL, LZFInMem)
   PointCloud<PointXYZRGBNormal> cloud;
   cloud.width  = 640;
   cloud.height = 480;
-  cloud.points.resize (cloud.width * cloud.height);
+  cloud.resize (cloud.width * cloud.height);
   cloud.is_dense = true;
 
   srand (static_cast<unsigned int> (time (nullptr)));
@@ -1245,7 +1245,7 @@ TEST (PCL, Locale)
     PointCloud<PointXYZ> cloud, cloud2;
     cloud.width  = 640;
     cloud.height = 480;
-    cloud.points.resize (cloud.width * cloud.height);
+    cloud.resize (cloud.width * cloud.height);
     cloud.is_dense = true;
 
     srand (static_cast<unsigned int> (time (nullptr)));
diff --git a/test/io/test_octree_compression.cpp b/test/io/test_octree_compression.cpp
index 3eb4beb6..d268e45d 100644
--- a/test/io/test_octree_compression.cpp
+++ b/test/io/test_octree_compression.cpp
@@ -48,107 +48,111 @@ int total_runs = 0;
 char* pcd_file;
 
 #define MAX_POINTS 10000.0
-#define MAX_XYZ 1024.0
 #define MAX_COLOR 255
-#define NUMBER_OF_TEST_RUNS 2
+#define NUMBER_OF_TEST_RUNS 3
+
+template<typename PointT> inline PointT generateRandomPoint(const float MAX_XYZ);
+
+template<> inline pcl::PointXYZRGBA generateRandomPoint(const float MAX_XYZ) {
+  return pcl::PointXYZRGBA(static_cast<float> (MAX_XYZ * rand() / RAND_MAX),
+                           static_cast<float> (MAX_XYZ * rand() / RAND_MAX),
+                           static_cast<float> (MAX_XYZ * rand() / RAND_MAX),
+                           static_cast<int> (MAX_COLOR * rand() / RAND_MAX),
+                           static_cast<int> (MAX_COLOR * rand() / RAND_MAX),
+                           static_cast<int> (MAX_COLOR * rand() / RAND_MAX),
+                           static_cast<int> (MAX_COLOR * rand() / RAND_MAX));
+}
+
+template<> inline pcl::PointXYZ generateRandomPoint(const float MAX_XYZ) {
+  return pcl::PointXYZ(static_cast<float> (MAX_XYZ * rand() / RAND_MAX),
+                       static_cast<float> (MAX_XYZ * rand() / RAND_MAX),
+                       static_cast<float> (MAX_XYZ * rand() / RAND_MAX));
+}
+
+template<typename PointT> inline
+typename pcl::PointCloud<PointT>::Ptr generateRandomCloud(const float MAX_XYZ) {
+  // empty point cloud hangs decoder
+  const unsigned int point_count = 1 + (MAX_POINTS - 1) * rand() / RAND_MAX;
+  // create shared pointcloud instances
+  typename pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
+  for (unsigned int point = 0; point < point_count; point++) {
+    cloud->push_back(generateRandomPoint<PointT>(MAX_XYZ));
+  }
+  return cloud;
+}
 
-TEST (PCL, OctreeDeCompressionRandomPointXYZRGBA)
+template<typename PointT>
+class OctreeDeCompressionTest : public testing::Test {};
+
+using TestTypes = ::testing::Types<pcl::PointXYZ, pcl::PointXYZRGBA>;
+TYPED_TEST_SUITE(OctreeDeCompressionTest, TestTypes);
+
+TYPED_TEST (OctreeDeCompressionTest, RandomClouds)
 {
   srand(static_cast<unsigned int> (time(NULL)));
-
+  for (const double MAX_XYZ : {1.0, 1024.0}) { // Small clouds, large clouds
     // iterate over all pre-defined compression profiles
-  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<pcl::PointXYZRGBA> pointcloud_encoder((pcl::io::compression_Profiles_e) compression_profile, false);
-    pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA> pointcloud_decoder;
-    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZRGBA>());
-    // iterate over runs
-    for (int test_idx = 0; test_idx < NUMBER_OF_TEST_RUNS; test_idx++, total_runs++)
-    {
-      try
+    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<TypeParam> pointcloud_encoder((pcl::io::compression_Profiles_e) compression_profile, false);
+      pcl::io::OctreePointCloudCompression<TypeParam> pointcloud_decoder;
+      typename pcl::PointCloud<TypeParam>::Ptr cloud_out(new pcl::PointCloud<TypeParam>());
+      // iterate over runs
+      for (int test_idx = 0; test_idx < NUMBER_OF_TEST_RUNS; test_idx++, total_runs++)
       {
-        int point_count = MAX_POINTS * rand() / RAND_MAX;
-        if (point_count < 1)
-        { // empty point cloud hangs decoder
-          total_runs--;
-          continue;
-        }
-        // create shared pointcloud instances
-        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>());
-        // assign input point clouds to octree
-        // create random point cloud
-        for (int point = 0; point < point_count; point++)
-        {
-          // gereate a random point
-          pcl::PointXYZRGBA new_point;
-          new_point.x = static_cast<float> (MAX_XYZ * rand() / RAND_MAX);
-          new_point.y = static_cast<float> (MAX_XYZ * rand() / RAND_MAX),
-          new_point.z = static_cast<float> (MAX_XYZ * rand() / RAND_MAX);
-          new_point.r = static_cast<int> (MAX_COLOR * rand() / RAND_MAX);
-          new_point.g = static_cast<int> (MAX_COLOR * rand() / RAND_MAX);
-          new_point.b = static_cast<int> (MAX_COLOR * rand() / RAND_MAX);
-          new_point.a = static_cast<int> (MAX_COLOR * rand() / RAND_MAX);
-          // OctreePointCloudPointVector can store all points..
-          cloud->push_back(new_point);
-        }
+        auto cloud = generateRandomCloud<TypeParam>(MAX_XYZ);
+        EXPECT_EQ(cloud->height, 1);
 
-//        std::cout << "Run: " << total_runs << " compression profile:" << compression_profile << " point_count: " << point_count;
+//          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);
-        EXPECT_GT((int)cloud_out->width, 0) << "decoded PointCloud width <= 0";
-        EXPECT_GT((int)cloud_out->height, 0) << " decoded PointCloud height <= 0 ";
-      }
-      catch (std::exception& e)
-      {
-        std::cout << e.what() << std::endl;
-      }
-    } // runs
-  } // compression profiles
+        if (pcl::io::compressionProfiles_[compression_profile].doVoxelGridDownSampling) {
+          EXPECT_GT(cloud_out->width, 0);
+          EXPECT_LE(cloud_out->width, cloud->width) << "cloud width after encoding and decoding greater than before. Profile: " << compression_profile;
+        }
+        else {
+          EXPECT_EQ(cloud_out->width, cloud->width) << "cloud width after encoding and decoding not the same. Profile: " << compression_profile;
+        }
+        EXPECT_EQ(cloud_out->height, 1) << "cloud height after encoding and decoding should be 1 (as before). Profile: " << compression_profile;
+      } // runs
+    } // compression profiles
+  } // small clouds, large clouds
 } // TEST
 
-TEST (PCL, OctreeDeCompressionRandomPointXYZ)
+TEST (PCL, OctreeDeCompressionRandomPointXYZRGBASameCloud)
 {
+  // Generate a random cloud. Put it into the encoder several times and make
+  // sure that the decoded cloud has correct width and height each time.
+  const double MAX_XYZ = 1.0;
   srand(static_cast<unsigned int> (time(NULL)));
-
   // iterate over all pre-defined compression profiles
   for (int compression_profile = pcl::io::LOW_RES_ONLINE_COMPRESSION_WITHOUT_COLOR;
-        compression_profile != pcl::io::COMPRESSION_PROFILE_COUNT; ++compression_profile)
-  {
+    compression_profile != pcl::io::COMPRESSION_PROFILE_COUNT; ++compression_profile) {
     // instantiate point cloud compression encoder/decoder
-    pcl::io::OctreePointCloudCompression<pcl::PointXYZ> pointcloud_encoder((pcl::io::compression_Profiles_e) compression_profile, false);
-    pcl::io::OctreePointCloudCompression<pcl::PointXYZ> pointcloud_decoder;
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>());
-    // loop over runs
+    pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA> pointcloud_encoder((pcl::io::compression_Profiles_e) compression_profile, false);
+    pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA> pointcloud_decoder;
+
+    auto cloud = generateRandomCloud<pcl::PointXYZRGBA>(MAX_XYZ);
+    EXPECT_EQ(cloud->height, 1);
+
+    // iterate over runs
     for (int test_idx = 0; test_idx < NUMBER_OF_TEST_RUNS; test_idx++, total_runs++)
     {
-      int point_count = MAX_POINTS * rand() / RAND_MAX;
-      // create shared pointcloud instances
-      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
-      // assign input point clouds to octree
-      // create random point cloud
-      for (int point = 0; point < point_count; point++)
-      {
-        // generate a random point
-        pcl::PointXYZ new_point(static_cast<float> (MAX_XYZ * rand() / RAND_MAX),
-                               static_cast<float> (MAX_XYZ * rand() / RAND_MAX),
-                               static_cast<float> (MAX_XYZ * rand() / RAND_MAX));
-        cloud->push_back(new_point);
-      }
-//      std::cout << "Run: " << total_runs << " compression profile:" << compression_profile << " point_count: " << point_count;
+//          std::cout << "Run: " << total_runs << " compression profile:" << compression_profile << " point_count: " << point_count;
       std::stringstream compressed_data;
-      try
-      { // decodePointCloud() throws exceptions on errors
-        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 ";
+      pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZRGBA>());
+      pointcloud_encoder.encodePointCloud(cloud, compressed_data);
+      pointcloud_decoder.decodePointCloud(compressed_data, cloud_out);
+      if (pcl::io::compressionProfiles_[compression_profile].doVoxelGridDownSampling) {
+        EXPECT_GT(cloud_out->width, 0);
+        EXPECT_LE(cloud_out->width, cloud->width) << "cloud width after encoding and decoding greater than before. Profile: " << compression_profile;
       }
-      catch (std::exception& e)
-      {
-        std::cout << e.what() << std::endl;
+      else {
+        EXPECT_EQ(cloud_out->width, cloud->width) << "cloud width after encoding and decoding not the same. Profile: " << compression_profile;
       }
+      EXPECT_EQ(cloud_out->height, 1) << "cloud height after encoding and decoding should be 1 (as before). Profile: " << compression_profile;
     } // runs
   } // compression profiles
 } // TEST
@@ -159,36 +163,34 @@ TEST(PCL, OctreeDeCompressionFile)
 
   // load point cloud from file, when present
   if (pcd_file == NULL) return;
-    int rv = pcl::io::loadPCDFile(pcd_file, *input_cloud_ptr);
-    float voxel_sizes[] = { 0.1, 0.01 };
+  int rv = pcl::io::loadPCDFile(pcd_file, *input_cloud_ptr);
+  float voxel_sizes[] = { 0.1, 0.01 };
 
-    EXPECT_EQ(rv, 0) << " loadPCDFile " << pcd_file;
-    EXPECT_GT((int) input_cloud_ptr->width , 0) << "invalid point cloud width from " << pcd_file;
-    EXPECT_GT((int) input_cloud_ptr->height, 0) << "invalid point cloud height from " << pcd_file;
+  EXPECT_EQ(rv, 0) << " loadPCDFile " << pcd_file;
+  EXPECT_GT(input_cloud_ptr->width , 0) << "invalid point cloud width from " << pcd_file;
+  EXPECT_GT(input_cloud_ptr->height, 0) << "invalid point cloud height from " << pcd_file;
 
-    // iterate over compression profiles
-    for (int compression_profile = pcl::io::LOW_RES_ONLINE_COMPRESSION_WITHOUT_COLOR;
-         compression_profile != pcl::io::COMPRESSION_PROFILE_COUNT; ++compression_profile) {
+  // iterate over compression profiles
+  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<pcl::PointXYZRGB>* PointCloudEncoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGB>((pcl::io::compression_Profiles_e) compression_profile, false);
-    pcl::io::OctreePointCloudCompression<pcl::PointXYZRGB>* PointCloudDecoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGB>();
+    pcl::io::OctreePointCloudCompression<pcl::PointXYZRGB> PointCloudEncoder((pcl::io::compression_Profiles_e) compression_profile, false);
+    pcl::io::OctreePointCloudCompression<pcl::PointXYZRGB> PointCloudDecoder;
 
     // iterate over various voxel sizes
     for (std::size_t i = 0; i < sizeof(voxel_sizes)/sizeof(voxel_sizes[0]); i++) {
       pcl::octree::OctreePointCloud<pcl::PointXYZRGB> octree(voxel_sizes[i]);
-      pcl::PointCloud<pcl::PointXYZRGB>::Ptr octree_out(new pcl::PointCloud<pcl::PointXYZRGB>());
+      pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZRGB>());
       octree.setInputCloud((*input_cloud_ptr).makeShared());
       octree.addPointsFromInputCloud();
 
       std::stringstream compressedData;
-      PointCloudEncoder->encodePointCloud(octree.getInputCloud(), compressedData);
-      PointCloudDecoder->decodePointCloud(compressedData, octree_out);
-      EXPECT_GT((int)octree_out->width, 0) << "decoded PointCloud width <= 0";
-      EXPECT_GT((int)octree_out->height, 0) << " decoded PointCloud height <= 0 ";
+      PointCloudEncoder.encodePointCloud(octree.getInputCloud(), compressedData);
+      PointCloudDecoder.decodePointCloud(compressedData, cloud_out);
+      EXPECT_GT(cloud_out->width, 0) << "decoded PointCloud width <= 0";
+      EXPECT_GT(cloud_out->height, 0) << " decoded PointCloud height <= 0 ";
       total_runs++;
     }
-    delete PointCloudDecoder;
-    delete PointCloudEncoder;
   }
 }
 
diff --git a/test/io/test_ply_io.cpp b/test/io/test_ply_io.cpp
index 9756b56d..f743f31f 100644
--- a/test/io/test_ply_io.cpp
+++ b/test/io/test_ply_io.cpp
@@ -499,7 +499,7 @@ TEST_F (PLYTest, NoEndofLine)
   pcl::PLYReader Reader;
   Reader.read(PLYTest::mesh_file_ply_, cloud);
 
-  ASSERT_EQ (cloud.empty(), false);
+  ASSERT_FALSE (cloud.empty());
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -529,7 +529,7 @@ TEST_F (PLYTest, CommentAtTheEnd)
   pcl::PLYReader Reader;
   Reader.read(PLYTest::mesh_file_ply_, cloud);
 
-  ASSERT_EQ (cloud.empty(), false);
+  ASSERT_FALSE (cloud.empty());
 }
 
 /* ---[ */
diff --git a/test/io/test_ply_mesh_io.cpp b/test/io/test_ply_mesh_io.cpp
index 18d8f07a..106a0362 100644
--- a/test/io/test_ply_mesh_io.cpp
+++ b/test/io/test_ply_mesh_io.cpp
@@ -279,6 +279,38 @@ TEST (PCL, PLYPolygonMeshColoredIO)
   remove ("test_mesh_rgba_binary.ply");
 }
 
+
+TEST (PCL, PLYPolygonMeshUintIndices)
+{
+  std::ofstream fs;
+  fs.open ("mesh_uint_indices.ply");
+  fs <<
+    "ply\n"
+    "format ascii 1.0\n"
+    "element vertex 3\n"
+    "property float x\n"
+    "property float y\n"
+    "property float z\n"
+    "element face 1\n"
+    "property list uchar uint vertex_indices\n"
+    "end_header\n"
+    "0.0 0.0 0.0\n"
+    "1.0 0.0 0.0\n"
+    "1.0 1.1 0.0\n"
+    "3 2 0 1\n";
+  fs.close();
+  pcl::PolygonMesh mesh;
+  int const res = pcl::io::loadPLYFile("mesh_uint_indices.ply", mesh);
+  EXPECT_NE (res, -1);
+  EXPECT_EQ (mesh.cloud.width, 3);
+  EXPECT_EQ (mesh.cloud.height, 1);
+  EXPECT_EQ (mesh.polygons.size(), 1);
+  EXPECT_EQ (mesh.polygons.front().vertices.size(), 3);
+
+  remove("mesh_uint_indices.ply");
+}
+
+
 /* ---[ */
 int
   main (int argc, char** argv)
diff --git a/test/io/test_point_cloud_image_extractors.cpp b/test/io/test_point_cloud_image_extractors.cpp
index 51000ce8..adf02cc3 100644
--- a/test/io/test_point_cloud_image_extractors.cpp
+++ b/test/io/test_point_cloud_image_extractors.cpp
@@ -54,7 +54,7 @@ TEST (PCL, PointCloudImageExtractorFromNormalField)
   cloud.width = 2;
   cloud.height = 2;
   cloud.is_dense = true;
-  cloud.points.resize (cloud.width * cloud.height);
+  cloud.resize (cloud.width * cloud.height);
   for (auto &point : cloud.points)
   {
     point.normal_x = -1.0;
@@ -92,7 +92,7 @@ TEST (PCL, PointCloudImageExtractorFromRGBField)
   cloud.width = 2;
   cloud.height = 2;
   cloud.is_dense = true;
-  cloud.points.resize (cloud.width * cloud.height);
+  cloud.resize (cloud.width * cloud.height);
   for (auto &point : cloud.points)
   {
     point.r =   0;
@@ -130,7 +130,7 @@ TEST (PCL, PointCloudImageExtractorFromRGBAField)
   cloud.width = 2;
   cloud.height = 2;
   cloud.is_dense = true;
-  cloud.points.resize (cloud.width * cloud.height);
+  cloud.resize (cloud.width * cloud.height);
   for (auto &point : cloud.points)
   {
     point.r =   0;
@@ -169,7 +169,7 @@ TEST (PCL, PointCloudImageExtractorFromLabelFieldMono)
   cloud.width = 2;
   cloud.height = 2;
   cloud.is_dense = true;
-  cloud.points.resize (cloud.width * cloud.height);
+  cloud.resize (cloud.width * cloud.height);
   for (std::size_t i = 0; i < cloud.size (); i++)
     cloud[i].label = i;
 
@@ -196,7 +196,7 @@ TEST (PCL, PointCloudImageExtractorFromLabelFieldRGB)
   cloud.width = 2;
   cloud.height = 2;
   cloud.is_dense = true;
-  cloud.points.resize (cloud.width * cloud.height);
+  cloud.resize (cloud.width * cloud.height);
   for (std::size_t i = 0; i < cloud.size (); i++)
     cloud[i].label = i % 2;
 
@@ -234,7 +234,7 @@ TEST (PCL, PointCloudImageExtractorFromLabelFieldGlasbey)
   cloud.width = 2;
   cloud.height = 2;
   cloud.is_dense = true;
-  cloud.points.resize (cloud.width * cloud.height);
+  cloud.resize (cloud.width * cloud.height);
   for (std::size_t i = 0; i < cloud.size (); i++)
     cloud[i].label = i % 2;
 
@@ -271,7 +271,7 @@ TEST (PCL, PointCloudImageExtractorFromZField)
   cloud.width = 2;
   cloud.height = 2;
   cloud.is_dense = true;
-  cloud.points.resize (cloud.width * cloud.height);
+  cloud.resize (cloud.width * cloud.height);
   for (std::size_t i = 0; i < cloud.size (); i++)
     cloud[i].z = 1.0 + i;
 
@@ -298,7 +298,7 @@ TEST (PCL, PointCloudImageExtractorFromCurvatureField)
   cloud.width = 2;
   cloud.height = 2;
   cloud.is_dense = true;
-  cloud.points.resize (cloud.width * cloud.height);
+  cloud.resize (cloud.width * cloud.height);
 
   cloud[0].curvature = 1.0;
   cloud[1].curvature = 2.0;
@@ -330,7 +330,7 @@ TEST (PCL, PointCloudImageExtractorFromIntensityField)
   cloud.width = 2;
   cloud.height = 2;
   cloud.is_dense = true;
-  cloud.points.resize (cloud.width * cloud.height);
+  cloud.resize (cloud.width * cloud.height);
 
   cloud[0].intensity = 10.0;
   cloud[1].intensity = 23.3;
@@ -360,7 +360,7 @@ TEST (PCL, PointCloudImageExtractorBadInput)
   cloud.width = 2;
   cloud.height = 2;
   cloud.is_dense = true;
-  cloud.points.resize (cloud.width * cloud.height);
+  cloud.resize (cloud.width * cloud.height);
 
   pcl::PCLImage image;
   {
@@ -397,7 +397,7 @@ TEST (PCL, PointCloudImageExtractorBlackNaNs)
   cloud.width = 2;
   cloud.height = 2;
   cloud.is_dense = false;
-  cloud.points.resize (cloud.width * cloud.height);
+  cloud.resize (cloud.width * cloud.height);
 
   cloud[0].curvature = 1.0;
   cloud[1].curvature = 2.0;
diff --git a/test/kdtree/test_kdtree.cpp b/test/kdtree/test_kdtree.cpp
index 4a047bf7..f0baf755 100644
--- a/test/kdtree/test_kdtree.cpp
+++ b/test/kdtree/test_kdtree.cpp
@@ -74,7 +74,7 @@ init ()
   for (float z = -0.5f; z <= 0.5f; z += resolution)
     for (float y = -0.5f; y <= 0.5f; y += resolution)
       for (float x = -0.5f; x <= 0.5f; x += resolution)
-        cloud.points.emplace_back(x, y, z);
+        cloud.emplace_back(x, y, z);
   cloud.width  = cloud.size ();
   cloud.height = 1;
 
@@ -83,7 +83,7 @@ init ()
   srand (static_cast<unsigned int> (time (nullptr)));
   // Randomly create a new point cloud
   for (std::size_t i = 0; i < cloud_big.width * cloud_big.height; ++i)
-    cloud_big.points.emplace_back(static_cast<float> (1024 * rand () / (RAND_MAX + 1.0)),
+    cloud_big.emplace_back(static_cast<float> (1024 * rand () / (RAND_MAX + 1.0)),
                                          static_cast<float> (1024 * rand () / (RAND_MAX + 1.0)),
                                          static_cast<float> (1024 * rand () / (RAND_MAX + 1.0)));
 }
@@ -99,13 +99,13 @@ TEST (PCL, KdTreeFLANN_radiusSearch)
   for (std::size_t i=0; i < cloud.size(); ++i)
     if (euclideanDistance(cloud[i], test_point) < max_dist)
       brute_force_result.insert(i);
-  std::vector<int> k_indices;
+  pcl::Indices k_indices;
   std::vector<float> k_distances;
   kdtree.radiusSearch (test_point, max_dist, k_indices, k_distances, 100);
   
   //std::cout << k_indices.size()<<"=="<<brute_force_result.size()<<"?\n";
   
-  for (const int &k_index : k_indices)
+  for (const auto &k_index : k_indices)
   {
     std::set<int>::iterator brute_force_result_it = brute_force_result.find (k_index);
     bool ok = brute_force_result_it != brute_force_result.end ();
@@ -178,7 +178,7 @@ TEST (PCL, KdTreeFLANN_nearestKSearch)
     ++counter;
   }
 
-  std::vector<int> k_indices;
+  pcl::Indices k_indices;
   k_indices.resize (no_of_neighbors);
   std::vector<float> k_distances;
   k_distances.resize (no_of_neighbors);
@@ -187,7 +187,7 @@ TEST (PCL, KdTreeFLANN_nearestKSearch)
   EXPECT_EQ (k_indices.size (), no_of_neighbors);
 
   // Check if all found neighbors have distance smaller than max_dist
-  for (const int &k_index : k_indices)
+  for (const auto &k_index : k_indices)
   {
     const MyPoint& point = cloud[k_index];
     bool ok = euclideanDistance (test_point, point) <= max_dist;
@@ -243,7 +243,7 @@ TEST (PCL, KdTreeFLANN_setPointRepresentation)
   
   // Find k nearest neighbors
   const int k = 10;
-  std::vector<int> k_indices (k);
+  pcl::Indices k_indices (k);
   std::vector<float> k_distances (k);
   kdtree.nearestKSearch (p, k, k_indices, k_distances);
   for (int i = 0; i < k; ++i)
@@ -294,11 +294,11 @@ TEST (PCL, KdTreeFLANN_32_vs_64_bit)
   KdTreeFLANN<PointXYZ> tree;
   tree.setInputCloud (cloud_in);
 
-  std::vector<std::vector<int> > nn_indices_vector;
+  std::vector<pcl::Indices > nn_indices_vector;
   for (std::size_t i = 0; i < cloud_in->size (); ++i)
     if (isFinite ((*cloud_in)[i]))
     {
-      std::vector<int> nn_indices;
+      pcl::Indices nn_indices;
       std::vector<float> nn_dists;
       tree.radiusSearch ((*cloud_in)[i], 0.02, nn_indices, nn_dists);
 
diff --git a/test/keypoints/test_keypoints.cpp b/test/keypoints/test_keypoints.cpp
index ff64524f..09351424 100644
--- a/test/keypoints/test_keypoints.cpp
+++ b/test/keypoints/test_keypoints.cpp
@@ -147,7 +147,7 @@ TEST (PCL, SIFTKeypoint_radiusSearch)
   }
   Eigen::MatrixXf diff_of_gauss;
 
-  std::vector<int> nn_indices;
+  pcl::Indices nn_indices;
   std::vector<float> nn_dist;
   diff_of_gauss.resize (input.size (), scales.size () - 1);
 
@@ -157,8 +157,8 @@ TEST (PCL, SIFTKeypoint_radiusSearch)
   tree.radiusSearch (i_point, max_radius, nn_indices, nn_dist);
 
   // Are they all unique?
-  std::set<int> unique_indices;
-  for (const int &nn_index : nn_indices)
+  std::set<pcl::index_t> unique_indices;
+  for (const auto &nn_index : nn_indices)
   {
     unique_indices.insert (nn_index);
   }
diff --git a/test/ml/CMakeLists.txt b/test/ml/CMakeLists.txt
new file mode 100644
index 00000000..0cef1218
--- /dev/null
+++ b/test/ml/CMakeLists.txt
@@ -0,0 +1,14 @@
+set(SUBSYS_NAME tests_ml)
+set(SUBSYS_DESC "Point cloud library ml module unit tests")
+PCL_SET_TEST_DEPENDENCIES(SUBSYS_DEPS ml)
+
+set(DEFAULT ON)
+set(build TRUE)
+PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}")
+PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS ${OPT_DEPS})
+
+if(NOT build)
+  return()
+endif()
+
+PCL_ADD_TEST(ml_kmeans test_ml_kmeans FILES test_kmeans.cpp LINK_WITH pcl_gtest pcl_common pcl_ml)
diff --git a/test/ml/test_kmeans.cpp b/test/ml/test_kmeans.cpp
new file mode 100644
index 00000000..afe31089
--- /dev/null
+++ b/test/ml/test_kmeans.cpp
@@ -0,0 +1,107 @@
+/*
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2020-, Open Perception
+ *
+ *  All rights reserved
+ */
+#include <pcl/test/gtest.h>
+#include <pcl/common/random.h>
+#include <pcl/ml/kmeans.h>
+
+using namespace pcl;
+using namespace pcl::common;
+
+using Point = std::vector<float>;
+
+// Prepare random number generator in PCL
+UniformGenerator<float> engine (-100000.0, 100000.0, 2021);
+
+class SampleDataChecker
+{
+  public:
+    int data_size_;
+    int dim_;
+    int cluster_size_;
+    std::vector<Point> data_sequence_;
+    std::vector<Point> answer_centroids_;
+
+    // Create sample data
+    void
+    createDataSequence ()
+    {
+      for (int data_id = 0; data_id < data_size_; ++data_id)
+      {
+        Point data;
+        for (int dim_i = 0; dim_i < dim_; ++dim_i)
+          data.push_back (engine.run ());
+        data_sequence_.push_back (data);
+      }
+    }
+
+    void
+    testKmeans (Kmeans& k_means)
+    {
+      k_means.setClusterSize (cluster_size_);
+      k_means.setInputData (data_sequence_);
+      k_means.initialClusterPoints ();
+      k_means.computeCentroids ();
+
+      // Input centroids that should be the correct answer
+      answer_centroids_ = k_means.get_centroids ();
+
+      // If centroids_ was initialized before calculating it,
+      // then it should not change
+      // no matter how many times this class method is called.
+      k_means.computeCentroids ();
+      k_means.computeCentroids ();
+    }
+};
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST (ComputeCentroids, Case1)
+{
+  // Create sample data sequence
+  SampleDataChecker sdc;
+  sdc.data_size_ = 20;
+  sdc.dim_ = 21;
+  sdc.cluster_size_ = 9;
+  sdc.createDataSequence ();
+
+  // Compute centroids with K-means
+  Kmeans k_means (sdc.data_size_, sdc.dim_);
+  sdc.testKmeans (k_means);
+
+  // Evaluate if the two centroids are the same
+  EXPECT_EQ (sdc.cluster_size_, k_means.get_centroids ().size ());
+  EXPECT_EQ (sdc.answer_centroids_, k_means.get_centroids ());
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST (ComputeCentroids, Case2)
+{
+  // Create sample data sequence
+  SampleDataChecker sdc;
+  sdc.data_size_ = 1;
+  sdc.dim_ = 1;
+  sdc.cluster_size_ = 1;
+  sdc.createDataSequence ();
+
+  // Compute centroids with K-means
+  Kmeans k_means (sdc.data_size_, sdc.dim_);
+  sdc.testKmeans (k_means);
+
+  // Evaluate if the two centroids are the same
+  EXPECT_EQ (sdc.cluster_size_, k_means.get_centroids ().size ());
+  EXPECT_EQ (sdc.answer_centroids_, k_means.get_centroids ());
+}
+
+/* ---[ */
+int
+main (int argc, char** argv)
+{
+  testing::InitGoogleTest (&argc, argv);
+  return (RUN_ALL_TESTS ());
+}
+/* ]--- */
diff --git a/test/octree/test_octree.cpp b/test/octree/test_octree.cpp
index 7e24a46f..0a4df9e6 100644
--- a/test/octree/test_octree.cpp
+++ b/test/octree/test_octree.cpp
@@ -308,7 +308,7 @@ TEST (PCL, Octree_Dynamic_Depth_Test)
     //  test iterator
     unsigned int leaf_count = 0;
 
-    std::vector<int> indexVector;
+    Indices indexVector;
 
     // iterate over tree
     for (auto it = octree.leaf_depth_begin(), it_end = octree.leaf_depth_end(); it != it_end; ++it)
@@ -329,13 +329,13 @@ TEST (PCL, Octree_Dynamic_Depth_Test)
       container.getPointIndices (indexVector);
 
       // test points against bounding box of leaf node
-      std::vector<int> tmpVector;
+      Indices tmpVector;
       container.getPointIndices (tmpVector);
 
       Eigen::Vector3f min_pt, max_pt;
       octree.getVoxelBounds (it, min_pt, max_pt);
 
-      for (const int &i : tmpVector)
+      for (const auto &i : tmpVector)
       {
         ASSERT_GE ((*cloud)[i].x, min_pt(0));
         ASSERT_GE ((*cloud)[i].y, min_pt(1));
@@ -477,7 +477,7 @@ TEST (PCL, Octree2Buf_Test)
     leafVectorA.pop_back ();
 
     bool bFound = false;
-    for (const int &value : data)
+    for (const auto &value : data)
       if (value == leafInt)
       {
         bFound = true;
@@ -496,7 +496,7 @@ TEST (PCL, Octree2Buf_Test)
     leafVectorA.pop_back ();
 
     bool bFound = false;
-    for (const int &value : data)
+    for (const auto &value : data)
       if (value == leafInt)
       {
         bFound = true;
@@ -790,14 +790,14 @@ TEST (PCL, Octree_Pointcloud_Test)
 
     for (std::size_t i = 0; i < cloudB->size (); i++)
     {
-      std::vector<int> pointIdxVec;
+      Indices pointIdxVec;
       octreeB.voxelSearch ((*cloudB)[i], pointIdxVec);
 
       bool bIdxFound = false;
-      std::vector<int>::const_iterator current = pointIdxVec.begin ();
-      while (current != pointIdxVec.end ())
+      auto current = pointIdxVec.cbegin ();
+      while (current != pointIdxVec.cend ())
       {
-        if (*current == static_cast<int> (i))
+        if (*current == static_cast<pcl::index_t> (i))
         {
           bIdxFound = true;
           break;
@@ -869,7 +869,7 @@ TEST (PCL, Octree_Pointcloud_Iterator_Test)
   octreeA.setInputCloud (cloudIn);
   octreeA.addPointsFromInputCloud ();
 
-  std::vector<int> indexVector;
+  Indices indexVector;
   unsigned int leafNodeCounter = 0;
 
   for (auto it1 = octreeA.leaf_depth_begin(), it1_end = octreeA.leaf_depth_end(); it1 != it1_end; ++it1)
@@ -900,7 +900,7 @@ TEST (PCL, Octree_Pointcloud_Iterator_Test)
   for (auto bfIt = octreeA.breadth_begin(); bfIt != octreeA.breadth_end(); ++bfIt)
   {
     // tree depth of visited nodes must grow
-    ASSERT_TRUE (bfIt.getCurrentOctreeDepth () >= lastDepth);
+    ASSERT_GE (bfIt.getCurrentOctreeDepth (), lastDepth);
     lastDepth = bfIt.getCurrentOctreeDepth ();
 
     if (bfIt.isBranchNode ())
@@ -1007,7 +1007,7 @@ TEST (PCL, Octree_Pointcloud_Change_Detector_Test)
         cloudIn);
   }
 
-  std::vector<int> newPointIdxVector;
+  Indices newPointIdxVector;
 
   // get a vector of new points, which did not exist in previous buffer
   octree.getPointIndicesFromNewVoxels (newPointIdxVector);
@@ -1018,7 +1018,7 @@ TEST (PCL, Octree_Pointcloud_Change_Detector_Test)
   // all point indices found should have an index of >= 1000
   for (std::size_t i = 0; i < 1000; i++)
   {
-    ASSERT_TRUE (newPointIdxVector[i] >= 1000);
+    ASSERT_GE (newPointIdxVector[i], 1000);
   }
 }
 
@@ -1136,86 +1136,90 @@ TEST (PCL, Octree_Pointcloud_Nearest_K_Neighbour_Search)
   OctreePointCloudSearch<PointXYZ> octree (0.1);
   octree.setInputCloud (cloudIn);
 
-  std::vector<int> k_indices;
+  Indices k_indices;
   std::vector<float> k_sqr_distances;
 
-  std::vector<int> k_indices_bruteforce;
+  Indices k_indices_bruteforce;
   std::vector<float> k_sqr_distances_bruteforce;
 
-  for (unsigned int test_id = 0; test_id < test_runs; test_id++)
-  {
-    // define a random search point
+  for (const std::size_t maxObjsPerLeaf: {0, 5}) {
+    if (maxObjsPerLeaf != 0)
+      octree.enableDynamicDepth (maxObjsPerLeaf);
+    for (unsigned int test_id = 0; test_id < test_runs; test_id++)
+    {
+      // define a random search point
 
-    PointXYZ searchPoint (static_cast<float> (10.0 * rand () / RAND_MAX),
-                          static_cast<float> (10.0 * rand () / RAND_MAX),
-                          static_cast<float> (10.0 * rand () / RAND_MAX));
+      PointXYZ searchPoint (static_cast<float> (10.0 * rand () / RAND_MAX),
+                            static_cast<float> (10.0 * rand () / RAND_MAX),
+                            static_cast<float> (10.0 * rand () / RAND_MAX));
 
-    unsigned int K = 1 + rand () % 10;
+      unsigned int K = 1 + rand () % 10;
 
-    // generate point cloud
-    cloudIn->width = 1000;
-    cloudIn->height = 1;
-    cloudIn->points.resize (cloudIn->width * cloudIn->height);
-    for (std::size_t i = 0; i < 1000; i++)
-    {
-      (*cloudIn)[i] = PointXYZ (static_cast<float> (5.0  * rand () / RAND_MAX),
-                                     static_cast<float> (10.0 * rand () / RAND_MAX),
-                                     static_cast<float> (10.0 * rand () / RAND_MAX));
-    }
+      // generate point cloud
+      cloudIn->width = 1000;
+      cloudIn->height = 1;
+      cloudIn->points.resize (cloudIn->width * cloudIn->height);
+      for (std::size_t i = 0; i < 1000; i++)
+      {
+        (*cloudIn)[i] = PointXYZ (static_cast<float> (5.0  * rand () / RAND_MAX),
+                                       static_cast<float> (10.0 * rand () / RAND_MAX),
+                                       static_cast<float> (10.0 * rand () / RAND_MAX));
+      }
 
-    k_indices.clear ();
-    k_sqr_distances.clear ();
+      k_indices.clear ();
+      k_sqr_distances.clear ();
 
-    k_indices_bruteforce.clear ();
-    k_sqr_distances_bruteforce.clear ();
+      k_indices_bruteforce.clear ();
+      k_sqr_distances_bruteforce.clear ();
 
-    // push all points and their distance to the search point into a priority queue - bruteforce approach.
-    for (std::size_t i = 0; i < cloudIn->size (); i++)
-    {
-      double pointDist = (((*cloudIn)[i].x - searchPoint.x) * ((*cloudIn)[i].x - searchPoint.x) +
-                          ((*cloudIn)[i].y - searchPoint.y) * ((*cloudIn)[i].y - searchPoint.y) +
-                          ((*cloudIn)[i].z - searchPoint.z) * ((*cloudIn)[i].z - searchPoint.z));
+      // push all points and their distance to the search point into a priority queue - bruteforce approach.
+      for (std::size_t i = 0; i < cloudIn->size (); i++)
+      {
+        double pointDist = (((*cloudIn)[i].x - searchPoint.x) * ((*cloudIn)[i].x - searchPoint.x) +
+                            ((*cloudIn)[i].y - searchPoint.y) * ((*cloudIn)[i].y - searchPoint.y) +
+                            ((*cloudIn)[i].z - searchPoint.z) * ((*cloudIn)[i].z - searchPoint.z));
 
-      prioPointQueueEntry pointEntry ((*cloudIn)[i], pointDist, static_cast<int> (i));
+        prioPointQueueEntry pointEntry ((*cloudIn)[i], pointDist, static_cast<int> (i));
 
-      pointCandidates.push (pointEntry);
-    }
+        pointCandidates.push (pointEntry);
+      }
 
-    // pop priority queue until we have the nearest K elements
-    while (pointCandidates.size () > K)
-      pointCandidates.pop ();
+      // pop priority queue until we have the nearest K elements
+      while (pointCandidates.size () > K)
+        pointCandidates.pop ();
 
-    // copy results into vectors
-    unsigned idx = static_cast<unsigned> (pointCandidates.size ());
-    k_indices_bruteforce.resize (idx);
-    k_sqr_distances_bruteforce.resize (idx);
-    while (!pointCandidates.empty ())
-    {
-      --idx;
-      k_indices_bruteforce [idx] = pointCandidates.top ().pointIdx_;
-      k_sqr_distances_bruteforce [idx] = static_cast<float> (pointCandidates.top ().pointDistance_);
+      // copy results into vectors
+      unsigned idx = static_cast<unsigned> (pointCandidates.size ());
+      k_indices_bruteforce.resize (idx);
+      k_sqr_distances_bruteforce.resize (idx);
+      while (!pointCandidates.empty ())
+      {
+        --idx;
+        k_indices_bruteforce [idx] = pointCandidates.top ().pointIdx_;
+        k_sqr_distances_bruteforce [idx] = static_cast<float> (pointCandidates.top ().pointDistance_);
 
-      pointCandidates.pop ();
-    }
+        pointCandidates.pop ();
+      }
 
-    // octree nearest neighbor search
-    octree.deleteTree ();
-    octree.addPointsFromInputCloud ();
-    octree.nearestKSearch (searchPoint, static_cast<int> (K), k_indices, k_sqr_distances);
+      // octree nearest neighbor search
+      octree.deleteTree ();
+      octree.addPointsFromInputCloud ();
+      octree.nearestKSearch (searchPoint, static_cast<int> (K), k_indices, k_sqr_distances);
 
-    ASSERT_EQ (k_indices_bruteforce.size (), k_indices.size());
+      ASSERT_EQ (k_indices_bruteforce.size (), k_indices.size());
 
-    // compare nearest neighbor results of octree with bruteforce search
-    while (!k_indices_bruteforce.empty ())
-    {
-      ASSERT_EQ (k_indices_bruteforce.back(), k_indices.back ());
-      EXPECT_NEAR (k_sqr_distances_bruteforce.back (), k_sqr_distances.back (), 1e-4);
+      // compare nearest neighbor results of octree with bruteforce search
+      while (!k_indices_bruteforce.empty ())
+      {
+        ASSERT_EQ (k_indices_bruteforce.back(), k_indices.back ());
+        EXPECT_NEAR (k_sqr_distances_bruteforce.back (), k_sqr_distances.back (), 1e-4);
 
-      k_indices_bruteforce.pop_back ();
-      k_indices.pop_back ();
+        k_indices_bruteforce.pop_back ();
+        k_indices.pop_back ();
 
-      k_sqr_distances_bruteforce.pop_back ();
-      k_sqr_distances.pop_back ();
+        k_sqr_distances_bruteforce.pop_back ();
+        k_sqr_distances.pop_back ();
+      }
     }
   }
 }
@@ -1233,56 +1237,60 @@ TEST (PCL, Octree_Pointcloud_Box_Search)
   OctreePointCloudSearch<PointXYZ> octree (1);
   octree.setInputCloud (cloudIn);
 
-  for (unsigned int test_id = 0; test_id < test_runs; test_id++)
-  {
-    std::vector<int> k_indices;
-
-    // generate point cloud
-    cloudIn->width = 300;
-    cloudIn->height = 1;
-    cloudIn->points.resize (cloudIn->width * cloudIn->height);
-    for (auto &point : cloudIn->points)
+  for (const std::size_t maxObjsPerLeaf: {0, 5}) {
+    if (maxObjsPerLeaf != 0)
+      octree.enableDynamicDepth (maxObjsPerLeaf);
+    for (unsigned int test_id = 0; test_id < test_runs; test_id++)
     {
-      point = PointXYZ (static_cast<float> (10.0 * rand () / RAND_MAX),
-                        static_cast<float> (10.0 * rand () / RAND_MAX),
-                        static_cast<float> (10.0 * rand () / RAND_MAX));
-    }
-
+      Indices k_indices;
 
-    // octree points to octree
-    octree.deleteTree ();
-    octree.addPointsFromInputCloud ();
+      // generate point cloud
+      cloudIn->width = 300;
+      cloudIn->height = 1;
+      cloudIn->points.resize (cloudIn->width * cloudIn->height);
+      for (auto &point : cloudIn->points)
+      {
+        point = PointXYZ (static_cast<float> (10.0 * rand () / RAND_MAX),
+                          static_cast<float> (10.0 * rand () / RAND_MAX),
+                          static_cast<float> (10.0 * rand () / RAND_MAX));
+      }
 
-    // define a random search area
 
-    Eigen::Vector3f lowerBoxCorner (static_cast<float> (4.0 * rand () / RAND_MAX),
-                                    static_cast<float> (4.0 * rand () / RAND_MAX),
-                                    static_cast<float> (4.0 * rand () / RAND_MAX));
-    Eigen::Vector3f upperBoxCorner (static_cast<float> (5.0 + 4.0 * rand () / RAND_MAX),
-                                    static_cast<float> (5.0 + 4.0 * rand () / RAND_MAX),
-                                    static_cast<float> (5.0 + 4.0 * rand () / RAND_MAX));
+      // octree points to octree
+      octree.deleteTree ();
+      octree.addPointsFromInputCloud ();
 
-    octree.boxSearch (lowerBoxCorner, upperBoxCorner, k_indices);
+      // define a random search area
 
-    // test every point in point cloud
-    for (std::size_t i = 0; i < 300; i++)
-    {
-      bool inBox;
-      bool idxInResults;
-      const PointXYZ& pt = (*cloudIn)[i];
+      Eigen::Vector3f lowerBoxCorner (static_cast<float> (4.0 * rand () / RAND_MAX),
+                                      static_cast<float> (4.0 * rand () / RAND_MAX),
+                                      static_cast<float> (4.0 * rand () / RAND_MAX));
+      Eigen::Vector3f upperBoxCorner (static_cast<float> (5.0 + 4.0 * rand () / RAND_MAX),
+                                      static_cast<float> (5.0 + 4.0 * rand () / RAND_MAX),
+                                      static_cast<float> (5.0 + 4.0 * rand () / RAND_MAX));
 
-      inBox = (pt.x >= lowerBoxCorner (0)) && (pt.x <= upperBoxCorner (0)) &&
-              (pt.y >= lowerBoxCorner (1)) && (pt.y <= upperBoxCorner (1)) &&
-              (pt.z >= lowerBoxCorner (2)) && (pt.z <= upperBoxCorner (2));
+      octree.boxSearch (lowerBoxCorner, upperBoxCorner, k_indices);
 
-      idxInResults = false;
-      for (std::size_t j = 0; (j < k_indices.size ()) && (!idxInResults); ++j)
+      // test every point in point cloud
+      for (std::size_t i = 0; i < cloudIn->size(); i++)
       {
-        if (i == static_cast<unsigned int> (k_indices[j]))
-          idxInResults = true;
-      }
+        bool inBox;
+        bool idxInResults;
+        const PointXYZ& pt = (*cloudIn)[i];
+
+        inBox = (pt.x >= lowerBoxCorner (0)) && (pt.x <= upperBoxCorner (0)) &&
+                (pt.y >= lowerBoxCorner (1)) && (pt.y <= upperBoxCorner (1)) &&
+                (pt.z >= lowerBoxCorner (2)) && (pt.z <= upperBoxCorner (2));
 
-      ASSERT_EQ (idxInResults, inBox);
+        idxInResults = false;
+        for (std::size_t j = 0; (j < k_indices.size ()) && (!idxInResults); ++j)
+        {
+          if (i == static_cast<unsigned int> (k_indices[j]))
+            idxInResults = true;
+        }
+
+        ASSERT_EQ (idxInResults, inBox);
+      }
     }
   }
 }
@@ -1291,74 +1299,78 @@ TEST(PCL, Octree_Pointcloud_Approx_Nearest_Neighbour_Search)
 {
   constexpr unsigned int test_runs = 100;
 
-  unsigned int bestMatchCount = 0;
-
-  // instantiate point cloud
-  PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
-
-  srand (static_cast<unsigned int> (time (nullptr)));
+  for (const std::size_t maxObjsPerLeaf: {0, 5}) {
+    unsigned int bestMatchCount = 0;
 
-  constexpr double voxelResolution = 0.1;
+    // instantiate point cloud
+    PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
 
-  // create octree
-  OctreePointCloudSearch<PointXYZ> octree (voxelResolution);
-  octree.setInputCloud (cloudIn);
+    srand (static_cast<unsigned int> (time (nullptr)));
 
-  for (unsigned int test_id = 0; test_id < test_runs; test_id++)
-  {
-    // define a random search point
+    constexpr double voxelResolution = 0.1;
 
-    PointXYZ searchPoint (static_cast<float> (10.0 * rand () / RAND_MAX),
-                          static_cast<float> (10.0 * rand () / RAND_MAX),
-                          static_cast<float> (10.0 * rand () / RAND_MAX));
+    // create octree
+    OctreePointCloudSearch<PointXYZ> octree (voxelResolution);
+    octree.setInputCloud (cloudIn);
+    if (maxObjsPerLeaf != 0)
+      octree.enableDynamicDepth (maxObjsPerLeaf);
 
-    // generate point cloud
-    cloudIn->width = 1000;
-    cloudIn->height = 1;
-    cloudIn->points.resize (cloudIn->width * cloudIn->height);
-    for (std::size_t i = 0; i < 1000; i++)
+    for (unsigned int test_id = 0; test_id < test_runs; test_id++)
     {
-      (*cloudIn)[i] = PointXYZ (static_cast<float> (5.0  * rand () / RAND_MAX),
-                                     static_cast<float> (10.0 * rand () / RAND_MAX),
-                                     static_cast<float> (10.0 * rand () / RAND_MAX));
-    }
+      // define a random search point
 
-    // brute force search
-    double BFdistance = std::numeric_limits<double>::max ();
-    int BFindex = 0;
+      PointXYZ searchPoint (static_cast<float> (10.0 * rand () / RAND_MAX),
+                            static_cast<float> (10.0 * rand () / RAND_MAX),
+                            static_cast<float> (10.0 * rand () / RAND_MAX));
 
-    for (std::size_t i = 0; i < cloudIn->size (); i++)
-    {
-      double pointDist = (((*cloudIn)[i].x - searchPoint.x) * ((*cloudIn)[i].x - searchPoint.x) +
-                          ((*cloudIn)[i].y - searchPoint.y) * ((*cloudIn)[i].y - searchPoint.y) +
-                          ((*cloudIn)[i].z - searchPoint.z) * ((*cloudIn)[i].z - searchPoint.z));
+      // generate point cloud
+      cloudIn->width = 1000;
+      cloudIn->height = 1;
+      cloudIn->points.resize (cloudIn->width * cloudIn->height);
+      for (std::size_t i = 0; i < 1000; i++)
+      {
+        (*cloudIn)[i] = PointXYZ (static_cast<float> (5.0  * rand () / RAND_MAX),
+                                       static_cast<float> (10.0 * rand () / RAND_MAX),
+                                       static_cast<float> (10.0 * rand () / RAND_MAX));
+      }
+
+      // brute force search
+      double BFdistance = std::numeric_limits<double>::max ();
+      pcl::index_t BFindex = 0;
 
-      if (pointDist < BFdistance)
+      for (std::size_t i = 0; i < cloudIn->size (); i++)
       {
-        BFindex = static_cast<int> (i);
-        BFdistance = pointDist;
+        double pointDist = (((*cloudIn)[i].x - searchPoint.x) * ((*cloudIn)[i].x - searchPoint.x) +
+                            ((*cloudIn)[i].y - searchPoint.y) * ((*cloudIn)[i].y - searchPoint.y) +
+                            ((*cloudIn)[i].z - searchPoint.z) * ((*cloudIn)[i].z - searchPoint.z));
+
+        if (pointDist < BFdistance)
+        {
+          BFindex = static_cast<pcl::index_t> (i);
+          BFdistance = pointDist;
+        }
+
       }
 
-    }
+      index_t ANNindex;
+      float ANNdistance;
 
-    int ANNindex;
-    float ANNdistance;
+      // octree approx. nearest neighbor search
+      octree.deleteTree ();
+      octree.addPointsFromInputCloud ();
+      octree.approxNearestSearch (searchPoint, ANNindex, ANNdistance);
 
-    // octree approx. nearest neighbor search
-    octree.deleteTree ();
-    octree.addPointsFromInputCloud ();
-    octree.approxNearestSearch (searchPoint, ANNindex, ANNdistance);
+      if (BFindex == ANNindex)
+      {
+        EXPECT_NEAR (ANNdistance, BFdistance, 1e-4);
+        bestMatchCount++;
+      }
 
-    if (BFindex == ANNindex)
-    {
-      EXPECT_NEAR (ANNdistance, BFdistance, 1e-4);
-      bestMatchCount++;
     }
 
+    // we should have found the absolute nearest neighbor at least once
+    ASSERT_GT (bestMatchCount, 0);
   }
-
-  // we should have found the absolute nearest neighbor at least once
-  ASSERT_TRUE (bestMatchCount > 0);
 }
 
 TEST (PCL, Octree_Pointcloud_Neighbours_Within_Radius_Search)
@@ -1367,83 +1379,85 @@ TEST (PCL, Octree_Pointcloud_Neighbours_Within_Radius_Search)
 
   // instantiate point clouds
   PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
-  PointCloud<PointXYZ>::Ptr cloudOut (new PointCloud<PointXYZ> ());
 
   srand (static_cast<unsigned int> (time (nullptr)));
 
-  for (unsigned int test_id = 0; test_id < test_runs; test_id++)
-  {
-    // define a random search point
-    PointXYZ searchPoint (static_cast<float> (10.0 * rand () / RAND_MAX),
-                          static_cast<float> (10.0 * rand () / RAND_MAX),
-                          static_cast<float> (10.0 * rand () / RAND_MAX));
-
-    cloudIn->width = 1000;
-    cloudIn->height = 1;
-    cloudIn->points.resize (cloudIn->width * cloudIn->height);
-
-    // generate point cloud data
-    for (std::size_t i = 0; i < 1000; i++)
+  for (const std::size_t maxObjsPerLeaf: {0, 5}) {
+    for (unsigned int test_id = 0; test_id < test_runs; test_id++)
     {
-      (*cloudIn)[i] = PointXYZ (static_cast<float> (10.0 * rand () / RAND_MAX),
-                                     static_cast<float> (10.0 * rand () / RAND_MAX),
-                                     static_cast<float> (5.0  * rand () / RAND_MAX));
-    }
+      // define a random search point
+      PointXYZ searchPoint (static_cast<float> (10.0 * rand () / RAND_MAX),
+                            static_cast<float> (10.0 * rand () / RAND_MAX),
+                            static_cast<float> (10.0 * rand () / RAND_MAX));
 
-    OctreePointCloudSearch<PointXYZ> octree (0.001);
+      cloudIn->width = 1000;
+      cloudIn->height = 1;
+      cloudIn->points.resize (cloudIn->width * cloudIn->height);
 
-    // build octree
-    octree.setInputCloud (cloudIn);
-    octree.addPointsFromInputCloud ();
+      // generate point cloud data
+      for (std::size_t i = 0; i < 1000; i++)
+      {
+        (*cloudIn)[i] = PointXYZ (static_cast<float> (10.0 * rand () / RAND_MAX),
+                                       static_cast<float> (10.0 * rand () / RAND_MAX),
+                                       static_cast<float> (5.0  * rand () / RAND_MAX));
+      }
 
-    double pointDist;
-    double searchRadius = 5.0 * static_cast<float> (rand () / RAND_MAX);
+      OctreePointCloudSearch<PointXYZ> octree (0.001);
 
-    // bruteforce radius search
-    std::vector<int> cloudSearchBruteforce;
-    for (std::size_t i = 0; i < cloudIn->size (); i++)
-    {
-      pointDist = sqrt (
-          ((*cloudIn)[i].x - searchPoint.x) * ((*cloudIn)[i].x - searchPoint.x)
-              + ((*cloudIn)[i].y - searchPoint.y) * ((*cloudIn)[i].y - searchPoint.y)
-              + ((*cloudIn)[i].z - searchPoint.z) * ((*cloudIn)[i].z - searchPoint.z));
+      // build octree
+      octree.setInputCloud (cloudIn);
+      if (maxObjsPerLeaf != 0)
+        octree.enableDynamicDepth (maxObjsPerLeaf);
+      octree.addPointsFromInputCloud ();
 
-      if (pointDist <= searchRadius)
+      double pointDist;
+      double searchRadius = 5.0 * static_cast<float> (rand () / RAND_MAX);
+
+      // bruteforce radius search
+      std::vector<int> cloudSearchBruteforce;
+      for (std::size_t i = 0; i < cloudIn->size (); i++)
       {
-        // add point candidates to vector list
-        cloudSearchBruteforce.push_back (static_cast<int> (i));
+        pointDist = sqrt (
+            ((*cloudIn)[i].x - searchPoint.x) * ((*cloudIn)[i].x - searchPoint.x)
+                + ((*cloudIn)[i].y - searchPoint.y) * ((*cloudIn)[i].y - searchPoint.y)
+                + ((*cloudIn)[i].z - searchPoint.z) * ((*cloudIn)[i].z - searchPoint.z));
+
+        if (pointDist <= searchRadius)
+        {
+          // add point candidates to vector list
+          cloudSearchBruteforce.push_back (static_cast<int> (i));
+        }
       }
-    }
 
-    std::vector<int> cloudNWRSearch;
-    std::vector<float> cloudNWRRadius;
+      Indices cloudNWRSearch;
+      std::vector<float> cloudNWRRadius;
 
-    // execute octree radius search
-    octree.radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius);
+      // execute octree radius search
+      octree.radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius);
 
-    ASSERT_EQ (cloudSearchBruteforce.size (), cloudNWRRadius.size ());
+      ASSERT_EQ (cloudSearchBruteforce.size (), cloudNWRRadius.size ());
 
-    // check if result from octree radius search can be also found in bruteforce search
-    std::vector<int>::const_iterator current = cloudNWRSearch.begin ();
-    while (current != cloudNWRSearch.end ())
-    {
-      pointDist = sqrt (
-          ((*cloudIn)[*current].x - searchPoint.x) * ((*cloudIn)[*current].x - searchPoint.x)
-              + ((*cloudIn)[*current].y - searchPoint.y) * ((*cloudIn)[*current].y - searchPoint.y)
-              + ((*cloudIn)[*current].z - searchPoint.z) * ((*cloudIn)[*current].z - searchPoint.z));
+      // check if result from octree radius search can be also found in bruteforce search
+      auto current = cloudNWRSearch.cbegin ();
+      while (current != cloudNWRSearch.cend ())
+      {
+        pointDist = sqrt (
+            ((*cloudIn)[*current].x - searchPoint.x) * ((*cloudIn)[*current].x - searchPoint.x)
+                + ((*cloudIn)[*current].y - searchPoint.y) * ((*cloudIn)[*current].y - searchPoint.y)
+                + ((*cloudIn)[*current].z - searchPoint.z) * ((*cloudIn)[*current].z - searchPoint.z));
 
-      ASSERT_TRUE (pointDist <= searchRadius);
+        ASSERT_LE (pointDist, searchRadius);
 
-      ++current;
-    }
+        ++current;
+      }
 
-    // check if result limitation works
-    octree.radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius, 5);
+      // check if result limitation works
+      octree.radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius, 5);
 
-    ASSERT_TRUE (cloudNWRRadius.size () <= 5);
+      ASSERT_LE (cloudNWRRadius.size (), 5);
 
+    }
   }
-
 }
 
 TEST (PCL, Octree_Pointcloud_Ray_Traversal)
@@ -1459,7 +1473,7 @@ TEST (PCL, Octree_Pointcloud_Ray_Traversal)
   pcl::PointCloud<pcl::PointXYZ>::VectorType voxelsInRay, voxelsInRay2;
 
   // Indices in ray
-  std::vector<int> indicesInRay, indicesInRay2;
+  Indices indicesInRay, indicesInRay2;
 
   srand (static_cast<unsigned int> (time (nullptr)));
 
@@ -1620,7 +1634,7 @@ TEST (PCL, Octree_Pointcloud_Bounds)
     const double LARGE_MAX = 1e7-5*SOME_RESOLUTION;
     tree.defineBoundingBox (LARGE_MIN, LARGE_MIN, LARGE_MIN, LARGE_MAX, LARGE_MAX, LARGE_MAX);
     tree.getBoundingBox (min_x, min_y, min_z, max_x, max_y, max_z);
-    const unsigned int depth = tree.getTreeDepth ();
+    const auto depth = tree.getTreeDepth ();
     tree.defineBoundingBox (min_x, min_y, min_z, max_x, max_y, max_z);
 
     ASSERT_EQ (depth, tree.getTreeDepth ());
diff --git a/test/outofcore/test_outofcore.cpp b/test/outofcore/test_outofcore.cpp
index 50c49efc..0abba40b 100644
--- a/test/outofcore/test_outofcore.cpp
+++ b/test/outofcore/test_outofcore.cpp
@@ -742,12 +742,12 @@ TEST_F (OutofcoreTest, PointCloud2_Insertion)
 
   pcl::PointCloud<pcl::PointXYZ> point_cloud;
 
-  point_cloud.points.reserve (numPts);
+  point_cloud.reserve (numPts);
   point_cloud.width = static_cast<std::uint32_t> (numPts);
   point_cloud.height = 1;
 
   for (std::size_t i=0; i < numPts; i++)
-    point_cloud.points.emplace_back(static_cast<float>(rand () % 10), static_cast<float>(rand () % 10), static_cast<float>(rand () % 10));
+    point_cloud.emplace_back(static_cast<float>(rand () % 10), static_cast<float>(rand () % 10), static_cast<float>(rand () % 10));
 
 
   pcl::PCLPointCloud2::Ptr input_cloud (new pcl::PCLPointCloud2 ());
@@ -914,7 +914,7 @@ TEST_F (OutofcoreTest, PointCloud2_Query)
   std::uint64_t points_added = octreeA.addPointCloud (dst_blob, false);
   std::uint64_t LOD_points_added = octreeB.addPointCloud_and_genLOD (dst_blob);
 
-  ASSERT_EQ (points_added, dst_blob->width*dst_blob->height) << "Number of points returned by addPointCloud does not match the number of poitns in the input point cloud\n";
+  ASSERT_EQ (points_added, dst_blob->width*dst_blob->height) << "Number of points returned by addPointCloud does not match the number of points in the input point cloud\n";
   ASSERT_EQ (LOD_points_added, dst_blob->width*dst_blob->height) << "Number of points returned by addPointCloud_and_genLOD does not match the number of points in the input point cloud\n";
 
   pcl::PCLPointCloud2::Ptr query_result_a (new pcl::PCLPointCloud2 ());
diff --git a/test/recognition/test_recognition_cg.cpp b/test/recognition/test_recognition_cg.cpp
index 108b4fff..b65f3671 100644
--- a/test/recognition/test_recognition_cg.cpp
+++ b/test/recognition/test_recognition_cg.cpp
@@ -79,7 +79,7 @@ computeRmsE (const PointCloud<PointType>::ConstPtr &model, const PointCloud<Poin
   double sqr_norm_sum = 0;
   int found_points = 0;
 
-  std::vector<int> neigh_indices (1);
+  pcl::Indices neigh_indices (1);
   std::vector<float> neigh_sqr_dists (1);
   for (const auto &model : transformed_model)
   {
@@ -221,7 +221,7 @@ main (int argc, char** argv)
   {
     if ( std::isfinite( scene_descriptors_->at (i).descriptor[0] ) )
     {
-      std::vector<int> neigh_indices (1);
+      pcl::Indices neigh_indices (1);
       std::vector<float> neigh_sqr_dists (1);
       int found_neighs = match_search.nearestKSearch (scene_descriptors_->at (i), 1, neigh_indices, neigh_sqr_dists);
       if(found_neighs == 1 && neigh_sqr_dists[0] < 0.25f)
diff --git a/test/registration/test_kfpcs_ia.cpp b/test/registration/test_kfpcs_ia.cpp
index 81cda560..3fe30516 100644
--- a/test/registration/test_kfpcs_ia.cpp
+++ b/test/registration/test_kfpcs_ia.cpp
@@ -52,6 +52,8 @@ PointCloud<PointXYZI> cloud_source, cloud_target;
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 TEST (PCL, KFPCSInitialAlignment)
 {
+  const auto previous_verbosity_level = pcl::console::getVerbosityLevel();
+  pcl::console::setVerbosityLevel(pcl::console::L_VERBOSE);
   // create shared pointers
   PointCloud<PointXYZI>::Ptr cloud_source_ptr, cloud_target_ptr;
   cloud_source_ptr = cloud_source.makeShared ();
@@ -93,6 +95,7 @@ TEST (PCL, KFPCSInitialAlignment)
   EXPECT_EQ (cloud_source_aligned.size (), cloud_source.size ());
   EXPECT_NEAR (angle3d, 0.f, max_angle3d);
   EXPECT_NEAR (translation3d, 0.f, max_translation3d);
+  pcl::console::setVerbosityLevel(previous_verbosity_level); // reset verbosity level
 }
 
 
diff --git a/test/registration/test_ndt.cpp b/test/registration/test_ndt.cpp
index 6da60b3e..91276c04 100644
--- a/test/registration/test_ndt.cpp
+++ b/test/registration/test_ndt.cpp
@@ -46,9 +46,9 @@
 
 using namespace pcl;
 using namespace pcl::io;
- 
+
 PointCloud<PointXYZ> cloud_source, cloud_target;
- 
+
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 TEST (PCL, NormalDistributionsTransform)
 {
@@ -74,8 +74,8 @@ TEST (PCL, NormalDistributionsTransform)
   // Check again, for all possible caching schemes
   for (int iter = 0; iter < 4; iter++)
   {
-    bool force_cache = (bool) iter/2;
-    bool force_cache_reciprocal = (bool) iter%2;
+    bool force_cache = static_cast<bool> (iter/2);
+    bool force_cache_reciprocal = static_cast<bool> (iter%2);
     pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
     // Ensure that, when force_cache is not set, we are robust to the wrong input
     if (force_cache)
diff --git a/test/registration/test_registration.cpp b/test/registration/test_registration.cpp
index 5219a50c..2c33e690 100644
--- a/test/registration/test_registration.cpp
+++ b/test/registration/test_registration.cpp
@@ -103,19 +103,19 @@ TEST (PCL, findFeatureCorrespondences)
       FeatureT f;
       f.histogram[0] = x;
       f.histogram[1] = y;
-      feature0.points.push_back (f);
+      feature0.push_back (f);
 
       f.histogram[0] = x;
       f.histogram[1] = y - 2.5f;
-      feature1.points.push_back (f);
+      feature1.push_back (f);
 
       f.histogram[0] = x - 2.0f;
       f.histogram[1] = y + 1.5f;
-      feature2.points.push_back (f);
+      feature2.push_back (f);
 
       f.histogram[0] = x + 2.0f;
       f.histogram[1] = y + 1.5f;
-      feature3.points.push_back (f);
+      feature3.push_back (f);
     }
   }
   feature0.width = feature0.size ();
@@ -184,7 +184,7 @@ TEST(PCL, ICP_translated)
   icp.align(Final);
 
   // Check that we have sucessfully converged
-  ASSERT_EQ(icp.hasConverged(), true);
+  ASSERT_TRUE(icp.hasConverged());
 
   // Test that the fitness score is below acceptable threshold
   EXPECT_LT(icp.getFitnessScore(), 1e-6);
@@ -475,8 +475,8 @@ TEST (PCL, IterativeClosestPoint_PointToPlane)
   // Check again, for all possible caching schemes
   for (int iter = 0; iter < 4; iter++)
   {
-    bool force_cache = (bool) iter/2;
-    bool force_cache_reciprocal = (bool) iter%2;
+    bool force_cache = static_cast<bool> (iter/2);
+    bool force_cache_reciprocal = static_cast<bool> (iter%2);
     pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
     // Ensure that, when force_cache is not set, we are robust to the wrong input
     if (force_cache)
@@ -548,8 +548,8 @@ TEST (PCL, GeneralizedIterativeClosestPoint)
   // Check again, for all possible caching schemes
   for (int iter = 0; iter < 4; iter++)
   {
-    bool force_cache = (bool) iter/2;
-    bool force_cache_reciprocal = (bool) iter%2;
+    bool force_cache = static_cast<bool> (iter/2);
+    bool force_cache_reciprocal = static_cast<bool> (iter%2);
     pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
     // Ensure that, when force_cache is not set, we are robust to the wrong input
     if (force_cache)
@@ -621,8 +621,8 @@ TEST (PCL, GeneralizedIterativeClosestPoint6D)
   // Check again, for all possible caching schemes
   for (int iter = 0; iter < 4; iter++)
   {
-    bool force_cache = (bool) iter/2;
-    bool force_cache_reciprocal = (bool) iter%2;
+    bool force_cache = static_cast<bool> (iter/2);
+    bool force_cache_reciprocal = static_cast<bool> (iter%2);
     pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
     // Ensure that, when force_cache is not set, we are robust to the wrong input
     if (force_cache)
@@ -694,7 +694,7 @@ TEST (PCL, PyramidFeatureHistogram)
   pyramid_target->compute ();
 
   float similarity_value = PyramidFeatureHistogram<PPFSignature>::comparePyramidFeatureHistograms (pyramid_source, pyramid_target);
-  EXPECT_NEAR (similarity_value, 0.74101555347442627, 1e-4);
+  EXPECT_NEAR (similarity_value, 0.738492727, 1e-4);
 
   std::vector<std::pair<float, float> > dim_range_target2;
   for (std::size_t i = 0; i < 3; ++i) dim_range_target2.emplace_back(static_cast<float> (-M_PI) * 5.0f, static_cast<float> (M_PI) * 5.0f);
@@ -707,7 +707,7 @@ TEST (PCL, PyramidFeatureHistogram)
   pyramid_target->compute ();
 
   float similarity_value2 = PyramidFeatureHistogram<PPFSignature>::comparePyramidFeatureHistograms (pyramid_source, pyramid_target);
-  EXPECT_NEAR (similarity_value2, 0.80097091197967529, 1e-4);
+  EXPECT_NEAR (similarity_value2, 0.798465133, 1e-4);
 
 
   std::vector<std::pair<float, float> > dim_range_target3;
@@ -721,7 +721,7 @@ TEST (PCL, PyramidFeatureHistogram)
   pyramid_target->compute ();
 
   float similarity_value3 = PyramidFeatureHistogram<PPFSignature>::comparePyramidFeatureHistograms (pyramid_source, pyramid_target);
-  EXPECT_NEAR (similarity_value3, 0.87623238563537598, 1e-3);
+  EXPECT_NEAR (similarity_value3, 0.873699546, 1e-3);
 }
 
 // Suat G: disabled, since the transformation does not look correct.
diff --git a/test/registration/test_registration_api.cpp b/test/registration/test_registration_api.cpp
index d5ba510b..5667a8cc 100644
--- a/test/registration/test_registration_api.cpp
+++ b/test/registration/test_registration_api.cpp
@@ -43,7 +43,6 @@
 
 #include <pcl/point_types.h>
 #include <pcl/io/pcd_io.h>
-#include <pcl/registration/eigen.h>
 #include <pcl/registration/correspondence_estimation.h>
 #include <pcl/registration/correspondence_rejection_distance.h>
 #include <pcl/registration/correspondence_rejection_median_distance.h>
diff --git a/test/registration/test_sac_ia.cpp b/test/registration/test_sac_ia.cpp
index a39fbc83..c1f717d0 100644
--- a/test/registration/test_sac_ia.cpp
+++ b/test/registration/test_sac_ia.cpp
@@ -115,8 +115,8 @@ TEST (PCL, SampleConsensusInitialAlignment)
   using PointT = pcl::PointXYZ;
   for (int iter = 0; iter < 4; iter++)
   {
-    bool force_cache = (bool) iter/2;
-    bool force_cache_reciprocal = (bool) iter%2;
+    bool force_cache = static_cast<bool> (iter/2);
+    bool force_cache_reciprocal = static_cast<bool> (iter%2);
     pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
     // Ensure that, when force_cache is not set, we are robust to the wrong input
     if (force_cache)
@@ -212,8 +212,8 @@ TEST (PCL, SampleConsensusPrerejective)
   using PointT = pcl::PointXYZ;
   for (int iter = 0; iter < 4; iter++)
   {
-    bool force_cache = (bool) iter/2;
-    bool force_cache_reciprocal = (bool) iter%2;
+    bool force_cache = static_cast<bool> (iter/2);
+    bool force_cache_reciprocal = static_cast<bool> (iter%2);
     pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
     // Ensure that, when force_cache is not set, we are robust to the wrong input
     if (force_cache)
diff --git a/test/registration/test_warps.cpp b/test/registration/test_warps.cpp
index fde72dc2..488e9eb8 100644
--- a/test/registration/test_warps.cpp
+++ b/test/registration/test_warps.cpp
@@ -35,6 +35,8 @@
  *
  */
 
+#include <Eigen/Geometry> // for Quaternionf
+
 #include <pcl/test/gtest.h>
 
 #include <pcl/point_types.h>
diff --git a/test/sample_consensus/test_sample_consensus.cpp b/test/sample_consensus/test_sample_consensus.cpp
index a37d9912..5d56c571 100644
--- a/test/sample_consensus/test_sample_consensus.cpp
+++ b/test/sample_consensus/test_sample_consensus.cpp
@@ -99,7 +99,7 @@ TYPED_TEST(SacTest, InfiniteLoop)
 
   const unsigned point_count = 100;
   PointCloud<PointXYZ> cloud;
-  cloud.points.resize (point_count);
+  cloud.resize (point_count);
   for (unsigned idx = 0; idx < point_count; ++idx)
   {
     cloud[idx].x = static_cast<float> (idx);
@@ -110,6 +110,16 @@ TYPED_TEST(SacTest, InfiniteLoop)
   SampleConsensusModelSpherePtr model (new SampleConsensusModelSphere<PointXYZ> (cloud.makeShared ()));
   TypeParam sac (model, 0.03);
 
+  // This test sometimes fails for LMedS on azure, but always passes when run locally.
+  // Enable all output for LMedS, so that when it fails next time, we hopefully see why.
+  // This can be removed again when the failure reason is found and fixed.
+  int debug_verbosity_level = 0;
+  const auto previous_verbosity_level = pcl::console::getVerbosityLevel();
+  if (std::is_same<TypeParam, LeastMedianSquares<PointXYZ>>::value) {
+    debug_verbosity_level = 2;
+    pcl::console::setVerbosityLevel(pcl::console::L_VERBOSE);
+  }
+
   // Set up timed conditions
   std::condition_variable cv;
   std::mutex mtx;
@@ -117,7 +127,7 @@ TYPED_TEST(SacTest, InfiniteLoop)
   // Create the RANSAC object
   std::thread thread ([&] ()
   {
-    sac.computeModel (0);
+    sac.computeModel (debug_verbosity_level);
 
     // Notify things are done
     std::lock_guard<std::mutex> lock (mtx);
@@ -135,6 +145,8 @@ TYPED_TEST(SacTest, InfiniteLoop)
   // release lock to avoid deadlock
   lock.unlock();
   thread.join ();
+
+  pcl::console::setVerbosityLevel(previous_verbosity_level); // reset verbosity level
 }
 
 int
diff --git a/test/sample_consensus/test_sample_consensus_line_models.cpp b/test/sample_consensus/test_sample_consensus_line_models.cpp
index 1196190e..b564092a 100644
--- a/test/sample_consensus/test_sample_consensus_line_models.cpp
+++ b/test/sample_consensus/test_sample_consensus_line_models.cpp
@@ -55,7 +55,7 @@ TEST (SampleConsensusModelLine, RANSAC)
 {
   // Use a custom point cloud for these tests until we need something better
   PointCloud<PointXYZ> cloud;
-  cloud.points.resize (10);
+  cloud.resize (10);
 
   cloud[0].getVector3fMap () <<  1.0f,  2.00f,  3.00f;
   cloud[1].getVector3fMap () <<  4.0f,  5.00f,  6.00f;
@@ -78,11 +78,11 @@ TEST (SampleConsensusModelLine, RANSAC)
   bool result = sac.computeModel ();
   ASSERT_TRUE (result);
 
-  std::vector<int> sample;
+  pcl::Indices sample;
   sac.getModel (sample);
   EXPECT_EQ (2, sample.size ());
 
-  std::vector<int> inliers;
+  pcl::Indices inliers;
   sac.getInliers (inliers);
   EXPECT_EQ (8, inliers.size ());
 
@@ -110,7 +110,7 @@ TEST (SampleConsensusModelLine, RANSAC)
 TEST (SampleConsensusModelLine, OnGroundPlane)
 {
   PointCloud<PointXYZ> cloud;
-  cloud.points.resize (10);
+  cloud.resize (10);
 
   // All the points are on the ground plane (z=0).
   // The line is parallel to the x axis, so all the inlier points have the same z and y coordinates.
@@ -136,7 +136,7 @@ TEST (SampleConsensusModelLine, OnGroundPlane)
   bool result = sac.computeModel ();
   ASSERT_TRUE (result);
 
-  std::vector<int> inliers;
+  pcl::Indices inliers;
   sac.getInliers (inliers);
   EXPECT_EQ (6, inliers.size ());
 
@@ -188,11 +188,11 @@ TEST (SampleConsensusModelParallelLine, RANSAC)
   bool result = sac.computeModel ();
   ASSERT_TRUE (result);
 
-  std::vector<int> sample;
+  pcl::Indices sample;
   sac.getModel (sample);
   EXPECT_EQ (2, sample.size ());
 
-  std::vector<int> inliers;
+  pcl::Indices inliers;
   sac.getInliers (inliers);
   EXPECT_EQ (6, inliers.size ());
 
diff --git a/test/sample_consensus/test_sample_consensus_plane_models.cpp b/test/sample_consensus/test_sample_consensus_plane_models.cpp
index 5eb91e50..6f9ede77 100644
--- a/test/sample_consensus/test_sample_consensus_plane_models.cpp
+++ b/test/sample_consensus/test_sample_consensus_plane_models.cpp
@@ -41,6 +41,7 @@
 #include <pcl/pcl_tests.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/point_types.h>
+#include <pcl/common/utils.h>
 
 #include <pcl/sample_consensus/msac.h>
 #include <pcl/sample_consensus/lmeds.h>
@@ -61,7 +62,7 @@ using SampleConsensusModelNormalParallelPlanePtr = SampleConsensusModelNormalPar
 
 PointCloud<PointXYZ>::Ptr cloud_ (new PointCloud<PointXYZ> ());
 PointCloud<Normal>::Ptr normals_ (new PointCloud<Normal> ());
-std::vector<int> indices_;
+pcl::Indices indices_;
 float plane_coeffs_[] = {-0.8964f, -0.5868f, -1.208f};
 
 template <typename ModelType, typename SacType>
@@ -76,11 +77,11 @@ void verifyPlaneSac (ModelType& model,
   bool result = sac.computeModel ();
   ASSERT_TRUE (result);
 
-  std::vector<int> sample;
+  pcl::Indices sample;
   sac.getModel (sample);
   EXPECT_EQ (3, sample.size ());
 
-  std::vector<int> inliers;
+  pcl::Indices inliers;
   sac.getInliers (inliers);
   EXPECT_LT (inlier_number, inliers.size ());
 
@@ -247,7 +248,7 @@ TEST (SampleConsensusModelNormalParallelPlane, RANSAC)
   // Use a custom point cloud for these tests until we need something better
   PointCloud<PointXYZ> cloud;
   PointCloud<Normal> normals;
-  cloud.points.resize (10);
+  cloud.resize (10);
   normals.resize (10);
 
   for (std::size_t idx = 0; idx < cloud.size (); ++idx)
@@ -276,7 +277,7 @@ TEST (SampleConsensusModelNormalParallelPlane, RANSAC)
     RandomSampleConsensus<PointXYZ> sac (model, 0.03);
     sac.computeModel();
 
-    std::vector<int> inliers;
+    pcl::Indices inliers;
     sac.getInliers (inliers);
     ASSERT_EQ (cloud.size (), inliers.size ());
   }
@@ -287,7 +288,7 @@ TEST (SampleConsensusModelNormalParallelPlane, RANSAC)
     RandomSampleConsensus<PointXYZ> sac (model, 0.03);
     sac.computeModel ();
 
-    std::vector<int> inliers;
+    pcl::Indices inliers;
     sac.getInliers (inliers);
     ASSERT_EQ (cloud.size (), inliers.size ());
   }
@@ -298,12 +299,153 @@ TEST (SampleConsensusModelNormalParallelPlane, RANSAC)
     RandomSampleConsensus<PointXYZ> sac (model, 0.03);
     sac.computeModel ();
 
-    std::vector<int> inliers;
+    pcl::Indices inliers;
     sac.getInliers (inliers);
     ASSERT_EQ (0, inliers.size ());
   }
 }
 
+//////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT>
+class SampleConsensusModelPlaneTest : private SampleConsensusModelPlane<PointT>
+{
+  public:
+    using SampleConsensusModelPlane<PointT>::SampleConsensusModelPlane;
+    using SampleConsensusModelPlane<PointT>::countWithinDistanceStandard;
+#if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
+    using SampleConsensusModelPlane<PointT>::countWithinDistanceSSE;
+#endif
+#if defined (__AVX__) && defined (__AVX2__)
+    using SampleConsensusModelPlane<PointT>::countWithinDistanceAVX;
+#endif
+};
+
+TEST (SampleConsensusModelPlane, SIMD_countWithinDistance) // Test if all countWithinDistance implementations return the same value
+{
+  const auto seed = static_cast<unsigned> (std::time (nullptr));
+  srand (seed);
+  for (size_t i=0; i<100; i++) // Run as often as you like
+  {
+    // Generate a cloud with 1000 random points
+    PointCloud<PointXYZ> cloud;
+    pcl::Indices indices;
+    cloud.resize (1000);
+    for (std::size_t idx = 0; idx < cloud.size (); ++idx)
+    {
+      cloud[idx].x = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
+      cloud[idx].y = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
+      cloud[idx].z = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
+      if (rand () % 2 == 0)
+      {
+        indices.push_back (static_cast<int> (idx));
+      }
+    }
+    SampleConsensusModelPlaneTest<PointXYZ> model (cloud.makeShared (), indices, true);
+
+    // Generate random model parameters
+    Eigen::VectorXf model_coefficients(4);
+    model_coefficients << 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0,
+                          2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0,
+                          2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0, 0.0;
+    model_coefficients.normalize ();
+    model_coefficients(3) = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0; // Last parameter
+
+    const double threshold = 0.1 * static_cast<double> (rand ()) / RAND_MAX; // threshold in [0; 0.1]
+
+    // The number of inliers is usually somewhere between 0 and 100
+    const auto res_standard = model.countWithinDistanceStandard (model_coefficients, threshold); // Standard
+    PCL_DEBUG ("seed=%lu, i=%lu, model=(%f, %f, %f, %f), threshold=%f, res_standard=%lu\n", seed, i,
+               model_coefficients(0), model_coefficients(1), model_coefficients(2), model_coefficients(3), threshold, res_standard);
+#if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
+    const auto res_sse      = model.countWithinDistanceSSE (model_coefficients, threshold); // SSE
+    ASSERT_EQ (res_standard, res_sse);
+#endif
+#if defined (__AVX__) && defined (__AVX2__)
+    const auto res_avx      = model.countWithinDistanceAVX (model_coefficients, threshold); // AVX
+    ASSERT_EQ (res_standard, res_avx);
+#endif
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT, typename PointNT>
+class SampleConsensusModelNormalPlaneTest : private SampleConsensusModelNormalPlane<PointT, PointNT>
+{
+  public:
+    using SampleConsensusModelNormalPlane<PointT, PointNT>::SampleConsensusModelNormalPlane;
+    using SampleConsensusModelNormalPlane<PointT, PointNT>::setNormalDistanceWeight;
+    using SampleConsensusModelNormalPlane<PointT, PointNT>::setInputNormals;
+    using SampleConsensusModelNormalPlane<PointT, PointNT>::countWithinDistanceStandard;
+#if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
+    using SampleConsensusModelNormalPlane<PointT, PointNT>::countWithinDistanceSSE;
+#endif
+#if defined (__AVX__) && defined (__AVX2__)
+    using SampleConsensusModelNormalPlane<PointT, PointNT>::countWithinDistanceAVX;
+#endif
+};
+
+TEST (SampleConsensusModelNormalPlane, SIMD_countWithinDistance) // Test if all countWithinDistance implementations return the same value
+{
+  const auto seed = static_cast<unsigned> (std::time (nullptr));
+  srand (seed);
+  for (size_t i=0; i<1000; i++) // Run as often as you like
+  {
+    // Generate a cloud with 10000 random points
+    PointCloud<PointXYZ> cloud;
+    PointCloud<Normal> normal_cloud;
+    pcl::Indices indices;
+    cloud.resize (10000);
+    normal_cloud.resize (10000);
+    for (std::size_t idx = 0; idx < cloud.size (); ++idx)
+    {
+      cloud[idx].x = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
+      cloud[idx].y = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
+      cloud[idx].z = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
+      const double a = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
+      const double b = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
+      const double c = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
+      const double factor = 1.0 / sqrt(a * a + b * b + c * c);
+      normal_cloud[idx].normal[0] = a * factor;
+      normal_cloud[idx].normal[1] = b * factor;
+      normal_cloud[idx].normal[2] = c * factor;
+      if (rand () % 4 != 0)
+      {
+        indices.push_back (static_cast<int> (idx));
+      }
+    }
+    SampleConsensusModelNormalPlaneTest<PointXYZ, Normal> model (cloud.makeShared (), indices, true);
+    
+    const double normal_distance_weight = 0.3 * static_cast<double> (rand ()) / RAND_MAX; // in [0; 0.3]
+    model.setNormalDistanceWeight (normal_distance_weight);
+    model.setInputNormals (normal_cloud.makeShared ());
+
+    // Generate random model parameters
+    Eigen::VectorXf model_coefficients(4);
+    model_coefficients << 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0,
+                          2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0,
+                          2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0, 0.0;
+    model_coefficients.normalize ();
+    model_coefficients(3) = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0; // Last parameter
+
+    const double threshold = 0.1 * static_cast<double> (rand ()) / RAND_MAX; // threshold in [0; 0.1]
+
+    // The number of inliers is usually somewhere between 0 and 100
+    const auto res_standard = model.countWithinDistanceStandard (model_coefficients, threshold); // Standard
+    pcl::utils::ignore(res_standard);
+#if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
+    const auto res_sse      = model.countWithinDistanceSSE (model_coefficients, threshold); // SSE
+    EXPECT_LE ((res_standard > res_sse ? res_standard - res_sse : res_sse - res_standard), 2u) << "seed=" << seed << ", i=" << i
+        << ", model=(" << model_coefficients(0) << ", " << model_coefficients(1) << ", " << model_coefficients(2) << ", " << model_coefficients(3)
+        << "), threshold=" << threshold << ", normal_distance_weight=" << normal_distance_weight << ", res_standard=" << res_standard << std::endl;
+#endif
+#if defined (__AVX__) && defined (__AVX2__)
+    const auto res_avx      = model.countWithinDistanceAVX (model_coefficients, threshold); // AVX
+    EXPECT_LE ((res_standard > res_avx ? res_standard - res_avx : res_avx - res_standard), 2u) << "seed=" << seed << ", i=" << i
+        << ", model=(" << model_coefficients(0) << ", " << model_coefficients(1) << ", " << model_coefficients(2) << ", " << model_coefficients(3)
+        << "), threshold=" << threshold << ", normal_distance_weight=" << normal_distance_weight << ", res_standard=" << res_standard << std::endl;
+#endif
+  }
+}
 
 int
 main (int argc, char** argv)
diff --git a/test/sample_consensus/test_sample_consensus_quadric_models.cpp b/test/sample_consensus/test_sample_consensus_quadric_models.cpp
index 80a114d3..c98f02f4 100644
--- a/test/sample_consensus/test_sample_consensus_quadric_models.cpp
+++ b/test/sample_consensus/test_sample_consensus_quadric_models.cpp
@@ -62,7 +62,7 @@ TEST (SampleConsensusModelSphere, RANSAC)
 
   // Use a custom point cloud for these tests until we need something better
   PointCloud<PointXYZ> cloud;
-  cloud.points.resize (10);
+  cloud.resize (10);
   cloud[0].getVector3fMap () << 1.7068f, 1.0684f, 2.2147f;
   cloud[1].getVector3fMap () << 2.4708f, 2.3081f, 1.1736f;
   cloud[2].getVector3fMap () << 2.7609f, 1.9095f, 1.3574f;
@@ -84,11 +84,11 @@ TEST (SampleConsensusModelSphere, RANSAC)
   bool result = sac.computeModel ();
   ASSERT_TRUE (result);
 
-  std::vector<int> sample;
+  pcl::Indices sample;
   sac.getModel (sample);
   EXPECT_EQ (4, sample.size ());
 
-  std::vector<int> inliers;
+  pcl::Indices inliers;
   sac.getInliers (inliers);
   EXPECT_EQ (9, inliers.size ());
 
@@ -107,6 +107,67 @@ TEST (SampleConsensusModelSphere, RANSAC)
   EXPECT_NEAR (2, coeff_refined[2] / coeff_refined[3], 1e-2);
 }
 
+//////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT>
+class SampleConsensusModelSphereTest : private SampleConsensusModelSphere<PointT>
+{
+  public:
+    using SampleConsensusModelSphere<PointT>::SampleConsensusModelSphere;
+    using SampleConsensusModelSphere<PointT>::countWithinDistanceStandard;
+#if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
+    using SampleConsensusModelSphere<PointT>::countWithinDistanceSSE;
+#endif
+#if defined (__AVX__) && defined (__AVX2__)
+    using SampleConsensusModelSphere<PointT>::countWithinDistanceAVX;
+#endif
+};
+
+TEST (SampleConsensusModelSphere, SIMD_countWithinDistance) // Test if all countWithinDistance implementations return the same value
+{
+  const auto seed = static_cast<unsigned> (std::time (nullptr));
+  srand (seed);
+  for (size_t i=0; i<100; i++) // Run as often as you like
+  {
+    // Generate a cloud with 1000 random points
+    PointCloud<PointXYZ> cloud;
+    pcl::Indices indices;
+    cloud.resize (1000);
+    for (std::size_t idx = 0; idx < cloud.size (); ++idx)
+    {
+      cloud[idx].x = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
+      cloud[idx].y = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
+      cloud[idx].z = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
+      if (rand () % 3 != 0)
+      {
+        indices.push_back (static_cast<int> (idx));
+      }
+    }
+    SampleConsensusModelSphereTest<PointXYZ> model (cloud.makeShared (), indices, true);
+
+    // Generate random sphere model parameters
+    Eigen::VectorXf model_coefficients(4);
+    model_coefficients << 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0,
+                          2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0,
+                          2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0,
+                          0.15 * static_cast<float> (rand ()) / RAND_MAX; // center and radius
+
+    const double threshold = 0.15 * static_cast<double> (rand ()) / RAND_MAX; // threshold in [0; 0.1]
+
+    // The number of inliers is usually somewhere between 0 and 10
+    const auto res_standard = model.countWithinDistanceStandard (model_coefficients, threshold); // Standard
+    PCL_DEBUG ("seed=%lu, i=%lu, model=(%f, %f, %f, %f), threshold=%f, res_standard=%lu\n", seed, i,
+               model_coefficients(0), model_coefficients(1), model_coefficients(2), model_coefficients(3), threshold, res_standard);
+#if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
+    const auto res_sse      = model.countWithinDistanceSSE (model_coefficients, threshold); // SSE
+    ASSERT_EQ (res_standard, res_sse);
+#endif
+#if defined (__AVX__) && defined (__AVX2__)
+    const auto res_avx      = model.countWithinDistanceAVX (model_coefficients, threshold); // AVX
+    ASSERT_EQ (res_standard, res_avx);
+#endif
+  }
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 TEST (SampleConsensusModelNormalSphere, RANSAC)
 {
@@ -115,7 +176,7 @@ TEST (SampleConsensusModelNormalSphere, RANSAC)
   // Use a custom point cloud for these tests until we need something better
   PointCloud<PointXYZ> cloud;
   PointCloud<Normal> normals;
-  cloud.points.resize (27); normals.points.resize (27);
+  cloud.resize (27); normals.resize (27);
   cloud[ 0].getVector3fMap () << -0.014695f,  0.009549f, 0.954775f;
   cloud[ 1].getVector3fMap () <<  0.014695f,  0.009549f, 0.954775f;
   cloud[ 2].getVector3fMap () << -0.014695f,  0.040451f, 0.954775f;
@@ -183,11 +244,11 @@ TEST (SampleConsensusModelNormalSphere, RANSAC)
   bool result = sac.computeModel ();
   ASSERT_TRUE (result);
 
-  std::vector<int> sample;
+  pcl::Indices sample;
   sac.getModel (sample);
   EXPECT_EQ (4, sample.size ());
 
-  std::vector<int> inliers;
+  pcl::Indices inliers;
   sac.getInliers (inliers);
   EXPECT_EQ (27, inliers.size ());
 
@@ -215,7 +276,7 @@ TEST (SampleConsensusModelCone, RANSAC)
   // Use a custom point cloud for these tests until we need something better
   PointCloud<PointXYZ> cloud;
   PointCloud<Normal> normals;
-  cloud.points.resize (31); normals.points.resize (31);
+  cloud.resize (31); normals.resize (31);
 
   cloud[ 0].getVector3fMap () << -0.011247f, 0.200000f, 0.965384f;
   cloud[ 1].getVector3fMap () <<  0.000000f, 0.200000f, 0.963603f;
@@ -292,11 +353,11 @@ TEST (SampleConsensusModelCone, RANSAC)
   bool result = sac.computeModel ();
   ASSERT_TRUE (result);
 
-  std::vector<int> sample;
+  pcl::Indices sample;
   sac.getModel (sample);
   EXPECT_EQ (3, sample.size ());
 
-  std::vector<int> inliers;
+  pcl::Indices inliers;
   sac.getInliers (inliers);
   EXPECT_EQ (31, inliers.size ());
 
@@ -321,7 +382,7 @@ TEST (SampleConsensusModelCylinder, RANSAC)
   // Use a custom point cloud for these tests until we need something better
   PointCloud<PointXYZ> cloud;
   PointCloud<Normal> normals;
-  cloud.points.resize (20); normals.points.resize (20);
+  cloud.resize (20); normals.resize (20);
 
   cloud[ 0].getVector3fMap () << -0.499902f, 2.199701f, 0.000008f;
   cloud[ 1].getVector3fMap () << -0.875397f, 2.030177f, 0.050104f;
@@ -376,11 +437,11 @@ TEST (SampleConsensusModelCylinder, RANSAC)
   bool result = sac.computeModel ();
   ASSERT_TRUE (result);
 
-  std::vector<int> sample;
+  pcl::Indices sample;
   sac.getModel (sample);
   EXPECT_EQ (2, sample.size ());
 
-  std::vector<int> inliers;
+  pcl::Indices inliers;
   sac.getInliers (inliers);
   EXPECT_EQ (20, inliers.size ());
 
@@ -404,7 +465,7 @@ TEST (SampleConsensusModelCircle2D, RANSAC)
 
   // Use a custom point cloud for these tests until we need something better
   PointCloud<PointXYZ> cloud;
-  cloud.points.resize (18);
+  cloud.resize (18);
 
   cloud[ 0].getVector3fMap () << 3.587751f, -4.190982f, 0.0f;
   cloud[ 1].getVector3fMap () << 3.808883f, -4.412265f, 0.0f;
@@ -435,11 +496,11 @@ TEST (SampleConsensusModelCircle2D, RANSAC)
   bool result = sac.computeModel ();
   ASSERT_TRUE (result);
 
-  std::vector<int> sample;
+  pcl::Indices sample;
   sac.getModel (sample);
   EXPECT_EQ (3, sample.size ());
 
-  std::vector<int> inliers;
+  pcl::Indices inliers;
   sac.getInliers (inliers);
   EXPECT_EQ (17, inliers.size ());
 
@@ -458,6 +519,66 @@ TEST (SampleConsensusModelCircle2D, RANSAC)
   EXPECT_NEAR ( 1, coeff_refined[2], 1e-3);
 }
 
+//////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT>
+class SampleConsensusModelCircle2DTest : private SampleConsensusModelCircle2D<PointT>
+{
+  public:
+    using SampleConsensusModelCircle2D<PointT>::SampleConsensusModelCircle2D;
+    using SampleConsensusModelCircle2D<PointT>::countWithinDistanceStandard;
+#if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
+    using SampleConsensusModelCircle2D<PointT>::countWithinDistanceSSE;
+#endif
+#if defined (__AVX__) && defined (__AVX2__)
+    using SampleConsensusModelCircle2D<PointT>::countWithinDistanceAVX;
+#endif
+};
+
+TEST (SampleConsensusModelCircle2D, SIMD_countWithinDistance) // Test if all countWithinDistance implementations return the same value
+{
+  const auto seed = static_cast<unsigned> (std::time (nullptr));
+  srand (seed);
+  for (size_t i=0; i<100; i++) // Run as often as you like
+  {
+    // Generate a cloud with 1000 random points
+    PointCloud<PointXYZ> cloud;
+    pcl::Indices indices;
+    cloud.resize (1000);
+    for (std::size_t idx = 0; idx < cloud.size (); ++idx)
+    {
+      cloud[idx].x = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
+      cloud[idx].y = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
+      cloud[idx].z = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
+      if (rand () % 2 == 0)
+      {
+        indices.push_back (static_cast<int> (idx));
+      }
+    }
+    SampleConsensusModelCircle2DTest<PointXYZ> model (cloud.makeShared (), indices, true);
+
+    // Generate random circle model parameters
+    Eigen::VectorXf model_coefficients(3);
+    model_coefficients << 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0,
+                          2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0,
+                          0.1 * static_cast<float> (rand ()) / RAND_MAX; // center and radius
+
+    const double threshold = 0.1 * static_cast<double> (rand ()) / RAND_MAX; // threshold in [0; 0.1]
+
+    // The number of inliers is usually somewhere between 0 and 20
+    const auto res_standard = model.countWithinDistanceStandard (model_coefficients, threshold); // Standard
+    PCL_DEBUG ("seed=%lu, i=%lu, model=(%f, %f, %f), threshold=%f, res_standard=%lu\n", seed, i,
+               model_coefficients(0), model_coefficients(1), model_coefficients(2), threshold, res_standard);
+#if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
+    const auto res_sse      = model.countWithinDistanceSSE (model_coefficients, threshold); // SSE
+    ASSERT_EQ (res_standard, res_sse);
+#endif
+#if defined (__AVX__) && defined (__AVX2__)
+    const auto res_avx      = model.countWithinDistanceAVX (model_coefficients, threshold); // AVX
+    ASSERT_EQ (res_standard, res_avx);
+#endif
+  }
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 TEST (SampleConsensusModelCircle3D, RANSAC)
 {
@@ -465,7 +586,7 @@ TEST (SampleConsensusModelCircle3D, RANSAC)
 
   // Use a custom point cloud for these tests until we need something better
   PointCloud<PointXYZ> cloud;
-  cloud.points.resize (20);
+  cloud.resize (20);
 
   cloud[ 0].getVector3fMap () << 1.00000000f, 5.0000000f, -2.9000001f;
   cloud[ 1].getVector3fMap () << 1.03420200f, 5.0000000f, -2.9060307f;
@@ -498,11 +619,11 @@ TEST (SampleConsensusModelCircle3D, RANSAC)
   bool result = sac.computeModel ();
   ASSERT_TRUE (result);
 
-  std::vector<int> sample;
+  pcl::Indices sample;
   sac.getModel (sample);
   EXPECT_EQ (3, sample.size ());
 
-  std::vector<int> inliers;
+  pcl::Indices inliers;
   sac.getInliers (inliers);
   EXPECT_EQ (18, inliers.size ());
 
diff --git a/test/search/test_flann_search.cpp b/test/search/test_flann_search.cpp
index 6664bb5e..8d1786c0 100644
--- a/test/search/test_flann_search.cpp
+++ b/test/search/test_flann_search.cpp
@@ -38,9 +38,10 @@
 
 #include <iostream>
 #include <pcl/test/gtest.h>
+#include <pcl/common/io.h> // for copyPointCloud
 #include <pcl/common/distances.h>
 #include <pcl/common/time.h>
-#include <pcl/search/pcl_search.h>
+#include <pcl/search/kdtree.h> // for pcl::search::KdTree
 #include <pcl/search/impl/flann_search.hpp>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -56,19 +57,18 @@ init ()
   for (float z = -0.5f; z <= 0.5f; z += resolution)
     for (float y = -0.5f; y <= 0.5f; y += resolution)
       for (float x = -0.5f; x <= 0.5f; x += resolution)
-        cloud.points.emplace_back(x, y, z);
+        cloud.emplace_back(x, y, z);
   cloud.width = cloud.size ();
   cloud.height = 1;
 
+  srand (int (time (nullptr)));
+  // Randomly create a new point cloud, use points.emplace_back
   cloud_big.width = 640;
   cloud_big.height = 480;
-  srand (int (time (nullptr)));
-  // Randomly create a new point cloud
   for (std::size_t i = 0; i < cloud_big.width * cloud_big.height; ++i)
-    cloud_big.points.emplace_back(
-                                  float (1024 * rand () / (RAND_MAX + 1.0)),
-                                  float (1024 * rand () / (RAND_MAX + 1.0)),
-                                  float (1024 * rand () / (RAND_MAX + 1.0)));
+    cloud_big.points.emplace_back(static_cast<float>(1024 * rand() / (RAND_MAX + 1.0)),
+                                  static_cast<float>(1024 * rand() / (RAND_MAX + 1.0)),
+                                  static_cast<float>(1024 * rand() / (RAND_MAX + 1.0)));
 }
 
 
@@ -94,7 +94,7 @@ TEST (PCL, FlannSearch_nearestKSearch)
     ++counter;
   }
 
-  std::vector<int> k_indices;
+  pcl::Indices k_indices;
   k_indices.resize (no_of_neighbors);
   std::vector<float> k_distances;
   k_distances.resize (no_of_neighbors);
@@ -105,7 +105,7 @@ TEST (PCL, FlannSearch_nearestKSearch)
   EXPECT_EQ (k_indices.size (), no_of_neighbors);
 
   // Check if all found neighbors have distance smaller than max_dist
-  for (const int &k_index : k_indices)
+  for (const auto &k_index : k_indices)
   {
     const PointXYZ& point = cloud[k_index];
     bool ok = euclideanDistance (test_point, point) <= max_dist;
@@ -143,15 +143,15 @@ TEST (PCL, FlannSearch_differentPointT)
 
 
   std::vector< std::vector< float > > dists;
-  std::vector< std::vector< int > > indices;
-  FlannSearch.nearestKSearchT (cloud_rgb, std::vector<int> (),no_of_neighbors,indices,dists);
+  std::vector< pcl::Indices > indices;
+  FlannSearch.nearestKSearchT (cloud_rgb, pcl::Indices (),no_of_neighbors,indices,dists);
 
-  std::vector<int> k_indices;
+  pcl::Indices k_indices;
   k_indices.resize (no_of_neighbors);
   std::vector<float> k_distances;
   k_distances.resize (no_of_neighbors);
 
-  //vector<int> k_indices_t;
+  //pcl::Indices k_indices_t;
   //k_indices_t.resize (no_of_neighbors);
   //vector<float> k_distances_t;
   //k_distances_t.resize (no_of_neighbors);
@@ -165,8 +165,8 @@ TEST (PCL, FlannSearch_differentPointT)
     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]);
-      //EXPECT_TRUE (k_indices[j] == k_indices_t[j]);
-      //EXPECT_TRUE (k_distances[j] == k_distances_t[j]);
+      //EXPECT_EQ (k_indices[j], k_indices_t[j]);
+      //EXPECT_EQ (k_distances[j], k_distances_t[j]);
     }
 
   }
@@ -184,10 +184,10 @@ TEST (PCL, FlannSearch_multipointKnnSearch)
   FlannSearch.setInputCloud (cloud_big.makeShared ());
 
   std::vector< std::vector< float > > dists;
-  std::vector< std::vector< int > > indices;
-  FlannSearch.nearestKSearch (cloud_big, std::vector<int>(),no_of_neighbors,indices,dists);
+  std::vector< pcl::Indices > indices;
+  FlannSearch.nearestKSearch (cloud_big, pcl::Indices(),no_of_neighbors,indices,dists);
 
-  std::vector<int> k_indices;
+  pcl::Indices k_indices;
   k_indices.resize (no_of_neighbors);
   std::vector<float> k_distances;
   k_distances.resize (no_of_neighbors);
@@ -217,15 +217,15 @@ TEST (PCL, FlannSearch_knnByIndex)
   flann_search.setInputCloud (cloud_big.makeShared ());
 
   std::vector< std::vector< float > > dists;
-  std::vector< std::vector< int > > indices;
-  std::vector< int > query_indices;
+  std::vector< pcl::Indices > indices;
+  pcl::Indices query_indices;
   for (std::size_t i = 0; i<cloud_big.size (); i+=2)
   {
     query_indices.push_back (int (i));
   }
   flann_search.nearestKSearch (cloud_big, query_indices,no_of_neighbors,indices,dists);
 
-  std::vector<int> k_indices;
+  pcl::Indices k_indices;
   k_indices.resize (no_of_neighbors);
   std::vector<float> k_distances;
   k_distances.resize (no_of_neighbors);
@@ -256,7 +256,7 @@ TEST (PCL, FlannSearch_compareToKdTreeFlann)
 {
 
   int no_of_neighbors=3;
-  std::vector<int> k_indices;
+  pcl::Indices k_indices;
   k_indices.resize (no_of_neighbors);
   std::vector<float> k_distances;
   k_distances.resize (no_of_neighbors);
@@ -289,9 +289,9 @@ TEST (PCL, FlannSearch_compareToKdTreeFlann)
       kdtree_search->nearestKSearch (point, no_of_neighbors, k_indices, k_distances);
   }
 
-  std::vector<std::vector<int> > indices_flann;
+  std::vector<pcl::Indices> indices_flann;
   std::vector<std::vector<float> > dists_flann;
-  std::vector<std::vector<int> > indices_tree;
+  std::vector<pcl::Indices> indices_tree;
   std::vector<std::vector<float> > dists_tree;
   indices_flann.resize (cloud_big.size ());
   dists_flann.resize (cloud_big.size ());
@@ -307,11 +307,11 @@ TEST (PCL, FlannSearch_compareToKdTreeFlann)
 
   {
     ScopeTime scopeTime ("FLANN multi nearestKSearch");
-    flann_search->nearestKSearch (cloud_big, std::vector<int> (), no_of_neighbors, indices_flann,dists_flann);
+    flann_search->nearestKSearch (cloud_big, pcl::Indices (), no_of_neighbors, indices_flann,dists_flann);
   }
   {
     ScopeTime scopeTime ("kd tree multi nearestKSearch");
-    kdtree_search->nearestKSearch (cloud_big, std::vector<int> (), no_of_neighbors, indices_tree,dists_tree);
+    kdtree_search->nearestKSearch (cloud_big, pcl::Indices (), no_of_neighbors, indices_tree,dists_tree);
   }
 
   ASSERT_EQ (indices_flann.size (), dists_flann.size ());
@@ -332,7 +332,7 @@ TEST (PCL, FlannSearch_compareToKdTreeFlann)
   }
 
 
-  std::vector<int> query_indices;
+  pcl::Indices query_indices;
   for (std::size_t i = 0; i<cloud_big.size (); i+=2)
     query_indices.push_back (int (i));
 
diff --git a/test/search/test_kdtree.cpp b/test/search/test_kdtree.cpp
index 46b797b3..a47c07f8 100644
--- a/test/search/test_kdtree.cpp
+++ b/test/search/test_kdtree.cpp
@@ -37,6 +37,7 @@
  */
 #include <iostream>
 #include <pcl/test/gtest.h>
+#include <pcl/common/io.h> // for copyPointCloud
 #include <pcl/common/time.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -54,18 +55,18 @@ init ()
   for (float z = -0.5f; z <= 0.5f; z += resolution)
     for (float y = -0.5f; y <= 0.5f; y += resolution)
       for (float x = -0.5f; x <= 0.5f; x += resolution)
-        cloud.points.emplace_back(x, y, z);
+        cloud.emplace_back(x, y, z);
   cloud.width = cloud.size ();
   cloud.height = 1;
 
+  srand (static_cast<unsigned int> (time (nullptr)));
   cloud_big.width = 640;
   cloud_big.height = 480;
-  srand (static_cast<unsigned int> (time (nullptr)));
-  // Randomly create a new point cloud
+  // Randomly create a new point cloud, use points.emplace_back
   for (std::size_t i = 0; i < cloud_big.width * cloud_big.height; ++i)
-    cloud_big.points.emplace_back(static_cast<float> (1024 * rand () / (RAND_MAX + 1.0)), 
-                                          static_cast<float> (1024 * rand () / (RAND_MAX + 1.0)),
-                                          static_cast<float> (1024 * rand () / (RAND_MAX + 1.0)));
+    cloud_big.points.emplace_back(static_cast<float>(1024 * rand() / (RAND_MAX + 1.0)),
+                                  static_cast<float>(1024 * rand() / (RAND_MAX + 1.0)),
+                                  static_cast<float>(1024 * rand() / (RAND_MAX + 1.0)));
 }
 
 /* Test for KdTree nearestKSearch */TEST (PCL, KdTree_nearestKSearch)
@@ -89,7 +90,7 @@ init ()
     ++counter;
   }
 
-  std::vector<int> k_indices;
+  pcl::Indices k_indices;
   k_indices.resize (no_of_neighbors);
   std::vector<float> k_distances;
   k_distances.resize (no_of_neighbors);
@@ -100,7 +101,7 @@ init ()
   EXPECT_EQ (k_indices.size (), no_of_neighbors);
 
   // Check if all found neighbors have distance smaller than max_dist
-  for (const int &k_index : k_indices)
+  for (const auto &k_index : k_indices)
   {
     const PointXYZ& point = cloud[k_index];
     bool ok = euclideanDistance (test_point, point) <= max_dist;
@@ -136,15 +137,15 @@ TEST (PCL, KdTree_differentPointT)
   copyPointCloud (cloud_big, cloud_rgb);
 
   std::vector< std::vector< float > > dists;
-  std::vector< std::vector< int > > indices;
-  kdtree.nearestKSearchT (cloud_rgb, std::vector<int> (),no_of_neighbors,indices,dists);
+  std::vector< pcl::Indices > indices;
+  kdtree.nearestKSearchT (cloud_rgb, pcl::Indices (),no_of_neighbors,indices,dists);
 
-  std::vector<int> k_indices;
+  pcl::Indices k_indices;
   k_indices.resize (no_of_neighbors);
   std::vector<float> k_distances;
   k_distances.resize (no_of_neighbors);
 
-  std::vector<int> k_indices_t;
+  pcl::Indices k_indices_t;
   k_indices_t.resize (no_of_neighbors);
   std::vector<float> k_distances_t;
   k_distances_t.resize (no_of_neighbors);
@@ -158,8 +159,8 @@ TEST (PCL, KdTree_differentPointT)
     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]);
-      EXPECT_TRUE (k_indices[j] == k_indices_t[j]);
-      EXPECT_TRUE (k_distances[j] == k_distances_t[j]);
+      EXPECT_EQ (k_indices[j], k_indices_t[j]);
+      EXPECT_EQ (k_distances[j], k_distances_t[j]);
     }
   }
 }
@@ -174,10 +175,10 @@ TEST (PCL, KdTree_multipointKnnSearch)
   kdtree.setInputCloud (cloud_big.makeShared ());
 
   std::vector< std::vector< float > > dists;
-  std::vector< std::vector< int > > indices;
-  kdtree.nearestKSearch (cloud_big, std::vector<int> (),no_of_neighbors,indices,dists);
+  std::vector< pcl::Indices > indices;
+  kdtree.nearestKSearch (cloud_big, pcl::Indices (),no_of_neighbors,indices,dists);
 
-  std::vector<int> k_indices;
+  pcl::Indices k_indices;
   k_indices.resize (no_of_neighbors);
   std::vector<float> k_distances;
   k_distances.resize (no_of_neighbors);
@@ -206,4 +207,3 @@ main (int argc, char** argv)
 
   return (RUN_ALL_TESTS ());
 }
-
diff --git a/test/search/test_octree.cpp b/test/search/test_octree.cpp
index 6328ca4e..65208b75 100644
--- a/test/search/test_octree.cpp
+++ b/test/search/test_octree.cpp
@@ -40,7 +40,7 @@
 #include <pcl/common/time.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
-#include <pcl/search/pcl_search.h>
+#include <pcl/search/octree.h> // for pcl::search::Octree
 
 using namespace pcl;
 using namespace octree;
@@ -76,14 +76,16 @@ TEST (PCL, Octree_Pointcloud_Nearest_K_Neighbour_Search)
   // instantiate point cloud
   PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
 
-  srand (static_cast<unsigned int> (time (nullptr)));
+  const unsigned int seed = time (nullptr);
+  srand (seed);
+  SCOPED_TRACE("seed=" + std::to_string(seed));
 
   std::priority_queue<prioPointQueueEntry, pcl::PointCloud<prioPointQueueEntry>::VectorType> pointCandidates;
 
   // create octree
   pcl::search::Octree<PointXYZ> octree(0.1);
 
-  std::vector<int> k_indices;
+  pcl::Indices k_indices;
   std::vector<float> k_sqr_distances;
 
   std::vector<int> k_indices_bruteforce;
@@ -173,7 +175,9 @@ TEST (PCL, Octree_Pointcloud_Approx_Nearest_Neighbour_Search)
   // instantiate point cloud
   PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
 
-  srand (time (NULL));
+  const unsigned int seed = time (nullptr);
+  srand (seed);
+  SCOPED_TRACE("seed=" + std::to_string(seed));
 
   double voxelResolution = 0.1;
 
@@ -225,7 +229,7 @@ TEST (PCL, Octree_Pointcloud_Approx_Nearest_Neighbour_Search)
     }
   }
   // we should have found the absolute nearest neighbor at least once
-  //ASSERT_EQ ( (bestMatchCount > 0) , true);
+  //ASSERT_GT (bestMatchCount, 0);
 }
 #endif
 #if 0
@@ -259,7 +263,7 @@ TEST (PCL, Octree_RadiusSearch_GPU)
   radiuses.push_back(radius);
   radiuses.push_back(radius);
   radiuses.push_back(radius);
-  std::vector<std::vector<int> > k_indices;
+  std::vector<pcl::Indices > k_indices;
   std::vector<std::vector<float> > k_distances;
   int max_nn = -1;
 
@@ -275,7 +279,9 @@ TEST (PCL, Octree_Pointcloud_Neighbours_Within_Radius_Search)
   PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
   PointCloud<PointXYZ>::Ptr cloudOut (new PointCloud<PointXYZ> ());
 
-  srand (static_cast<unsigned int> (time (nullptr)));
+  const unsigned int seed = time (nullptr);
+  srand (seed);
+  SCOPED_TRACE("seed=" + std::to_string(seed));
 
   for (unsigned int test_id = 0; test_id < test_runs; test_id++)
   {
@@ -318,7 +324,7 @@ TEST (PCL, Octree_Pointcloud_Neighbours_Within_Radius_Search)
       }
     }
 
-    std::vector<int> cloudNWRSearch;
+    pcl::Indices cloudNWRSearch;
     std::vector<float> cloudNWRRadius;
 
     // execute octree radius search
@@ -328,23 +334,20 @@ TEST (PCL, Octree_Pointcloud_Neighbours_Within_Radius_Search)
     ASSERT_EQ ( cloudNWRRadius.size() , cloudSearchBruteforce.size());
 
     // check if result from octree radius search can be also found in bruteforce search
-    std::vector<int>::const_iterator current = cloudNWRSearch.begin();
-    while (current != cloudNWRSearch.end())
+    for (const auto& current : cloudNWRSearch)
     {
       pointDist = sqrt (
-          ((*cloudIn)[*current].x-searchPoint.x) * ((*cloudIn)[*current].x-searchPoint.x) +
-          ((*cloudIn)[*current].y-searchPoint.y) * ((*cloudIn)[*current].y-searchPoint.y) +
-          ((*cloudIn)[*current].z-searchPoint.z) * ((*cloudIn)[*current].z-searchPoint.z)
+          ((*cloudIn)[current].x-searchPoint.x) * ((*cloudIn)[current].x-searchPoint.x) +
+          ((*cloudIn)[current].y-searchPoint.y) * ((*cloudIn)[current].y-searchPoint.y) +
+          ((*cloudIn)[current].z-searchPoint.z) * ((*cloudIn)[current].z-searchPoint.z)
       );
 
-      ASSERT_EQ ( (pointDist<=searchRadius) , true);
-
-      ++current;
+      ASSERT_LE (pointDist, searchRadius);
     }
 
     // check if result limitation works
     octree->radiusSearch(searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius, 5);
-    ASSERT_EQ ( cloudNWRRadius.size() <= 5, true);
+    ASSERT_LE (cloudNWRRadius.size(), 5);
   }
 }
 
diff --git a/test/search/test_organized.cpp b/test/search/test_organized.cpp
index 3a407c40..c974b3e7 100644
--- a/test/search/test_organized.cpp
+++ b/test/search/test_organized.cpp
@@ -79,12 +79,14 @@ TEST (PCL, Organized_Neighbor_Pointcloud_Nearest_K_Neighbour_Search)
   // instantiate point cloud
   PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
 
-  srand (int (time (nullptr)));
+  const unsigned int seed = time (nullptr);
+  srand (seed);
+  SCOPED_TRACE("seed=" + std::to_string(seed));
 
   // create organized search
   search::OrganizedNeighbor<PointXYZ> organizedNeighborSearch;
 
-  std::vector<int> k_indices;
+  pcl::Indices k_indices;
   std::vector<float> k_sqr_distances;
 
   std::vector<int> k_indices_bruteforce;
@@ -180,7 +182,9 @@ TEST (PCL, Organized_Neighbor_Pointcloud_Neighbours_Within_Radius_Search)
 {
   constexpr unsigned int test_runs = 10;
 
-  srand (int (time (nullptr)));
+  const unsigned int seed = time (nullptr);
+  srand (seed);
+  SCOPED_TRACE("seed=" + std::to_string(seed));
 
   search::OrganizedNeighbor<PointXYZ> organizedNeighborSearch;
 
@@ -236,7 +240,7 @@ TEST (PCL, Organized_Neighbor_Pointcloud_Neighbours_Within_Radius_Search)
       }
     }
 
-    std::vector<int> cloudNWRSearch;
+    pcl::Indices cloudNWRSearch;
     std::vector<float> cloudNWRRadius;
 
     // execute organized search
@@ -244,35 +248,28 @@ TEST (PCL, Organized_Neighbor_Pointcloud_Neighbours_Within_Radius_Search)
     organizedNeighborSearch.radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius);
 
     // check if result from organized radius search can be also found in bruteforce search
-    std::vector<int>::const_iterator current = cloudNWRSearch.begin();
-    while (current != cloudNWRSearch.end())
+    for (const auto& current : cloudNWRSearch)
     {
       pointDist = sqrt (
-          ((*cloudIn)[*current].x-searchPoint.x) * ((*cloudIn)[*current].x-searchPoint.x) +
-          ((*cloudIn)[*current].y-searchPoint.y) * ((*cloudIn)[*current].y-searchPoint.y) +
-          ((*cloudIn)[*current].z-searchPoint.z) * ((*cloudIn)[*current].z-searchPoint.z)
+          ((*cloudIn)[current].x-searchPoint.x) * ((*cloudIn)[current].x-searchPoint.x) +
+          ((*cloudIn)[current].y-searchPoint.y) * ((*cloudIn)[current].y-searchPoint.y) +
+          ((*cloudIn)[current].z-searchPoint.z) * ((*cloudIn)[current].z-searchPoint.z)
       );
 
-      ASSERT_EQ ( (pointDist<=searchRadius) , true);
-
-      ++current;
+      ASSERT_LE (pointDist, searchRadius);
     }
 
 
     // check if bruteforce result from organized radius search can be also found in bruteforce search
-    current = cloudSearchBruteforce.begin();
-    while (current != cloudSearchBruteforce.end())
+    for (const auto& current : cloudSearchBruteforce)
     {
-
       pointDist = sqrt (
-          ((*cloudIn)[*current].x-searchPoint.x) * ((*cloudIn)[*current].x-searchPoint.x) +
-          ((*cloudIn)[*current].y-searchPoint.y) * ((*cloudIn)[*current].y-searchPoint.y) +
-          ((*cloudIn)[*current].z-searchPoint.z) * ((*cloudIn)[*current].z-searchPoint.z)
+          ((*cloudIn)[current].x-searchPoint.x) * ((*cloudIn)[current].x-searchPoint.x) +
+          ((*cloudIn)[current].y-searchPoint.y) * ((*cloudIn)[current].y-searchPoint.y) +
+          ((*cloudIn)[current].z-searchPoint.z) * ((*cloudIn)[current].z-searchPoint.z)
       );
 
-      ASSERT_EQ ( (pointDist<=searchRadius) , true);
-
-      ++current;
+      ASSERT_LE (pointDist, searchRadius);
     }
 
     ASSERT_EQ (cloudNWRRadius.size() , cloudSearchBruteforce.size ());
@@ -280,7 +277,7 @@ TEST (PCL, Organized_Neighbor_Pointcloud_Neighbours_Within_Radius_Search)
     // check if result limitation works
     organizedNeighborSearch.radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius, 5);
 
-    ASSERT_EQ (cloudNWRRadius.size () <= 5, true);
+    ASSERT_LE (cloudNWRRadius.size (), 5);
   }
 }
 
diff --git a/test/search/test_organized_index.cpp b/test/search/test_organized_index.cpp
index cf821b82..683e7476 100644
--- a/test/search/test_organized_index.cpp
+++ b/test/search/test_organized_index.cpp
@@ -39,7 +39,6 @@
 #include <vector>
 #include <cstdio>
 #include <pcl/common/time.h>
-#include <pcl/common/geometry.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl/io/pcd_io.h>
@@ -49,6 +48,16 @@ using namespace pcl;
 
 std::string pcd_filename;
 
+// Here we want a very precise distance function, speed is less important. So we use
+// double precision, unlike euclideanDistance() in pcl/common/distances and distance()
+// in pcl/common/geometry which use float (single precision) and possibly vectorization
+template <typename PointT> inline double
+point_distance(const PointT& p1, const PointT& p2)
+{
+  const double x_diff = p1.x - p2.x, y_diff = p1.y - p2.y, z_diff = p1.z - p2.z;
+  return std::sqrt(x_diff * x_diff + y_diff * y_diff + z_diff * z_diff);
+}
+
 // helper class for priority queue
 class prioPointQueueEntry
 {
@@ -79,7 +88,9 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Nearest_K_Neighbour_Search)
   // instantiate point cloud
   PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
 
-  srand (time (NULL));
+  const unsigned int seed = time (nullptr);
+  srand (seed);
+  SCOPED_TRACE("seed=" + std::to_string(seed));
 
   // create organized search
   search::OrganizedNeighbor<PointXYZ> organizedNeighborSearch;
@@ -135,7 +146,7 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Nearest_K_Neighbour_Search)
 
     for (auto it = cloudIn->begin(); it != cloudIn->end(); ++it)
     {
-      const auto pointDist = geometry::distance(*it, searchPoint);
+      const auto pointDist = point_distance(*it, searchPoint);
       prioPointQueueEntry pointEntry (*it, pointDist, std::distance(cloudIn->begin(), it));
       pointCandidates.push (pointEntry);
     }
@@ -174,12 +185,14 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Nearest_K_Neighbour_Search_Kinec
 
   // instantiate point cloud
 
-  srand (time (NULL));
+  const unsigned int seed = time (nullptr);
+  srand (seed);
+  SCOPED_TRACE("seed=" + std::to_string(seed));
 
   // create organized search
   search::OrganizedNeighbor<PointXYZ> organizedNeighborSearch;
 
-  std::vector<int> k_indices;
+  pcl::Indices k_indices;
   std::vector<float> k_sqr_distances;
 
   std::vector<int> k_indices_bruteforce;
@@ -226,7 +239,7 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Nearest_K_Neighbour_Search_Kinec
     // push all points and their distance to the search point into a priority queue - bruteforce approach.
     for (auto it = cloudIn->begin(); it != cloudIn->end(); ++it)
     {
-      const auto pointDist = geometry::distance(*it, searchPoint);
+      const auto pointDist = point_distance(*it, searchPoint);
       prioPointQueueEntry pointEntry (*it, pointDist, std::distance(cloudIn->begin(), it));
       pointCandidates.push (pointEntry);
     }
@@ -251,7 +264,9 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search)
 {
   constexpr unsigned int test_runs = 10;
 
-  srand (time (NULL));
+  const unsigned int seed = time (nullptr);
+  srand (seed);
+  SCOPED_TRACE("seed=" + std::to_string(seed));
   search::OrganizedNeighbor<PointXYZ> organizedNeighborSearch;
 
   std::vector<int> k_indices;
@@ -299,7 +314,7 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search)
 
     for (auto it = cloudIn->points.cbegin(); it != cloudIn->points.cend(); ++it)
     {
-      const auto pointDist = geometry::distance(*it, searchPoint);
+      const auto pointDist = point_distance(*it, searchPoint);
 
       if (pointDist <= searchRadius)
       {
@@ -308,7 +323,7 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search)
       }
     }
 
-    std::vector<int> cloudNWRSearch;
+    pcl::Indices cloudNWRSearch;
     std::vector<float> cloudNWRRadius;
 
     organizedNeighborSearch.setInputCloud (cloudIn);
@@ -319,16 +334,16 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search)
     // check if result from organized radius search can be also found in bruteforce search
     for (const auto it : cloudNWRSearch)
     {
-      auto const pointDist = geometry::distance((*cloudIn)[it], searchPoint);
-      ASSERT_EQ ( (pointDist <= searchRadius) , true);
+      const auto pointDist = point_distance((*cloudIn)[it], searchPoint);
+      ASSERT_LE (pointDist, searchRadius);
     }
 
 
     // check if bruteforce result from organized radius search can be also found in bruteforce search
     for (const auto it : cloudSearchBruteforce)
     {
-      const auto pointDist = geometry::distance((*cloudIn)[it], searchPoint);
-      ASSERT_EQ ( (pointDist <= searchRadius) , true);
+      const auto pointDist = point_distance((*cloudIn)[it], searchPoint);
+      ASSERT_LE (pointDist, searchRadius);
     }
 
     ASSERT_EQ (cloudNWRRadius.size() , cloudSearchBruteforce.size ());
@@ -336,7 +351,7 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search)
     // check if result limitation works
     organizedNeighborSearch.radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius, 5);
 
-    ASSERT_EQ (cloudNWRRadius.size () <= 5, true);
+    ASSERT_LE (cloudNWRRadius.size (), 5);
   }
 }
 
@@ -344,7 +359,8 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search)
 TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search_Benchmark_Test)
 {
   constexpr unsigned int test_runs = 10;
-  srand (time (NULL));
+  const unsigned int seed = time (nullptr);
+  srand (seed);
 
   search::OrganizedNeighbor<PointXYZ> organizedNeighborSearch;
 
@@ -396,7 +412,7 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search_
 
     for (auto it = cloudIn->points.cbegin(); it != cloudIn->points.cend(); ++it)
     {
-      const auto pointDist = geometry::distance(*it, searchPoint);
+      const auto pointDist = point_distance(*it, searchPoint);
 
       if (pointDist <= searchRadius)
       {
@@ -405,7 +421,7 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search_
       }
     }
 
-    std::vector<int> cloudNWRSearch;
+    pcl::Indices cloudNWRSearch;
     std::vector<float> cloudNWRRadius;
 
     double check_time = getTime();
@@ -436,7 +452,9 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search_
     return;
   }
   constexpr unsigned int test_runs = 10;
-  srand (time (NULL));
+  const unsigned int seed = time (nullptr);
+  srand (seed);
+  SCOPED_TRACE("seed=" + std::to_string(seed));
 
   search::OrganizedNeighbor<PointXYZ> organizedNeighborSearch;
 
@@ -462,10 +480,10 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search_
     double searchRadius = 1.0 * (test_id*1.0/10*1.0);
     double sum_time = 0, sum_time2 = 0;
 
-    std::vector<int> cloudNWRSearch;
+    pcl::Indices cloudNWRSearch;
     std::vector<float> cloudNWRRadius;
 
-    std::vector<int> cloudNWRSearch2;
+    pcl::Indices cloudNWRSearch2;
     std::vector<float> cloudNWRRadius2;
 
     for (int iter = 0; iter < 100; iter++)
@@ -510,7 +528,7 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search_
 
     for (auto it = cloudIn->points.cbegin(); it != cloudIn->points.cend(); ++it)
     {
-      const auto pointDist = geometry::distance(*it, searchPoint);
+      const auto pointDist = point_distance(*it, searchPoint);
 
       if (pointDist <= searchRadius)
       {
@@ -522,16 +540,16 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search_
     // check if result from organized radius search can be also found in bruteforce search
     for (const auto it : cloudNWRSearch)
     {
-      double pointDist = geometry::distance((*cloudIn)[it], searchPoint);
-      ASSERT_EQ ( (pointDist <= searchRadius) , true);
+      const auto pointDist = point_distance((*cloudIn)[it], searchPoint);
+      ASSERT_LE (pointDist, searchRadius);
     }
 
 
     // check if bruteforce result from organized radius search can be also found in bruteforce search
     for (const auto it : cloudSearchBruteforce)
     {
-      double pointDist = geometry::distance((*cloudIn)[it], searchPoint);
-      ASSERT_EQ ( (pointDist <= searchRadius) , true);
+      const auto pointDist = point_distance((*cloudIn)[it], searchPoint);
+      ASSERT_LE (pointDist, searchRadius);
     }
 
     ASSERT_EQ (cloudNWRRadius.size() , cloudSearchBruteforce.size ());
@@ -539,7 +557,7 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search_
     // check if result limitation works
     organizedNeighborSearch.radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius, 5);
 
-    ASSERT_EQ (cloudNWRRadius.size () <= 5, true);
+    ASSERT_LE (cloudNWRRadius.size (), 5);
   }
 }
 
diff --git a/test/search/test_search.cpp b/test/search/test_search.cpp
index 70af4e48..59cdd319 100644
--- a/test/search/test_search.cpp
+++ b/test/search/test_search.cpp
@@ -103,10 +103,10 @@ std::uniform_int_distribution<unsigned> rand_uint(0, 10);
 std::uniform_real_distribution<float> rand_float (0.0f, 1.0f);
 
 /** \brief used by the *_VIEW_* tests to use only a subset of points from the point cloud*/
-std::vector<int> unorganized_input_indices;
+pcl::Indices unorganized_input_indices;
 
 /** \brief used by the *_VIEW_* tests to use only a subset of points from the point cloud*/
-std::vector<int> organized_input_indices;
+pcl::Indices organized_input_indices;
 
 /** \brief instance of brute force search method to be tested*/
 pcl::search::BruteForce<pcl::PointXYZ> brute_force;
@@ -127,16 +127,16 @@ std::vector<search::Search<PointXYZ>* > unorganized_search_methods;
 std::vector<search::Search<PointXYZ>* > organized_search_methods;
 
 /** \brief lists of indices to be used as query points for various search methods and different cloud types*/
-std::vector<int> unorganized_dense_cloud_query_indices;
-std::vector<int> unorganized_sparse_cloud_query_indices;
-std::vector<int> organized_sparse_query_indices;
+pcl::Indices unorganized_dense_cloud_query_indices;
+pcl::Indices unorganized_sparse_cloud_query_indices;
+pcl::Indices organized_sparse_query_indices;
 
 /** \briet test whether the result of a search contains unique point ids or not
   * @param indices resulting indices from a search
   * @param name name of the search method that returned these distances
   * @return true if indices are unique, false otherwise
   */
-bool testUniqueness (const std::vector<int>& indices, const std::string& name)
+bool testUniqueness (const pcl::Indices& indices, const std::string& name)
 {
   bool uniqueness = true;
   for (unsigned idx1 = 1; idx1 < indices.size () && uniqueness; ++idx1)
@@ -191,10 +191,10 @@ bool testOrder (const std::vector<float>& distances, const std::string& name)
  * @return true if result is valid, false otherwise
  */
 template<typename PointT> bool
-testResultValidity (const typename PointCloud<PointT>::ConstPtr point_cloud, const std::vector<bool>& indices_mask, const std::vector<bool>& nan_mask, const std::vector<int>& indices, const std::vector<int>& /*input_indices*/, const std::string& name)
+testResultValidity (const typename PointCloud<PointT>::ConstPtr point_cloud, const std::vector<bool>& indices_mask, const std::vector<bool>& nan_mask, const pcl::Indices& indices, const pcl::Indices& /*input_indices*/, const std::string& name)
 {
   bool validness = true;
-  for (const int &index : indices)
+  for (const auto &index : indices)
   {
     if (!indices_mask [index])
     {
@@ -233,8 +233,8 @@ testResultValidity (const typename PointCloud<PointT>::ConstPtr point_cloud, con
   * \param eps threshold for comparing the distances
   * \return true if both sets are the same, false otherwise
   */
-bool compareResults (const std::vector<int>& indices1, const std::vector<float>& distances1, const std::string& name1,
-                     const std::vector<int>& indices2, const std::vector<float>& distances2, const std::string& name2, float eps)
+bool compareResults (const pcl::Indices& indices1, const std::vector<float>& distances1, const std::string& name1,
+                     const pcl::Indices& indices2, const std::vector<float>& distances2, const std::string& name2, float eps)
 {
   bool equal = true;
   if (indices1.size () != indices2.size ())
@@ -282,9 +282,9 @@ bool compareResults (const std::vector<int>& indices1, const std::vector<float>&
   */
 template<typename PointT> void
 testKNNSearch (typename PointCloud<PointT>::ConstPtr point_cloud, std::vector<search::Search<PointT>*> search_methods,
-                const std::vector<int>& query_indices, const std::vector<int>& input_indices = std::vector<int> () )
+                const pcl::Indices& query_indices, const pcl::Indices& input_indices = pcl::Indices () )
 {
-  std::vector< std::vector<int> >indices (search_methods.size ());
+  std::vector< pcl::Indices >indices (search_methods.size ());
   std::vector< std::vector<float> >distances (search_methods.size ());
   std::vector<bool> passed (search_methods.size (), true);
   
@@ -294,7 +294,7 @@ testKNNSearch (typename PointCloud<PointT>::ConstPtr point_cloud, std::vector<se
   if (!input_indices.empty ())
   {
     indices_mask.assign (point_cloud->size (), false);
-    for (const int &input_index : input_indices)
+    for (const auto &input_index : input_indices)
       indices_mask [input_index] = true;
   }
   
@@ -322,7 +322,7 @@ testKNNSearch (typename PointCloud<PointT>::ConstPtr point_cloud, std::vector<se
   for (unsigned knn = 1; knn <= 512; knn <<= 3)
   {
     // find nn for each point in the cloud
-    for (const int &query_index : query_indices)
+    for (const auto &query_index : query_indices)
     {
       #pragma omp parallel for \
         shared(indices, input_indices, indices_mask, distances, knn, nan_mask, passed, point_cloud, query_index, search_methods) \
@@ -361,9 +361,9 @@ testKNNSearch (typename PointCloud<PointT>::ConstPtr point_cloud, std::vector<se
   */
 template<typename PointT> void
 testRadiusSearch (typename PointCloud<PointT>::ConstPtr point_cloud, std::vector<search::Search<PointT>*> search_methods, 
-                   const std::vector<int>& query_indices, const std::vector<int>& input_indices = std::vector<int> ())
+                   const pcl::Indices& query_indices, const pcl::Indices& input_indices = pcl::Indices   ())
 {
-  std::vector< std::vector<int> >indices (search_methods.size ());
+  std::vector< pcl::Indices >indices (search_methods.size ());
   std::vector< std::vector<float> >distances (search_methods.size ());
   std::vector<bool> passed (search_methods.size (), true);
   std::vector<bool> indices_mask (point_cloud->size (), true);
@@ -372,7 +372,7 @@ testRadiusSearch (typename PointCloud<PointT>::ConstPtr point_cloud, std::vector
   if (!input_indices.empty ())
   {
     indices_mask.assign (point_cloud->size (), false);
-    for (const int &input_index : input_indices)
+    for (const auto &input_index : input_indices)
       indices_mask [input_index] = true;
   }
   
@@ -401,7 +401,7 @@ testRadiusSearch (typename PointCloud<PointT>::ConstPtr point_cloud, std::vector
   {
     //std::cout << radius << std::endl;
     // find nn for each point in the cloud
-    for (const int &query_index : query_indices)
+    for (const auto &query_index : query_indices)
     {
       #pragma omp parallel for \
         default(none) \
@@ -475,7 +475,7 @@ TEST (PCL, unorganized_dense_cloud_Complete_Radius)
 // Test search on unorganized point clouds in a grid
 TEST (PCL, unorganized_grid_cloud_Complete_Radius)
 {
-  std::vector<int> query_indices;
+  pcl::Indices query_indices;
   query_indices.reserve (query_count);
   
   unsigned skip = static_cast<unsigned> (unorganized_grid_cloud->size ()) / query_count;
@@ -543,7 +543,7 @@ TEST (PCL, Organized_Sparse_View_Radius)
   * \param cloud input cloud required to check for nans and to get number of points
   * \param[in] query_count maximum number of query points
   */
-void createQueryIndices (std::vector<int>& query_indices, PointCloud<PointXYZ>::ConstPtr point_cloud, unsigned query_count)
+void createQueryIndices (pcl::Indices& query_indices, PointCloud<PointXYZ>::ConstPtr point_cloud, unsigned query_count)
 {
   query_indices.clear ();
   query_indices.reserve (query_count);
@@ -558,7 +558,7 @@ void createQueryIndices (std::vector<int>& query_indices, PointCloud<PointXYZ>::
   * \param indices 
   * \param max_index highest accented index usually given by cloud->size () - 1
   */
-void createIndices (std::vector<int>& indices, unsigned max_index)
+void createIndices (pcl::Indices& indices, unsigned max_index)
 {
   // ~10% of the input cloud
   for (unsigned idx = 0; idx <= max_index; ++idx)
diff --git a/test/surface/test_concave_hull.cpp b/test/surface/test_concave_hull.cpp
index 43bc1fe4..3f22c606 100644
--- a/test/surface/test_concave_hull.cpp
+++ b/test/surface/test_concave_hull.cpp
@@ -206,7 +206,7 @@ TEST (PCL, ConcaveHull_LTable)
 {
   //construct dataset
   pcl::PointCloud<pcl::PointXYZ> cloud_out_ltable;
-  cloud_out_ltable.points.resize (100);
+  cloud_out_ltable.resize (100);
 
   int npoints = 0;
   for (std::size_t i = 0; i < 8; i++)
@@ -231,7 +231,7 @@ TEST (PCL, ConcaveHull_LTable)
     }
   }
 
-  cloud_out_ltable.points.resize (npoints);
+  cloud_out_ltable.resize (npoints);
 
   pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloudptr (new pcl::PointCloud<pcl::PointXYZ> (cloud_out_ltable));
 
diff --git a/test/surface/test_convex_hull.cpp b/test/surface/test_convex_hull.cpp
index 592c7f3b..7dc05a02 100644
--- a/test/surface/test_convex_hull.cpp
+++ b/test/surface/test_convex_hull.cpp
@@ -188,7 +188,7 @@ TEST (PCL, ConvexHull_LTable)
 {
   //construct dataset
   pcl::PointCloud<pcl::PointXYZ> cloud_out_ltable;
-  cloud_out_ltable.points.resize (100);
+  cloud_out_ltable.resize (100);
 
   int npoints = 0;
   for (std::size_t i = 0; i < 8; i++)
@@ -239,7 +239,7 @@ TEST (PCL, ConvexHull_LTable)
   cloud_out_ltable[npoints].z = 0.f;
   npoints++;
 
-  cloud_out_ltable.points.resize (npoints);
+  cloud_out_ltable.resize (npoints);
 
   pcl::PointCloud<pcl::PointXYZ> hull;
   std::vector<pcl::Vertices> polygons;
diff --git a/test/visualization/test_visualization.cpp b/test/visualization/test_visualization.cpp
index 7762a6eb..a81abf23 100644
--- a/test/visualization/test_visualization.cpp
+++ b/test/visualization/test_visualization.cpp
@@ -60,10 +60,29 @@ PointCloud<PointNormal>::Ptr cloud_with_normals1 (new PointCloud<PointNormal>);
 search::KdTree<PointXYZ>::Ptr tree3;
 search::KdTree<PointNormal>::Ptr tree4;
 
+// Test that updatepointcloud works when adding points.
+////////////////////////////////////////////////////////////////////////////////
+TEST(PCL, PCLVisualizer_updatePointCloudAddPoint)
+{
+  pcl::common::CloudGenerator<pcl::PointXYZ, pcl::common::UniformGenerator<float> > generator;
+
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
+  generator.fill(3, 1, *cloud);
+
+  // Setup a basic viewport window
+  pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
+  viewer->addPointCloud<pcl::PointXYZ>(cloud);
+
+  cloud->push_back(pcl::PointXYZ());
+
+  viewer->updatePointCloud(cloud);
+  viewer->spinOnce(100);
+}
+
 // 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)
+TEST(PCL, PCLVisualizer_updatePointCloudRemovePoint)
 {
   pcl::common::CloudGenerator<pcl::PointXYZRGB, pcl::common::UniformGenerator<float> > generator;
 
diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt
index 1911f7b9..5bfe6e94 100644
--- a/tools/CMakeLists.txt
+++ b/tools/CMakeLists.txt
@@ -200,12 +200,20 @@ else()
 
   PCL_ADD_EXECUTABLE(pcl_obj2pcd COMPONENT ${SUBSYS_NAME} SOURCES obj2pcd.cpp)
   target_link_libraries(pcl_obj2pcd pcl_common pcl_io)
+  #TODO: Update when CMAKE 3.10 is available
+  if(NOT (${VTK_VERSION} VERSION_LESS 9.0))
+    target_link_libraries(pcl_obj2pcd VTK::FiltersCore)
+  endif()
 
   PCL_ADD_EXECUTABLE(pcl_obj2ply COMPONENT ${SUBSYS_NAME} SOURCES obj2ply.cpp)
   target_link_libraries(pcl_obj2ply pcl_common pcl_io)
 
   PCL_ADD_EXECUTABLE(pcl_vtk2pcd COMPONENT ${SUBSYS_NAME} SOURCES vtk2pcd.cpp)
   target_link_libraries(pcl_vtk2pcd pcl_common pcl_io)
+  #TODO: Update when CMAKE 3.10 is available
+  if(NOT (${VTK_VERSION} VERSION_LESS 9.0))
+    target_link_libraries(pcl_vtk2pcd VTK::FiltersCore)
+  endif()
 
   if(BUILD_visualization)
     PCL_ADD_EXECUTABLE(pcl_obj_rec_ransac_model_opps COMPONENT ${SUBSYS_NAME} SOURCES obj_rec_ransac_model_opps.cpp)
diff --git a/tools/boost.h b/tools/boost.h
index ed20728a..41a95217 100644
--- a/tools/boost.h
+++ b/tools/boost.h
@@ -44,9 +44,6 @@
 #pragma GCC system_header 
 #endif
 
-#ifndef Q_MOC_RUN
-// Marking all Boost headers as system headers to remove warnings
 #include <boost/date_time/gregorian/gregorian_types.hpp>
 #include <boost/date_time/posix_time/posix_time.hpp>
 #include <boost/date_time/posix_time/posix_time_types.hpp>
-#endif
diff --git a/tools/compute_cloud_error.cpp b/tools/compute_cloud_error.cpp
index 20f55a65..6b8b25a5 100644
--- a/tools/compute_cloud_error.cpp
+++ b/tools/compute_cloud_error.cpp
@@ -120,7 +120,7 @@ compute (const pcl::PCLPointCloud2::ConstPtr &cloud_source, const pcl::PCLPointC
       if (!std::isfinite ((*xyz_source)[point_i].x) || !std::isfinite ((*xyz_source)[point_i].y) || !std::isfinite ((*xyz_source)[point_i].z))
         continue;
 
-      std::vector<int> nn_indices (1);
+      pcl::Indices nn_indices (1);
       std::vector<float> nn_distances (1);
       if (!tree->nearestKSearch ((*xyz_source)[point_i], 1, nn_indices, nn_distances))
         continue;
@@ -152,7 +152,7 @@ compute (const pcl::PCLPointCloud2::ConstPtr &cloud_source, const pcl::PCLPointC
       if (!std::isfinite ((*xyz_source)[point_i].x) || !std::isfinite ((*xyz_source)[point_i].y) || !std::isfinite ((*xyz_source)[point_i].z))
         continue;
 
-      std::vector<int> nn_indices (1);
+      pcl::Indices nn_indices (1);
       std::vector<float> nn_distances (1);
       if (!tree->nearestKSearch ((*xyz_source)[point_i], 1, nn_indices, nn_distances))
         continue;
diff --git a/tools/compute_hausdorff.cpp b/tools/compute_hausdorff.cpp
index a3a4c585..75103725 100644
--- a/tools/compute_hausdorff.cpp
+++ b/tools/compute_hausdorff.cpp
@@ -89,7 +89,7 @@ compute (Cloud &cloud_a, Cloud &cloud_b)
   float max_dist_a = -std::numeric_limits<float>::max ();
   for (const auto &point : cloud_a.points)
   {
-    std::vector<int> indices (1);
+    pcl::Indices indices (1);
     std::vector<float> sqr_distances (1);
 
     tree_b.nearestKSearch (point, 1, indices, sqr_distances);
@@ -103,7 +103,7 @@ compute (Cloud &cloud_a, Cloud &cloud_b)
   float max_dist_b = -std::numeric_limits<float>::max ();
   for (const auto &point : cloud_b.points)
   {
-    std::vector<int> indices (1);
+    pcl::Indices indices (1);
     std::vector<float> sqr_distances (1);
 
     tree_a.nearestKSearch (point, 1, indices, sqr_distances);
diff --git a/tools/concatenate_points_pcd.cpp b/tools/concatenate_points_pcd.cpp
index 9a9364ab..f96125f9 100644
--- a/tools/concatenate_points_pcd.cpp
+++ b/tools/concatenate_points_pcd.cpp
@@ -47,7 +47,6 @@
 #include <iostream>
 #include <pcl/console/time.h>
 #include <pcl/io/pcd_io.h>
-#include <pcl/point_types.h>
 
 Eigen::Vector4f    translation;
 Eigen::Quaternionf orientation;
diff --git a/tools/crf_segmentation.cpp b/tools/crf_segmentation.cpp
index 8d8d756f..a54351e8 100644
--- a/tools/crf_segmentation.cpp
+++ b/tools/crf_segmentation.cpp
@@ -195,7 +195,7 @@ main (int argc, char** argv)
 
 
   // TODO:: make this as an optional argument ??
-  std::vector<int> tmp_indices;
+  pcl::Indices tmp_indices;
   pcl::removeNaNFromPointCloud (*cloud, *cloud, tmp_indices);
   
   // parse optional input arguments from the command line
diff --git a/tools/elch.cpp b/tools/elch.cpp
index aa6b55f5..1d170ec9 100644
--- a/tools/elch.cpp
+++ b/tools/elch.cpp
@@ -40,7 +40,6 @@
 #include <pcl/console/parse.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/point_types.h>
-#include <pcl/common/transforms.h>
 #include <pcl/registration/icp.h>
 #include <pcl/registration/elch.h>
 
diff --git a/tools/fast_bilateral_filter.cpp b/tools/fast_bilateral_filter.cpp
index 33a250c8..4bf9835c 100644
--- a/tools/fast_bilateral_filter.cpp
+++ b/tools/fast_bilateral_filter.cpp
@@ -41,6 +41,8 @@
 #include <pcl/console/print.h>
 #include <pcl/console/parse.h>
 #include <pcl/console/time.h>
+#include <boost/filesystem.hpp> // for path, exists, ...
+#include <boost/algorithm/string/case_conv.hpp> // for to_upper_copy
 
 using namespace pcl;
 using namespace pcl::io;
@@ -128,15 +130,11 @@ batchProcess (const std::vector<std::string> &pcd_files, std::string &output_dir
     compute (cloud, output, sigma_s, sigma_r);
 
     // Prepare output file name
-    std::string filename = pcd_files[i];
-    boost::trim (filename);
-    std::vector<std::string> st;
-    boost::split (st, filename, boost::is_any_of ("/\\"), boost::token_compress_on);
+    std::string filename = boost::filesystem::path(pcd_files[i]).filename().string();
     
     // Save into the second file
-    std::stringstream ss;
-    ss << output_dir << "/" << st.at (st.size () - 1);
-    saveCloud (ss.str (), output, translation, rotation);
+    const std::string filepath = output_dir + '/' + filename;
+    saveCloud (filepath, output, translation, rotation);
   }
   return (0);
 }
diff --git a/tools/grid_min.cpp b/tools/grid_min.cpp
index dacf46ff..3ca8d011 100644
--- a/tools/grid_min.cpp
+++ b/tools/grid_min.cpp
@@ -44,6 +44,8 @@
 #include <pcl/console/parse.h>
 #include <pcl/console/time.h>
 #include <pcl/filters/grid_minimum.h>
+#include <boost/filesystem.hpp> // for path, exists, ...
+#include <boost/algorithm/string/case_conv.hpp> // for to_upper_copy
 
 using namespace pcl;
 using namespace pcl::io;
@@ -128,14 +130,11 @@ batchProcess (const std::vector<std::string> &pcd_files, std::string &output_dir
     compute (cloud, output, resolution);
 
     // Prepare output file name
-    std::string filename = pcd_file;
-    boost::trim (filename);
-    boost::split (st, filename, boost::is_any_of ("/\\"), boost::token_compress_on);
+    std::string filename = boost::filesystem::path(pcd_file).filename().string();
     
     // Save into the second file
-    std::stringstream ss;
-    ss << output_dir << "/" << st.at (st.size () - 1);
-    saveCloud (ss.str (), output);
+    const std::string filepath = output_dir + '/' + filename;
+    saveCloud (filepath, output);
   }
   return (0);
 }
diff --git a/tools/hdl_viewer_simple.cpp b/tools/hdl_viewer_simple.cpp
index 48c3fa55..8ae39086 100644
--- a/tools/hdl_viewer_simple.cpp
+++ b/tools/hdl_viewer_simple.cpp
@@ -43,7 +43,6 @@
 #include <pcl/visualization/image_viewer.h>
 #include <pcl/io/openni_camera/openni_driver.h>
 #include <pcl/console/parse.h>
-#include <pcl/visualization/boost.h>
 #include <pcl/visualization/mouse_event.h>
 
 #include <boost/algorithm/string.hpp>
diff --git a/tools/image_grabber_saver.cpp b/tools/image_grabber_saver.cpp
index 5e5e9df0..764e672d 100644
--- a/tools/image_grabber_saver.cpp
+++ b/tools/image_grabber_saver.cpp
@@ -39,10 +39,8 @@
 #include <pcl/io/image_grabber.h>
 #include <pcl/console/parse.h>
 #include <pcl/console/print.h>
-#include <pcl/visualization/boost.h>
-#include <pcl/visualization/cloud_viewer.h>
-#include <pcl/visualization/image_viewer.h>
 #include <pcl/io/pcd_io.h>
+#include <boost/filesystem.hpp> // for exists
 
 using pcl::console::print_error;
 using pcl::console::print_info;
@@ -70,9 +68,8 @@ struct EventHelper
   void 
   cloud_cb (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr & cloud)
   {
-    std::stringstream ss;
-    ss << out_folder << "/" << grabber->getPrevDepthFileName() << ".pcd";
-    pcl::io::savePCDFileASCII (ss.str(), *cloud);
+    const std::string filepath = out_folder + '/' + grabber->getPrevDepthFileName() + ".pcd";
+    pcl::io::savePCDFileASCII (filepath, *cloud);
   }
 };
 
diff --git a/tools/image_grabber_viewer.cpp b/tools/image_grabber_viewer.cpp
index 4ce088b8..e7117baf 100644
--- a/tools/image_grabber_viewer.cpp
+++ b/tools/image_grabber_viewer.cpp
@@ -40,13 +40,12 @@
 #include <pcl/io/image_grabber.h>
 #include <pcl/console/parse.h>
 #include <pcl/console/print.h>
-#include <pcl/visualization/boost.h>
 #include <pcl/visualization/cloud_viewer.h>
 #include <pcl/visualization/image_viewer.h>
-#include <pcl/io/pcd_io.h>
 
 #include <mutex>
 #include <thread>
+#include <boost/filesystem.hpp> // for exists
 
 using namespace std::chrono_literals;
 using pcl::console::print_error;
diff --git a/tools/image_viewer.cpp b/tools/image_viewer.cpp
index 409b91b6..94e048f8 100644
--- a/tools/image_viewer.cpp
+++ b/tools/image_viewer.cpp
@@ -38,12 +38,9 @@
 
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
-#include <pcl/common/time.h> //fps calculations
 #include <pcl/io/pcd_io.h>
 #include <pcl/visualization/cloud_viewer.h>
 #include <pcl/visualization/image_viewer.h>
-#include <pcl/io/openni_camera/openni_driver.h>
-#include <pcl/console/parse.h>
 #include <vector>
 
 int
diff --git a/tools/local_max.cpp b/tools/local_max.cpp
index c3a80dc5..b7f1392e 100644
--- a/tools/local_max.cpp
+++ b/tools/local_max.cpp
@@ -44,6 +44,8 @@
 #include <pcl/console/parse.h>
 #include <pcl/console/time.h>
 #include <pcl/filters/local_maximum.h>
+#include <boost/filesystem.hpp> // for path, exists, ...
+#include <boost/algorithm/string/case_conv.hpp> // for to_upper_copy
 
 using namespace pcl;
 using namespace pcl::io;
@@ -129,14 +131,11 @@ batchProcess (const std::vector<std::string> &pcd_files, std::string &output_dir
     compute (cloud, output, radius);
 
     // Prepare output file name
-    std::string filename = pcd_file;
-    boost::trim (filename);
-    boost::split (st, filename, boost::is_any_of ("/\\"), boost::token_compress_on);
+    std::string filename = boost::filesystem::path(pcd_file).filename().string();
     
     // Save into the second file
-    std::stringstream ss;
-    ss << output_dir << "/" << st.at (st.size () - 1);
-    saveCloud (ss.str (), output);
+    const std::string filepath = output_dir + '/' + filename;
+    saveCloud (filepath, output);
   }
   return (0);
 }
diff --git a/tools/lum.cpp b/tools/lum.cpp
index a8763f7e..e4f80f22 100644
--- a/tools/lum.cpp
+++ b/tools/lum.cpp
@@ -40,7 +40,6 @@
 #include <pcl/console/parse.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/point_types.h>
-#include <pcl/common/transforms.h>
 #include <pcl/registration/lum.h>
 #include <pcl/registration/correspondence_estimation.h>
 
diff --git a/tools/match_linemod_template.cpp b/tools/match_linemod_template.cpp
index aa2323c0..b543e5c8 100644
--- a/tools/match_linemod_template.cpp
+++ b/tools/match_linemod_template.cpp
@@ -39,7 +39,6 @@
 #include <pcl/point_types.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/console/print.h>
-#include <pcl/console/parse.h>
 #include <pcl/console/time.h>
 
 #include <pcl/recognition/linemod.h>
diff --git a/tools/mesh_sampling.cpp b/tools/mesh_sampling.cpp
index 681cde1d..5e627220 100644
--- a/tools/mesh_sampling.cpp
+++ b/tools/mesh_sampling.cpp
@@ -36,9 +36,9 @@
  */
 
 #include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/visualization/vtk/pcl_vtk_compatibility.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/io/vtk_lib_io.h>
-#include <pcl/common/transforms.h>
 #include <vtkVersion.h>
 #include <vtkPLYReader.h>
 #include <vtkOBJReader.h>
@@ -87,7 +87,8 @@ randPSurface (vtkPolyData * polydata, std::vector<double> * cumulativeAreas, dou
 
   double A[3], B[3], C[3];
   vtkIdType npts = 0;
-  vtkIdType *ptIds = nullptr;
+  vtkCellPtsPtr ptIds = nullptr;
+
   polydata->GetCellPoints (el, npts, ptIds);
   polydata->GetPoint (ptIds[0], A);
   polydata->GetPoint (ptIds[1], B);
@@ -124,7 +125,7 @@ randPSurface (vtkPolyData * polydata, std::vector<double> * cumulativeAreas, dou
     {
       static bool printed_once = false;
       if (!printed_once)
-        PCL_WARN ("Mesh has no vertex colors, or vertex colors are not RGB!");
+        PCL_WARN ("Mesh has no vertex colors, or vertex colors are not RGB!\n");
       printed_once = true;
     }
   }
@@ -138,7 +139,8 @@ uniform_sampling (vtkSmartPointer<vtkPolyData> polydata, std::size_t n_samples,
 
   double p1[3], p2[3], p3[3], totalArea = 0;
   std::vector<double> cumulativeAreas (cells->GetNumberOfCells (), 0);
-  vtkIdType npts = 0, *ptIds = nullptr;
+  vtkIdType npts = 0;
+  vtkCellPtsPtr ptIds = nullptr;
   std::size_t cellId = 0;
   for (cells->InitTraversal (); cells->GetNextCell (npts, ptIds); cellId++)
   {
@@ -149,7 +151,7 @@ uniform_sampling (vtkSmartPointer<vtkPolyData> polydata, std::size_t n_samples,
     cumulativeAreas[cellId] = totalArea;
   }
 
-  cloud_out.points.resize (n_samples);
+  cloud_out.resize (n_samples);
   cloud_out.width = static_cast<std::uint32_t> (n_samples);
   cloud_out.height = 1;
 
diff --git a/tools/mls_smoothing.cpp b/tools/mls_smoothing.cpp
index a8b1accb..4caf1476 100644
--- a/tools/mls_smoothing.cpp
+++ b/tools/mls_smoothing.cpp
@@ -42,7 +42,6 @@
 #include <pcl/console/parse.h>
 #include <pcl/console/time.h>
 #include <pcl/surface/mls.h>
-#include <pcl/filters/voxel_grid.h>
 #include <pcl/search/kdtree.h> // for KdTree
 
 using namespace pcl;
diff --git a/tools/morph.cpp b/tools/morph.cpp
index df3df667..1fb9bd15 100644
--- a/tools/morph.cpp
+++ b/tools/morph.cpp
@@ -44,6 +44,8 @@
 #include <pcl/console/parse.h>
 #include <pcl/console/time.h>
 #include <pcl/filters/morphological_filter.h>
+#include <boost/filesystem.hpp> // for path, exists, ...
+#include <boost/algorithm/string/case_conv.hpp> // for to_upper_copy
 
 using namespace pcl;
 using namespace pcl::io;
@@ -150,14 +152,11 @@ batchProcess (const std::vector<std::string> &pcd_files, std::string &output_dir
     compute (cloud, output, resolution, method);
 
     // Prepare output file name
-    std::string filename = pcd_file;
-    boost::trim (filename);
-    boost::split (st, filename, boost::is_any_of ("/\\"), boost::token_compress_on);
-
+    std::string filename = boost::filesystem::path(pcd_file).filename().string();
+    
     // Save into the second file
-    std::stringstream ss;
-    ss << output_dir << "/" << st.at (st.size () - 1);
-    saveCloud (ss.str (), output);
+    const std::string filepath = output_dir + '/' + filename;
+    saveCloud (filepath, output);
   }
   return (0);
 }
diff --git a/tools/ndt2d.cpp b/tools/ndt2d.cpp
index 0a5ab1ad..a378c865 100644
--- a/tools/ndt2d.cpp
+++ b/tools/ndt2d.cpp
@@ -38,8 +38,6 @@
 #include <pcl/io/pcd_io.h>
 #include <pcl/point_types.h>
 #include <pcl/registration/ndt_2d.h>
-#include <pcl/registration/transformation_estimation_lm.h>
-#include <pcl/registration/warp_point_rigid_3d.h>
 
 #include <string>
 #include <iostream>
diff --git a/tools/normal_estimation.cpp b/tools/normal_estimation.cpp
index 53561409..34401a8b 100644
--- a/tools/normal_estimation.cpp
+++ b/tools/normal_estimation.cpp
@@ -45,6 +45,8 @@
 #include <pcl/console/print.h>
 #include <pcl/console/parse.h>
 #include <pcl/console/time.h>
+#include <boost/filesystem.hpp> // for path, exists, ...
+#include <boost/algorithm/string/case_conv.hpp> // for to_upper_copy
 
 using namespace pcl;
 using namespace pcl::io;
@@ -147,15 +149,11 @@ batchProcess (const std::vector<std::string> &pcd_files, std::string &output_dir
     compute (cloud, output, k, radius);
 
     // Prepare output file name
-    std::string filename = pcd_files[i];
-    boost::trim (filename);
-    std::vector<std::string> st;
-    boost::split (st, filename, boost::is_any_of ("/\\"), boost::token_compress_on);
+    std::string filename = boost::filesystem::path(pcd_files[i]).filename().string();
     
     // Save into the second file
-    std::stringstream ss;
-    ss << output_dir << "/" << st.at (st.size () - 1);
-    saveCloud (ss.str (), output, translation, rotation);
+    const std::string filepath = output_dir + '/' + filename;
+    saveCloud (filepath, output, translation, rotation);
   }
   return (0);
 }
diff --git a/tools/obj_rec_ransac_accepted_hypotheses.cpp b/tools/obj_rec_ransac_accepted_hypotheses.cpp
index 00acc96d..050467a8 100644
--- a/tools/obj_rec_ransac_accepted_hypotheses.cpp
+++ b/tools/obj_rec_ransac_accepted_hypotheses.cpp
@@ -46,8 +46,6 @@
 #include <pcl/recognition/ransac_based/obj_rec_ransac.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/console/print.h>
-#include <pcl/console/parse.h>
-#include <pcl/io/pcd_io.h>
 #include <pcl/point_cloud.h>
 #include <vtkVersion.h>
 #include <vtkPolyDataReader.h>
diff --git a/tools/obj_rec_ransac_hash_table.cpp b/tools/obj_rec_ransac_hash_table.cpp
index e79789d7..bde3d941 100644
--- a/tools/obj_rec_ransac_hash_table.cpp
+++ b/tools/obj_rec_ransac_hash_table.cpp
@@ -47,8 +47,6 @@
 #include <pcl/recognition/ransac_based/obj_rec_ransac.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/console/print.h>
-#include <pcl/console/parse.h>
-#include <pcl/io/pcd_io.h>
 #include <pcl/point_cloud.h>
 #include <vtkPolyDataReader.h>
 #include <vtkDoubleArray.h>
diff --git a/tools/obj_rec_ransac_model_opps.cpp b/tools/obj_rec_ransac_model_opps.cpp
index 32062881..c2ac91bb 100644
--- a/tools/obj_rec_ransac_model_opps.cpp
+++ b/tools/obj_rec_ransac_model_opps.cpp
@@ -45,8 +45,6 @@
 #include <pcl/recognition/ransac_based/obj_rec_ransac.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/console/print.h>
-#include <pcl/console/parse.h>
-#include <pcl/io/pcd_io.h>
 #include <pcl/point_cloud.h>
 #include <vtkVersion.h>
 #include <vtkPolyDataReader.h>
diff --git a/tools/obj_rec_ransac_result.cpp b/tools/obj_rec_ransac_result.cpp
index c95e9536..327f903b 100644
--- a/tools/obj_rec_ransac_result.cpp
+++ b/tools/obj_rec_ransac_result.cpp
@@ -46,7 +46,6 @@
 #include <pcl/recognition/ransac_based/obj_rec_ransac.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/console/print.h>
-#include <pcl/io/pcd_io.h>
 #include <pcl/point_cloud.h>
 #include <vtkVersion.h>
 #include <vtkPolyDataReader.h>
@@ -348,10 +347,10 @@ loadScene (const char* file_name, PointCloud<PointXYZ>& non_plane_points, PointC
 
   // Make sure that the ids are sorted
   sort (inliers->indices.begin (), inliers->indices.end ());
-  std::size_t j = 0;
-  for ( std::size_t i = 0, id = 0 ; i < inliers->indices.size () ; )
+  pcl::uindex_t j = 0, i = 0;
+  for ( pcl::index_t id = 0 ; i < inliers->indices.size () ; ++id)
   {
-    if ( static_cast<int> (id) == inliers->indices[i] )
+    if ( id == inliers->indices[i] )
     {
       plane_points[i] = (*all_points)[id];
       ++i;
@@ -362,7 +361,6 @@ loadScene (const char* file_name, PointCloud<PointXYZ>& non_plane_points, PointC
       non_plane_normals[j] = (*all_normals)[id];
       ++j;
     }
-    ++id;
   }
 
   // Just copy the rest of the non-plane points
diff --git a/tools/obj_rec_ransac_scene_opps.cpp b/tools/obj_rec_ransac_scene_opps.cpp
index ce73f578..f3df089c 100644
--- a/tools/obj_rec_ransac_scene_opps.cpp
+++ b/tools/obj_rec_ransac_scene_opps.cpp
@@ -46,7 +46,6 @@
 #include <pcl/recognition/ransac_based/obj_rec_ransac.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/console/print.h>
-#include <pcl/io/pcd_io.h>
 #include <pcl/point_cloud.h>
 #include <vtkVersion.h>
 #include <vtkPolyDataReader.h>
diff --git a/tools/octree_viewer.cpp b/tools/octree_viewer.cpp
index 3fed3787..ac028aa5 100644
--- a/tools/octree_viewer.cpp
+++ b/tools/octree_viewer.cpp
@@ -40,7 +40,6 @@
 #include <pcl/io/auto_io.h>
 #include <pcl/common/time.h>
 #include <pcl/visualization/pcl_visualizer.h>
-#include <pcl/visualization/point_cloud_handlers.h>
 #include <pcl/visualization/common/common.h>
 
 #include <pcl/octree/octree_pointcloud_voxelcentroid.h>
@@ -212,7 +211,7 @@ private:
     }
 
     //remove NaN Points
-    std::vector<int> nanIndexes;
+    pcl::Indices nanIndexes;
     pcl::removeNaNFromPointCloud(*cloud, *cloud, nanIndexes);
     std::cout << "Loaded " << cloud->size() << " points" << std::endl;
 
@@ -244,7 +243,7 @@ private:
     viz.addText (dataDisplay, 0, 45, 1.0, 0.0, 0.0, "disp_original_points");
 
     char level[256];
-    sprintf (level, "Displayed depth is %d on %d", displayedDepth, octree.getTreeDepth());
+    sprintf (level, "Displayed depth is %d on %zu", displayedDepth, static_cast<std::size_t>(octree.getTreeDepth()));
     viz.removeShape ("level_t1");
     viz.addText (level, 0, 30, 1.0, 0.0, 0.0, "level_t1");
 
diff --git a/tools/oni_viewer_simple.cpp b/tools/oni_viewer_simple.cpp
index 54f35df7..fcf331d6 100644
--- a/tools/oni_viewer_simple.cpp
+++ b/tools/oni_viewer_simple.cpp
@@ -42,12 +42,10 @@
 #include <pcl/common/time.h> //fps calculations
 #include <pcl/console/parse.h>
 #include <pcl/io/oni_grabber.h>
-#include <pcl/visualization/boost.h>
 #include <pcl/visualization/cloud_viewer.h>
 #include <pcl/common/time_trigger.h>
 
 #include <mutex>
-#include <vector>
 
 #define SHOW_FPS 1
 #if SHOW_FPS
diff --git a/tools/openni2_viewer.cpp b/tools/openni2_viewer.cpp
index 3b5b465d..a6ecabd0 100644
--- a/tools/openni2_viewer.cpp
+++ b/tools/openni2_viewer.cpp
@@ -37,12 +37,10 @@
 
 #define MEASURE_FUNCTION_TIME
 #include <pcl/common/time.h> //fps calculations
-#include <pcl/common/angles.h>
 #include <pcl/io/openni2_grabber.h>
 #include <pcl/io/openni2/openni.h>
 #include <pcl/io/openni2/openni2_device_manager.h>
 #include <pcl/visualization/pcl_visualizer.h>
-#include <pcl/visualization/boost.h>
 #include <pcl/visualization/image_viewer.h>
 #include <pcl/console/print.h>
 #include <pcl/console/parse.h>
diff --git a/tools/openni_image.cpp b/tools/openni_image.cpp
index b54f589c..356858f5 100644
--- a/tools/openni_image.cpp
+++ b/tools/openni_image.cpp
@@ -39,7 +39,6 @@
 #include <pcl/common/time.h> //fps calculations
 #include <pcl/io/openni_grabber.h>
 #include <pcl/io/lzf_image_io.h>
-#include <pcl/visualization/boost.h>
 #include <pcl/visualization/common/float_image_utils.h>
 #include <pcl/visualization/image_viewer.h>
 #include <pcl/io/openni_camera/openni_driver.h>
@@ -47,6 +46,7 @@
 #include <pcl/visualization/mouse_event.h>
 
 #include <boost/circular_buffer.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp> // for ptime, to_iso_string, ...
 
 #include <csignal>
 #include <limits>
@@ -267,45 +267,43 @@ class Writer
       FPS_CALC_WRITER ("data write   ", buf_);
       nr_frames_total++;
       
-      std::stringstream ss1, ss2, ss3;
-
       std::string time_string = boost::posix_time::to_iso_string (frame->time);
       // Save RGB data
-      ss1 << "frame_" << time_string << "_rgb.pclzf";
+      const std::string rgb_filename = "frame_" + time_string + "_rgb.pclzf";
       switch (frame->image->getEncoding ())
       {
         case openni_wrapper::Image::YUV422:
         {
           io::LZFYUV422ImageWriter lrgb;
-          lrgb.write (reinterpret_cast<const char*> (&frame->image->getMetaData ().Data ()[0]), frame->image->getWidth (), frame->image->getHeight (), ss1.str ());
+          lrgb.write (reinterpret_cast<const char*> (&frame->image->getMetaData ().Data ()[0]), frame->image->getWidth (), frame->image->getHeight (), rgb_filename);
           break;
         }
         case openni_wrapper::Image::RGB:
         {
           io::LZFRGB24ImageWriter lrgb;
-          lrgb.write (reinterpret_cast<const char*> (&frame->image->getMetaData ().Data ()[0]), frame->image->getWidth (), frame->image->getHeight (), ss1.str ());
+          lrgb.write (reinterpret_cast<const char*> (&frame->image->getMetaData ().Data ()[0]), frame->image->getWidth (), frame->image->getHeight (), rgb_filename);
           break;
         }
         case openni_wrapper::Image::BAYER_GRBG:
         {
           io::LZFBayer8ImageWriter lrgb;
-          lrgb.write (reinterpret_cast<const char*> (&frame->image->getMetaData ().Data ()[0]), frame->image->getWidth (), frame->image->getHeight (), ss1.str ());
+          lrgb.write (reinterpret_cast<const char*> (&frame->image->getMetaData ().Data ()[0]), frame->image->getWidth (), frame->image->getHeight (), rgb_filename);
           break;
         }
       }
 
       // Save depth data
-      ss2 << "frame_" + time_string + "_depth.pclzf";
+      const std::string depth_filename = "frame_" + time_string + "_depth.pclzf";
       io::LZFDepth16ImageWriter ld;
       //io::LZFShift11ImageWriter ld;
-      ld.write (reinterpret_cast<const char*> (&frame->depth_image->getDepthMetaData ().Data ()[0]), frame->depth_image->getWidth (), frame->depth_image->getHeight (), ss2.str ());
+      ld.write (reinterpret_cast<const char*> (&frame->depth_image->getDepthMetaData ().Data ()[0]), frame->depth_image->getWidth (), frame->depth_image->getHeight (), depth_filename);
       
       // Save depth data
-      ss3 << "frame_" << time_string << ".xml";
+      const std::string xml_filename = "frame_" + time_string + ".xml";
          
       io::LZFRGB24ImageWriter lrgb;
-      lrgb.writeParameters (frame->parameters_rgb, ss3.str ());
-      ld.writeParameters (frame->parameters_depth, ss3.str ());
+      lrgb.writeParameters (frame->parameters_rgb, xml_filename);
+      ld.writeParameters (frame->parameters_depth, xml_filename);
       // By default, the z-value depth multiplication factor is written as part of the LZFDepthImageWriter's writeParameters as 0.001
       // If you want to change that, uncomment the next line and change the value
       //ld.writeParameter (0.001, "depth.z_multiplication_factor", ss3.str ());
diff --git a/tools/openni_save_image.cpp b/tools/openni_save_image.cpp
index 1f2f3b5f..e7cad451 100644
--- a/tools/openni_save_image.cpp
+++ b/tools/openni_save_image.cpp
@@ -39,14 +39,17 @@
 #include <pcl/io/openni_grabber.h>
 #include <pcl/io/openni_camera/openni_driver.h>
 #include <pcl/console/parse.h>
-#include <pcl/visualization/vtk.h>
 #include <pcl/visualization/pcl_visualizer.h>
 
-#include "boost.h"
+#include <vtkSmartPointer.h>
+#include <vtkImageImport.h>
+#include <vtkTIFFWriter.h>
+#include <vtkImageFlip.h>
 
 #include <mutex>
 #include <string>
 #include <vector>
+#include <boost/date_time/posix_time/posix_time.hpp> // for to_iso_string, local_time
 
 
 #define SHOW_FPS 1
@@ -148,13 +151,12 @@ class SimpleOpenNIViewer
             data = reinterpret_cast<const void*> (rgb_data);
           }
 
-          std::stringstream ss;
-          ss << "frame_" + time + "_rgb.tiff";
+          const std::string filename = "frame_" + time + "_rgb.tiff";
           importer_->SetImportVoidPointer (const_cast<void*>(data), 1);
           importer_->Update ();
           flipper_->SetInputConnection (importer_->GetOutputPort ());
           flipper_->Update ();
-          writer_->SetFileName (ss.str ().c_str ());
+          writer_->SetFileName (filename.c_str ());
           writer_->SetInputConnection (flipper_->GetOutputPort ());
           writer_->Write ();
         }
@@ -164,8 +166,7 @@ class SimpleOpenNIViewer
           openni_wrapper::DepthImage::Ptr depth_image;
           depth_image.swap (depth_image_);
 
-          std::stringstream ss;
-          ss << "frame_" + time + "_depth.tiff";
+          const std::string filename = "frame_" + time + "_depth.tiff";
 
           depth_importer_->SetWholeExtent (0, depth_image->getWidth () - 1, 0, depth_image->getHeight () - 1, 0, 0);
           depth_importer_->SetDataExtentToWholeExtent ();
@@ -173,7 +174,7 @@ class SimpleOpenNIViewer
           depth_importer_->Update ();
           flipper_->SetInputConnection (depth_importer_->GetOutputPort ());
           flipper_->Update ();
-          writer_->SetFileName (ss.str ().c_str ());
+          writer_->SetFileName (filename.c_str ());
           writer_->SetInputConnection (flipper_->GetOutputPort ());
           writer_->Write ();
         }
diff --git a/tools/openni_viewer.cpp b/tools/openni_viewer.cpp
index 6853be29..f35543f6 100644
--- a/tools/openni_viewer.cpp
+++ b/tools/openni_viewer.cpp
@@ -39,7 +39,6 @@
 #include <pcl/common/time.h> //fps calculations
 #include <pcl/io/openni_grabber.h>
 #include <pcl/visualization/pcl_visualizer.h>
-#include <pcl/visualization/boost.h>
 #include <pcl/visualization/image_viewer.h>
 #include <pcl/console/print.h>
 #include <pcl/console/parse.h>
diff --git a/tools/openni_viewer_simple.cpp b/tools/openni_viewer_simple.cpp
index 7236603f..0599ebdb 100644
--- a/tools/openni_viewer_simple.cpp
+++ b/tools/openni_viewer_simple.cpp
@@ -46,7 +46,6 @@
 #include <pcl/io/openni_camera/openni_driver.h>
 #include <pcl/console/print.h>
 #include <pcl/console/parse.h>
-#include <pcl/visualization/boost.h>
 #include <pcl/visualization/mouse_event.h>
 
 #include <vtkImageViewer.h>
diff --git a/tools/outlier_removal.cpp b/tools/outlier_removal.cpp
index 9f93ba05..c7d4406e 100644
--- a/tools/outlier_removal.cpp
+++ b/tools/outlier_removal.cpp
@@ -107,7 +107,7 @@ compute (const pcl::PCLPointCloud2::ConstPtr &input, pcl::PCLPointCloud2 &output
 
   pcl::PointIndices::Ptr removed_indices (new PointIndices),
                          indices (new PointIndices);
-  std::vector<int> valid_indices;
+  pcl::Indices valid_indices;
   if (keep_organized)
   {
     xyz_cloud = xyz_cloud_pre;
@@ -163,8 +163,8 @@ compute (const pcl::PCLPointCloud2::ConstPtr &input, pcl::PCLPointCloud2 &output
   else 
   {
     // Make sure we are addressing values in the original index vector
-    for (std::size_t i = 0; i < removed_indices->indices.size (); ++i)
-      indices->indices.push_back (valid_indices[removed_indices->indices[i]]);
+    for (const auto& i : (removed_indices->indices))
+      indices->indices.push_back (valid_indices[i]);
 
     // Extract the indices of the remaining points
     pcl::ExtractIndices<pcl::PCLPointCloud2> ei;
diff --git a/tools/passthrough_filter.cpp b/tools/passthrough_filter.cpp
index ac941c53..87c976f4 100644
--- a/tools/passthrough_filter.cpp
+++ b/tools/passthrough_filter.cpp
@@ -37,12 +37,13 @@
  */
 
 #include <pcl/PCLPointCloud2.h>
-#include <pcl/point_types.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/console/print.h>
 #include <pcl/console/parse.h>
 #include <pcl/console/time.h>
 #include <pcl/filters/passthrough.h>
+#include <boost/filesystem.hpp> // for path, exists, ...
+#include <boost/algorithm/string/case_conv.hpp> // for to_upper_copy
 
 using namespace pcl;
 using namespace pcl::io;
@@ -138,14 +139,11 @@ batchProcess (const std::vector<std::string> &pcd_files, std::string &output_dir
     compute (cloud, output, field_name, min, max, inside, keep_organized);
 
     // Prepare output file name
-    std::string filename = pcd_file;
-    boost::trim (filename);
-    boost::split (st, filename, boost::is_any_of ("/\\"), boost::token_compress_on);
+    std::string filename = boost::filesystem::path(pcd_file).filename().string();
     
     // Save into the second file
-    std::stringstream ss;
-    ss << output_dir << "/" << st.at (st.size () - 1);
-    saveCloud (ss.str (), output);
+    const std::string filepath = output_dir + '/' + filename;
+    saveCloud (filepath, output);
   }
   return (0);
 }
diff --git a/tools/pcd2png.cpp b/tools/pcd2png.cpp
index 3e8d9f8a..4254098a 100644
--- a/tools/pcd2png.cpp
+++ b/tools/pcd2png.cpp
@@ -49,6 +49,7 @@
 #include <pcl/io/pcd_io.h>
 #include <pcl/io/png_io.h>
 #include <pcl/conversions.h>
+#include <boost/lexical_cast.hpp> // for lexical_cast
 
 using namespace pcl;
 using namespace pcl::io;
diff --git a/tools/pcd_grabber_viewer.cpp b/tools/pcd_grabber_viewer.cpp
index d75525d0..3dd923b9 100644
--- a/tools/pcd_grabber_viewer.cpp
+++ b/tools/pcd_grabber_viewer.cpp
@@ -39,10 +39,10 @@
 #include <pcl/io/pcd_grabber.h>
 #include <pcl/console/parse.h>
 #include <pcl/console/print.h>
-#include <pcl/visualization/boost.h>
 #include <pcl/visualization/cloud_viewer.h>
 #include <pcl/visualization/image_viewer.h>
-
+#include <boost/filesystem.hpp> // for exists, extension, ...
+#include <boost/algorithm/string/case_conv.hpp> // for to_upper_copy
 #include <mutex>
 #include <thread>
 
diff --git a/tools/pcd_viewer.cpp b/tools/pcd_viewer.cpp
index 9b97dfa8..e8cdf124 100644
--- a/tools/pcd_viewer.cpp
+++ b/tools/pcd_viewer.cpp
@@ -44,9 +44,6 @@
 #include <pcl/common/common.h>
 #include <pcl/io/pcd_io.h>
 #include <cfloat>
-#include <pcl/visualization/eigen.h>
-//#include <pcl/visualization/vtk.h>
-#include <pcl/visualization/point_cloud_handlers.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/visualization/image_viewer.h>
 #include <pcl/visualization/histogram_visualizer.h>
@@ -170,7 +167,7 @@ pp_callback (const pcl::visualization::PointPickingEvent& event, void* cookie)
     search.setInputCloud (xyzcloud);
   }
   // Return the correct index in the cloud instead of the index on the screen
-  std::vector<int> indices (1);
+  pcl::Indices indices (1);
   std::vector<float> distances (1);
 
   // Because VTK/OpenGL stores data without NaN, we lose the 1-1 correspondence, so we must search for the real point
@@ -192,22 +189,21 @@ pp_callback (const pcl::visualization::PointPickingEvent& event, void* cookie)
   }
 
   // Else, if a single point has been selected
-  std::stringstream ss;
-  ss << idx;
+  const std::string idx_string = std::to_string(idx);
   // Get the cloud's fields
   for (std::size_t i = 0; i < cloud->fields.size (); ++i)
   {
     if (!isMultiDimensionalFeatureField (cloud->fields[i]))
       continue;
     PCL_INFO ("Multidimensional field found: %s\n", cloud->fields[i].name.c_str ());
-    ph_global.addFeatureHistogram (*cloud, cloud->fields[i].name, idx, ss.str ());
+    ph_global.addFeatureHistogram (*cloud, cloud->fields[i].name, idx, idx_string);
     ph_global.renderOnce ();
   }
   if (p)
   {
     pcl::PointXYZ pos;
     event.getPoint (pos.x, pos.y, pos.z);
-    p->addText3D<pcl::PointXYZ> (ss.str (), pos, 0.0005, 1.0, 1.0, 1.0, ss.str ());
+    p->addText3D<pcl::PointXYZ> (idx_string, pos, 0.0005, 1.0, 1.0, 1.0, idx_string);
   }
   
 }
@@ -378,21 +374,20 @@ main (int argc, char** argv)
     }
 
     // Add as actor
-    std::stringstream cloud_name ("vtk-");
-    cloud_name << argv[vtk_file_indices.at (i)] << "-" << i;
-    p->addModelFromPolyData (polydata, cloud_name.str (), viewport);
+    const std::string cloud_name = "vtk-" + std::string(argv[vtk_file_indices.at (i)]) + "-" + std::to_string(i);
+    p->addModelFromPolyData (polydata, cloud_name, viewport);
 
     // Change the shape rendered color
     if (fcolorparam && fcolor_r.size () > i && fcolor_g.size () > i && fcolor_b.size () > i)
-      p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, fcolor_r[i], fcolor_g[i], fcolor_b[i], cloud_name.str ());
+      p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, fcolor_r[i], fcolor_g[i], fcolor_b[i], cloud_name);
 
     // Change the shape rendered point size
     if (!psize.empty ())
-      p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize.at (i), cloud_name.str ());
+      p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize.at (i), cloud_name);
 
     // Change the shape rendered opacity
     if (!opaque.empty ())
-      p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opaque.at (i), cloud_name.str ());
+      p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opaque.at (i), cloud_name);
 
     // Change the shape rendered shading
     if (!shadings.empty ())
@@ -400,17 +395,17 @@ main (int argc, char** argv)
       if (shadings[i] == "flat")
       {
         print_highlight (stderr, "Setting shading property for %s to FLAT.\n", argv[vtk_file_indices.at (i)]);
-        p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_SHADING, pcl::visualization::PCL_VISUALIZER_SHADING_FLAT, cloud_name.str ());
+        p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_SHADING, pcl::visualization::PCL_VISUALIZER_SHADING_FLAT, cloud_name);
       }
       else if (shadings[i] == "gouraud")
       {
         print_highlight (stderr, "Setting shading property for %s to GOURAUD.\n", argv[vtk_file_indices.at (i)]);
-        p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_SHADING, pcl::visualization::PCL_VISUALIZER_SHADING_GOURAUD, cloud_name.str ());
+        p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_SHADING, pcl::visualization::PCL_VISUALIZER_SHADING_GOURAUD, cloud_name);
       }
       else if (shadings[i] == "phong")
       {
         print_highlight (stderr, "Setting shading property for %s to PHONG.\n", argv[vtk_file_indices.at (i)]);
-        p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_SHADING, pcl::visualization::PCL_VISUALIZER_SHADING_PHONG, cloud_name.str ());
+        p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_SHADING, pcl::visualization::PCL_VISUALIZER_SHADING_PHONG, cloud_name);
       }
     }
   }
@@ -443,18 +438,18 @@ main (int argc, char** argv)
       origin.block<3, 1> (0, 0) = (pose * Eigen::Translation3f (origin.block<3, 1> (0, 0))).translation ();
     }
 
-    std::stringstream cloud_name;
+    std::string cloud_name;
 
     // ---[ Special check for 1-point multi-dimension histograms
     if (cloud->fields.size () == 1 && isMultiDimensionalFeatureField (cloud->fields[0]))
     {
-      cloud_name << argv[p_file_indices.at (i)];
+      cloud_name = argv[p_file_indices.at (i)];
 
       if (!ph)
         ph.reset (new pcl::visualization::PCLPlotter);
 
       pcl::getMinMax (*cloud, 0, cloud->fields[0].name, min_p, max_p);
-      ph->addFeatureHistogram (*cloud, cloud->fields[0].name, cloud_name.str ());
+      ph->addFeatureHistogram (*cloud, cloud->fields[0].name, cloud_name);
       print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud->fields[0].count); print_info (" points]\n");
       continue;
     }
@@ -465,9 +460,8 @@ main (int argc, char** argv)
       print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%u", cloud->width * cloud->height); print_info (" points]\n");
       print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (*cloud).c_str ());
       
-      std::stringstream name;
-      name << "PCD Viewer :: " << argv[p_file_indices.at (i)];
-      pcl::visualization::ImageViewer::Ptr img (new pcl::visualization::ImageViewer (name.str ()));
+      std::string name = "PCD Viewer :: " + std::string(argv[p_file_indices.at (i)]);
+      pcl::visualization::ImageViewer::Ptr img (new pcl::visualization::ImageViewer (name));
       pcl::PointCloud<pcl::RGB> rgb_cloud;
       pcl::fromPCLPointCloud2 (*cloud, rgb_cloud);
 
@@ -477,7 +471,7 @@ main (int argc, char** argv)
       continue;
     }
 
-    cloud_name << argv[p_file_indices.at (i)] << "-" << i;
+    cloud_name += std::string(argv[p_file_indices.at (i)]) + "-" + std::to_string(i);
 
     // Create the PCLVisualizer object here on the first encountered XYZ file
     if (!p)
@@ -531,8 +525,8 @@ main (int argc, char** argv)
     // Add the dataset with a XYZ and a random handler
     geometry_handler.reset (new pcl::visualization::PointCloudGeometryHandlerXYZ<pcl::PCLPointCloud2> (cloud));
     // Add the cloud to the renderer
-    //p->addPointCloud<pcl::PointXYZ> (cloud_xyz, geometry_handler, color_handler, cloud_name.str (), viewport);
-    p->addPointCloud (cloud, geometry_handler, color_handler, origin, orientation, cloud_name.str (), viewport);
+    //p->addPointCloud<pcl::PointXYZ> (cloud_xyz, geometry_handler, color_handler, cloud_name, viewport);
+    p->addPointCloud (cloud, geometry_handler, color_handler, origin, orientation, cloud_name, viewport);
 
 
     if (mview)
@@ -558,9 +552,8 @@ main (int argc, char** argv)
 
       pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
       pcl::fromPCLPointCloud2 (*cloud, *cloud_normals);
-      std::stringstream cloud_name_normals;
-      cloud_name_normals << argv[p_file_indices.at (i)] << "-" << i << "-normals";
-      p->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud_xyz, cloud_normals, normals, normals_scale, cloud_name_normals.str (), viewport);
+      const std::string cloud_name_normals = std::string(argv[p_file_indices.at (i)]) + "-" + std::to_string(i) + "-normals";
+      p->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud_xyz, cloud_normals, normals, normals_scale, cloud_name_normals, viewport);
     }
 
     // If principal curvature lines are enabled
@@ -593,15 +586,14 @@ main (int argc, char** argv)
       pcl::fromPCLPointCloud2 (*cloud, *cloud_normals);
       pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr cloud_pc (new pcl::PointCloud<pcl::PrincipalCurvatures>);
       pcl::fromPCLPointCloud2 (*cloud, *cloud_pc);
-      std::stringstream cloud_name_normals_pc;
-      cloud_name_normals_pc << argv[p_file_indices.at (i)] << "-" << i << "-normals";
+      std::string cloud_name_normals_pc = std::string(argv[p_file_indices.at (i)]) + "-" + std::to_string(i) + "-normals";
       int factor = (std::min)(normals, pc);
-      p->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud_xyz, cloud_normals, factor, normals_scale, cloud_name_normals_pc.str (), viewport);
-      p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, cloud_name_normals_pc.str ());
-      p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, cloud_name_normals_pc.str ());
-      cloud_name_normals_pc << "-pc";
-      p->addPointCloudPrincipalCurvatures<pcl::PointXYZ, pcl::Normal> (cloud_xyz, cloud_normals, cloud_pc, factor, pc_scale, cloud_name_normals_pc.str (), viewport);
-      p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, cloud_name_normals_pc.str ());
+      p->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud_xyz, cloud_normals, factor, normals_scale, cloud_name_normals_pc, viewport);
+      p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, cloud_name_normals_pc);
+      p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, cloud_name_normals_pc);
+      cloud_name_normals_pc += "-pc";
+      p->addPointCloudPrincipalCurvatures<pcl::PointXYZ, pcl::Normal> (cloud_xyz, cloud_normals, cloud_pc, factor, pc_scale, cloud_name_normals_pc, viewport);
+      p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, cloud_name_normals_pc);
     }
 
     // Add every dimension as a possible color
@@ -632,35 +624,35 @@ main (int argc, char** argv)
           color_handler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (cloud, cloud->fields[f].name));
         }
         // Add the cloud to the renderer
-        //p->addPointCloud<pcl::PointXYZ> (cloud_xyz, color_handler, cloud_name.str (), viewport);
-        p->addPointCloud (cloud, color_handler, origin, orientation, cloud_name.str (), viewport);
+        //p->addPointCloud<pcl::PointXYZ> (cloud_xyz, color_handler, cloud_name, viewport);
+        p->addPointCloud (cloud, color_handler, origin, orientation, cloud_name, viewport);
       }
       // Set RGB color handler or label handler as default
-      p->updateColorHandlerIndex (cloud_name.str (), (rgb_idx ? rgb_idx : label_idx));
+      p->updateColorHandlerIndex (cloud_name, (rgb_idx ? rgb_idx : label_idx));
     }
 
     // Additionally, add normals as a handler
     geometry_handler.reset (new pcl::visualization::PointCloudGeometryHandlerSurfaceNormal<pcl::PCLPointCloud2> (cloud));
     if (geometry_handler->isCapable ())
-      //p->addPointCloud<pcl::PointXYZ> (cloud_xyz, geometry_handler, cloud_name.str (), viewport);
-      p->addPointCloud (cloud, geometry_handler, origin, orientation, cloud_name.str (), viewport);
+      //p->addPointCloud<pcl::PointXYZ> (cloud_xyz, geometry_handler, cloud_name, viewport);
+      p->addPointCloud (cloud, geometry_handler, origin, orientation, cloud_name, viewport);
 
     if (use_immediate_rendering)
       // Set immediate mode rendering ON
-      p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_IMMEDIATE_RENDERING, 1.0, cloud_name.str ());
+      p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_IMMEDIATE_RENDERING, 1.0, cloud_name);
 
     // Change the cloud rendered point size
     if (!psize.empty ())
-      p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize.at (i), cloud_name.str ());
+      p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize.at (i), cloud_name);
 
     // Change the cloud rendered opacity
     if (!opaque.empty ())
-      p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opaque.at (i), cloud_name.str ());
+      p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opaque.at (i), cloud_name);
 
     // Reset camera viewpoint to center of cloud if camera parameters were not passed manually and this is the first loaded cloud
     if (i == 0 && !p->cameraParamsSet () && !p->cameraFileLoaded ())
     {
-      p->resetCameraViewpoint (cloud_name.str ());
+      p->resetCameraViewpoint (cloud_name);
       p->resetCamera ();
     }
 
@@ -674,9 +666,9 @@ main (int argc, char** argv)
   {
     std::string str;
     if (!p_file_indices.empty ())
-      str = std::string (argv[p_file_indices.at (0)]);
+      str = argv[p_file_indices.at (0)];
     else if (!vtk_file_indices.empty ())
-      str = std::string (argv[vtk_file_indices.at (0)]);
+      str = argv[vtk_file_indices.at (0)];
 
     for (std::size_t i = 1; i < p_file_indices.size (); ++i)
       str += ", " + std::string (argv[p_file_indices.at (i)]);
diff --git a/tools/pcl_video.cpp b/tools/pcl_video.cpp
index 7c84aed0..b9dbf493 100644
--- a/tools/pcl_video.cpp
+++ b/tools/pcl_video.cpp
@@ -52,7 +52,9 @@
 #include <pcl/visualization/cloud_viewer.h>
 #include <pcl/console/parse.h>
 #include <pcl/common/time.h>
-#include "boost.h"
+#include <boost/date_time/gregorian/gregorian_types.hpp> // for date
+#include <boost/date_time/posix_time/posix_time.hpp> // for local_time
+#include <boost/date_time/posix_time/posix_time_types.hpp> // for time_duration
 
 using namespace std::chrono_literals;
 namespace bpt = boost::posix_time;
diff --git a/tools/png2pcd.cpp b/tools/png2pcd.cpp
index 20ef3233..e0c4e476 100644
--- a/tools/png2pcd.cpp
+++ b/tools/png2pcd.cpp
@@ -51,7 +51,9 @@
 #include <pcl/console/time.h>
 #include <pcl/console/print.h>
 #include <pcl/console/parse.h>
-#include <pcl/io/vtk_lib_io.h>
+#include <vtkImageData.h> // for vtkImageData
+#include <vtkSmartPointer.h> // for vtkSmartPointer
+#include <vtkPNGReader.h> // for vtkPNGReader
 
 #define RED_MULTIPLIER 0.299
 #define GREEN_MULTIPLIER 0.587
@@ -297,7 +299,7 @@ main (int argc, char** argv)
           mono_cloud.width = dimensions[0];
           mono_cloud.height = dimensions[1]; // This indicates that the point cloud is organized
           mono_cloud.is_dense = true;
-          mono_cloud.points.resize (mono_cloud.width * mono_cloud.height);
+          mono_cloud.resize (mono_cloud.width * mono_cloud.height);
 
           for (int y = 0; y < dimensions[1]; y++)
           {
@@ -321,7 +323,7 @@ main (int argc, char** argv)
         mono_cloud_u8.width = dimensions[0];
         mono_cloud_u8.height = dimensions[1]; // This indicates that the point cloud is organized
         mono_cloud_u8.is_dense = true;
-        mono_cloud_u8.points.resize (mono_cloud_u8.width * mono_cloud_u8.height);
+        mono_cloud_u8.resize (mono_cloud_u8.width * mono_cloud_u8.height);
 
         for (int y = 0; y < dimensions[1]; y++)
         {
@@ -377,7 +379,7 @@ main (int argc, char** argv)
         color_cloud.width = dimensions[0];
         color_cloud.height = dimensions[1]; // This indicates that the point cloud is organized
         color_cloud.is_dense = true;
-        color_cloud.points.resize (color_cloud.width * color_cloud.height);
+        color_cloud.resize (color_cloud.width * color_cloud.height);
 
         for (int y = 0; y < dimensions[1]; y++)
         {
@@ -447,7 +449,7 @@ main (int argc, char** argv)
         color_cloud.width = dimensions[0];
         color_cloud.height = dimensions[1]; // This indicates that the point cloud is organized
         color_cloud.is_dense = true;
-        color_cloud.points.resize (color_cloud.width * color_cloud.height);
+        color_cloud.resize (color_cloud.width * color_cloud.height);
 
         for (int y = 0; y < dimensions[1]; y++)
         {
@@ -499,7 +501,7 @@ main (int argc, char** argv)
       cloud.width = dimensions[0];
       cloud.height = dimensions[1]; // This indicates that the point cloud is organized
       cloud.is_dense = false;
-      cloud.points.resize (cloud.width * cloud.height);
+      cloud.resize (cloud.width * cloud.height);
 
       for (int y = 0; y < dimensions[1]; y++)
       {
@@ -552,7 +554,7 @@ main (int argc, char** argv)
       cloud.width = dimensions[0];
       cloud.height = dimensions[1]; // This indicates that the point cloud is organized
       cloud.is_dense = true;
-      cloud.points.resize (cloud.width * cloud.height);
+      cloud.resize (cloud.width * cloud.height);
 
       for (int y = 0; y < dimensions[1]; y++)
       {
@@ -635,7 +637,7 @@ main (int argc, char** argv)
       cloud.width = dimensions[0];
       cloud.height = dimensions[1]; // This indicates that the point cloud is organized
       cloud.is_dense = false;
-      cloud.points.resize (cloud.width * cloud.height);
+      cloud.resize (cloud.width * cloud.height);
 
       for (int y = 0; y < dimensions[1]; y++)
       {
@@ -691,7 +693,7 @@ main (int argc, char** argv)
           cloud.width = dimensions[0];
           cloud.height = dimensions[1]; // This indicates that the point cloud is organized
           cloud.is_dense = true;
-          cloud.points.resize (cloud.width * cloud.height);
+          cloud.resize (cloud.width * cloud.height);
 
           for (int y = 0; y < dimensions[1]; y++)
           {
@@ -730,7 +732,7 @@ main (int argc, char** argv)
           cloud8u.width = dimensions[0];
           cloud8u.height = dimensions[1]; // This indicates that the point cloud is organized
           cloud8u.is_dense = true;
-          cloud8u.points.resize (cloud8u.width * cloud8u.height);
+          cloud8u.resize (cloud8u.width * cloud8u.height);
 
           for (int y = 0; y < dimensions[1]; y++)
           {
diff --git a/tools/progressive_morphological_filter.cpp b/tools/progressive_morphological_filter.cpp
index 4391024d..b6ea2d00 100644
--- a/tools/progressive_morphological_filter.cpp
+++ b/tools/progressive_morphological_filter.cpp
@@ -46,6 +46,8 @@
 #include <pcl/filters/extract_indices.h>
 #include <pcl/segmentation/approximate_progressive_morphological_filter.h>
 #include <pcl/segmentation/progressive_morphological_filter.h>
+#include <boost/filesystem.hpp> // for path, exists, ...
+#include <boost/algorithm/string/case_conv.hpp> // for to_upper_copy
 
 using namespace pcl;
 using namespace pcl::io;
@@ -114,7 +116,7 @@ compute (ConstCloudPtr &input, Cloud &output, int max_window_size, float slope,
 
   print_highlight (stderr, "Computing ");
 
-  std::vector<int> ground;
+  pcl::Indices ground;
 
   if (approximate)
   {
@@ -187,14 +189,11 @@ batchProcess (const std::vector<std::string> &pcd_files, std::string &output_dir
     compute (cloud, output, max_window_size, slope, max_distance, initial_distance, cell_size, base, exponential, approximate);
 
     // Prepare output file name
-    std::string filename = pcd_file;
-    boost::trim (filename);
-    boost::split (st, filename, boost::is_any_of ("/\\"), boost::token_compress_on);
-
+    std::string filename = boost::filesystem::path(pcd_file).filename().string();
+    
     // Save into the second file
-    std::stringstream ss;
-    ss << output_dir << "/" << st.at (st.size () - 1);
-    saveCloud (ss.str (), output);
+    const std::string filepath = output_dir + '/' + filename;
+    saveCloud (filepath, output);
   }
   return (0);
 }
diff --git a/tools/radius_filter.cpp b/tools/radius_filter.cpp
index 9ce67e3b..60cb56fb 100644
--- a/tools/radius_filter.cpp
+++ b/tools/radius_filter.cpp
@@ -40,6 +40,8 @@
 #include <pcl/console/parse.h>
 #include <pcl/console/time.h>
 #include <pcl/filters/conditional_removal.h>
+#include <boost/filesystem.hpp> // for path, exists, ...
+#include <boost/algorithm/string/case_conv.hpp> // for to_upper_copy
 
 
 using PointType = pcl::PointXYZ;
@@ -130,14 +132,11 @@ batchProcess (const std::vector<std::string> &pcd_files, std::string &output_dir
     compute (cloud, output, radius, inside, keep_organized);
 
     // Prepare output file name
-    std::string filename = pcd_file;
-    boost::trim (filename);
-    boost::split (st, filename, boost::is_any_of ("/\\"), boost::token_compress_on);
-
+    std::string filename = boost::filesystem::path(pcd_file).filename().string();
+    
     // Save into the second file
-    std::stringstream ss;
-    ss << output_dir << "/" << st.at (st.size () - 1);
-    saveCloud (ss.str (), output);
+    const std::string filepath = output_dir + '/' + filename;
+    saveCloud (filepath, output);
   }
   return (0);
 }
diff --git a/tools/registration_visualizer.cpp b/tools/registration_visualizer.cpp
index afc72aac..322eb5a4 100644
--- a/tools/registration_visualizer.cpp
+++ b/tools/registration_visualizer.cpp
@@ -39,7 +39,6 @@
 #include <pcl/io/pcd_io.h>
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/registration/icp.h>
-#include <pcl/registration/icp_nl.h>
 #include <pcl/visualization/registration_visualizer.h>
 
 int
diff --git a/tools/sac_segmentation_plane.cpp b/tools/sac_segmentation_plane.cpp
index 9eb057ff..335eb910 100644
--- a/tools/sac_segmentation_plane.cpp
+++ b/tools/sac_segmentation_plane.cpp
@@ -43,6 +43,8 @@
 #include <pcl/console/print.h>
 #include <pcl/console/parse.h>
 #include <pcl/console/time.h>
+#include <boost/filesystem.hpp> // for path, exists, ...
+#include <boost/algorithm/string/case_conv.hpp> // for to_upper_copy
 
 using namespace pcl;
 using namespace pcl::io;
@@ -107,7 +109,7 @@ compute (const pcl::PCLPointCloud2::ConstPtr &input, pcl::PCLPointCloud2 &output
   sac.setMaxIterations (max_iterations);
   bool res = sac.computeModel ();
   
-  std::vector<int> inliers;
+  pcl::Indices inliers;
   sac.getInliers (inliers);
   Eigen::VectorXf coefficients;
   sac.getModelCoefficients (coefficients);
@@ -190,14 +192,11 @@ batchProcess (const std::vector<std::string> &pcd_files, std::string &output_dir
     compute (cloud, output, max_it, thresh, negative);
 
     // Prepare output file name
-    std::string filename = pcd_file;
-    boost::trim (filename);
-    boost::split (st, filename, boost::is_any_of ("/\\"), boost::token_compress_on);
+    std::string filename = boost::filesystem::path(pcd_file).filename().string();
     
     // Save into the second file
-    std::stringstream ss;
-    ss << output_dir << "/" << st.at (st.size () - 1);
-    saveCloud (ss.str (), output);
+    const std::string filepath = output_dir + '/' + filename;
+    saveCloud (filepath, output);
   }
   return (0);
 }
diff --git a/tools/tiff2pcd.cpp b/tools/tiff2pcd.cpp
index 50703b68..8602f859 100644
--- a/tools/tiff2pcd.cpp
+++ b/tools/tiff2pcd.cpp
@@ -42,14 +42,13 @@
 #include <iostream>
 #include <boost/filesystem.hpp>
 
-#include <pcl/pcl_base.h>
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
 #include <pcl/console/parse.h>
 #include <pcl/console/print.h>
 #include <pcl/io/pcd_io.h>
 
-#include <pcl/io/vtk_lib_io.h>
+#include <vtkImageData.h> // for vtkImageData
 #include <vtkSmartPointer.h>
 #include <vtkImageViewer2.h>
 #include <vtkTIFFReader.h>
@@ -128,8 +127,8 @@ void processAndSave( vtkSmartPointer<vtkImageData>  depth_data,
       color_point.b = xyzrgba_point.b = static_cast<std::uint8_t> (rgb_data->GetScalarComponentAsFloat(u,v,0,2));
       xyzrgba_point.a = 0;
 
-      pc_image.points.push_back(color_point);
-      pc_depth.points.push_back(depth_point);
+      pc_image.push_back(color_point);
+      pc_depth.push_back(depth_point);
 
       float d =  depth_data->GetScalarComponentAsFloat(u,v,0,0);
       depth_point.intensity = d;
@@ -144,38 +143,30 @@ void processAndSave( vtkSmartPointer<vtkImageData>  depth_data,
       {
         xyzrgba_point.z = xyzrgba_point.x = xyzrgba_point.y = bad_point;
       }
-      pc_xyzrgba.points.push_back(xyzrgba_point);
+      pc_xyzrgba.push_back(xyzrgba_point);
 
     } // for u
   } // for v
 
-  std::stringstream ss;
-
   if(depth)
   {
+    std::string depth_filename = "frame_" + time + "_depth.pcd";
     if(use_output_path)
-      ss << output_path << "/frame_" << time << "_depth.pcd";
-    else
-      ss << "frame_" << time << "_depth.pcd";
-    pcl::io::savePCDFile (ss.str(), pc_depth, format);
-    ss.str(""); //empty
+      depth_filename = output_path + '/' + depth_filename;
+    pcl::io::savePCDFile (depth_filename, pc_depth, format);
   }
 
   if(color)
   {
+    std::string color_filename = "frame_" + time + "_color.pcd";
     if(use_output_path)
-      ss << output_path << "/frame_" << time << "_color.pcd";
-    else
-      ss << "frame_" << time << "_color.pcd";
-    pcl::io::savePCDFile (ss.str(), pc_image, format);
-    ss.str(""); //empty
+      color_filename = output_path + '/' + color_filename;
+    pcl::io::savePCDFile (color_filename, pc_image, format);
   }
-
+  std::string xyzrgba_filename = "frame_" + time + "_xyzrgba.pcd";
   if(use_output_path)
-    ss << output_path << "/frame_" << time << "_xyzrgba.pcd";
-  else
-    ss << "frame_" << time << "_xyzrgba.pcd";
-  pcl::io::savePCDFile (ss.str(), pc_xyzrgba, format);
+    xyzrgba_filename = output_path + '/' + xyzrgba_filename;
+  pcl::io::savePCDFile (xyzrgba_filename, pc_xyzrgba, format);
 
   std::cout << "Saved " << time << " to pcd" << std::endl;
   return;
diff --git a/tools/timed_trigger_test.cpp b/tools/timed_trigger_test.cpp
index 4b1e8eeb..be9fc47c 100644
--- a/tools/timed_trigger_test.cpp
+++ b/tools/timed_trigger_test.cpp
@@ -2,7 +2,6 @@
 #include <thread>
 #include <pcl/common/time_trigger.h>
 #include <pcl/common/time.h>
-#include <pcl/visualization/boost.h>
 
 using namespace std::chrono_literals;
 using namespace pcl;
diff --git a/tools/train_linemod_template.cpp b/tools/train_linemod_template.cpp
index 0c3f476f..4c186acc 100644
--- a/tools/train_linemod_template.cpp
+++ b/tools/train_linemod_template.cpp
@@ -106,7 +106,7 @@ maskForegroundPoints (const PointCloudXYZRGBA::ConstPtr & input,
   std::vector<bool> foreground_mask (input->size (), false);
 
   // Mask off points outside the specified near and far depth thresholds
-  pcl::IndicesPtr indices (new std::vector<int>);
+  pcl::IndicesPtr indices (new pcl::Indices);
   for (std::size_t i = 0; i < input->size (); ++i)
   {
     const float z = (*input)[i].z;
@@ -133,7 +133,7 @@ maskForegroundPoints (const PointCloudXYZRGBA::ConstPtr & input,
   seg.segment (*inliers, *coefficients);
 
   // Mask off the plane inliers
-  for (const int &index : inliers->indices)
+  for (const auto &index : inliers->indices)
     foreground_mask[index] = false;
 
   // Mask off any foreground points that are too high above the detected plane
diff --git a/tools/transform_from_viewpoint.cpp b/tools/transform_from_viewpoint.cpp
index 26e72d6e..111756a8 100644
--- a/tools/transform_from_viewpoint.cpp
+++ b/tools/transform_from_viewpoint.cpp
@@ -39,7 +39,6 @@
 
 #include <pcl/PCLPointCloud2.h>
 #include <pcl/io/pcd_io.h>
-#include <pcl/features/fpfh.h>
 #include <pcl/console/print.h>
 #include <pcl/console/parse.h>
 #include <pcl/console/time.h>
diff --git a/tools/transform_point_cloud.cpp b/tools/transform_point_cloud.cpp
index c11b6e34..4d1b57cc 100644
--- a/tools/transform_point_cloud.cpp
+++ b/tools/transform_point_cloud.cpp
@@ -199,7 +199,7 @@ scaleInPlace (pcl::PCLPointCloud2 &cloud, double* multiplier)
   int z_idx = pcl::getFieldIndex (cloud, "z");
   Eigen::Array3i xyz_offset (cloud.fields[x_idx].offset, cloud.fields[y_idx].offset, cloud.fields[z_idx].offset);
  
-  for (std::uint32_t cp = 0; cp < cloud.width * cloud.height; ++cp)
+  for (uindex_t cp = 0; cp < cloud.width * cloud.height; ++cp)
   {
     // Assume all 3 fields are the same (XYZ)
     assert ((cloud.fields[x_idx].datatype == cloud.fields[y_idx].datatype));
diff --git a/tools/unary_classifier_segment.cpp b/tools/unary_classifier_segment.cpp
index 73f527a7..1a9c6e06 100644
--- a/tools/unary_classifier_segment.cpp
+++ b/tools/unary_classifier_segment.cpp
@@ -42,8 +42,10 @@
 #include <pcl/console/print.h>
 #include <pcl/console/parse.h>
 #include <pcl/console/time.h>
+#include <pcl/filters/filter.h> // for removeNaNFromPointCloud
 
 #include <pcl/segmentation/unary_classifier.h>
+#include <boost/filesystem.hpp> // for path, exists, ...
 
 using namespace pcl;
 using namespace pcl::io;
@@ -84,13 +86,12 @@ loadTrainedFeatures (std::vector<FeatureT::Ptr> &out,
     if (!boost::filesystem::is_directory (it->status ()) &&
         boost::filesystem::extension (it->path ()) == ".pcd")
     {   
-      std::stringstream ss;
-      ss << it->path ().string ();
+      const std::string path = it->path ().string ();
 
-      print_highlight ("Loading %s \n", ss.str ().c_str ());
+      print_highlight ("Loading %s \n", path.c_str ());
       
       FeatureT::Ptr features (new FeatureT);
-      if (loadPCDFile (ss.str (), *features) < 0)
+      if (loadPCDFile (path, *features) < 0)
         return false;
 
       out.push_back (features);
@@ -184,7 +185,7 @@ main (int argc, char** argv)
     return (-1);
 
   // TODO:: make this as an optional argument ??
-  std::vector<int> tmp_indices;
+  pcl::Indices tmp_indices;
   pcl::removeNaNFromPointCloud (*cloud, *cloud, tmp_indices);
   
   // parse optional input arguments from the command line
diff --git a/tools/uniform_sampling.cpp b/tools/uniform_sampling.cpp
index f46aadc6..2d92fd41 100644
--- a/tools/uniform_sampling.cpp
+++ b/tools/uniform_sampling.cpp
@@ -44,7 +44,6 @@
 #include <boost/filesystem.hpp>
 #include <algorithm>
 #include <string>
-#include <pcl/io/vtk_io.h>
 
 using namespace pcl;
 using namespace pcl::io;
@@ -108,7 +107,7 @@ compute (const pcl::PCLPointCloud2::ConstPtr &input, pcl::PCLPointCloud2 &output
   us.getRemovedIndices(removed_indices);
   std::sort(removed_indices.indices.begin(), removed_indices.indices.end());
   // Compute retained indices as a set difference between all and removed
-  std::vector<int> retained;
+  pcl::Indices retained;
   std::set_difference(input_indices->begin(),
                       input_indices->end(),
                       removed_indices.indices.begin(),
diff --git a/tools/vfh_estimation.cpp b/tools/vfh_estimation.cpp
index 9ef12a9e..9bf6273b 100644
--- a/tools/vfh_estimation.cpp
+++ b/tools/vfh_estimation.cpp
@@ -37,7 +37,6 @@
  *
  */
 
-#include <pcl/PCLPointCloud2.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/features/vfh.h>
 #include <pcl/console/print.h>
diff --git a/tools/virtual_scanner.cpp b/tools/virtual_scanner.cpp
index 6ebd563a..be4d9abe 100644
--- a/tools/virtual_scanner.cpp
+++ b/tools/virtual_scanner.cpp
@@ -54,7 +54,12 @@
 #include <pcl/memory.h>  // for pcl::make_shared
 #include <pcl/point_types.h>
 #include <pcl/console/parse.h>
-#include <pcl/visualization/vtk.h>
+
+#include <vtkGeneralTransform.h>
+#include <vtkPlatonicSolidSource.h>
+#include <vtkLoopSubdivisionFilter.h>
+#include <vtkCellLocator.h>
+#include <vtkMath.h>
 
 #include <boost/algorithm/string.hpp>  // for boost::is_any_of, boost::split, boost::token_compress_on, boost::trim
 #include <boost/filesystem.hpp>  // for boost::filesystem::create_directories, boost::filesystem::exists, boost::filesystem::extension, boost::filesystem::path
@@ -111,12 +116,13 @@ main (int argc, char** argv)
               "         -view_point <x,y,z>       : set the camera viewpoint from where the acquisition will take place\n"
               "         -target_point <x,y,z>     : the target point that the camera should look at (default: 0, 0, 0)\n"
               "         -organized <0|1>          : create an organized, grid-like point cloud of width x height (1), or keep it unorganized with height = 1 (0)\n"
+              "         -scale <double>           : scaling factor to the points XYZ (default 1(m), 1000(mm))\n"
               "         -noise <0|1>              : add gaussian noise (1) or keep the model noiseless (0)\n"
               "         -noise_std <x>            : use X times the standard deviation\n"
               "");
     return (-1);
   }
-  std::string filename;
+
   // Parse the command line arguments for .vtk or .ply files
   std::vector<int> p_file_indices_vtk = console::parse_file_extension_argument (argc, argv, ".vtk");
   std::vector<int> p_file_indices_ply = console::parse_file_extension_argument (argc, argv, ".ply");
@@ -130,6 +136,9 @@ main (int argc, char** argv)
   console::parse_3x_arguments (argc, argv, "-target_point", tx, ty, tz);
   int organized = 0;
   console::parse_argument (argc, argv, "-organized", organized);
+  double scale = 1;
+  console::parse_argument (argc, argv, "-scale", scale);
+
   if (organized)
     PCL_INFO ("Saving an organized dataset.\n");
   else
@@ -143,13 +152,11 @@ main (int argc, char** argv)
     return (-1);
   }
 
-  std::stringstream filename_stream;
+  std::string filename;
   if (!p_file_indices_ply.empty ())
-    filename_stream << argv[p_file_indices_ply.at (0)];
+    filename = argv[p_file_indices_ply.at (0)];
   else
-    filename_stream << argv[p_file_indices_vtk.at (0)];
-
-  filename = filename_stream.str ();
+    filename = argv[p_file_indices_vtk.at (0)];
 
   data = loadDataSet (filename.c_str ());
 
@@ -249,6 +256,9 @@ main (int argc, char** argv)
   int sid = -1;
   for (int i = 0; i < number_of_points; i++)
   {
+    // Clear cloud for next view scan
+    cloud.clear();
+
     sphere->GetPoint (i, eye);
     if (std::abs(eye[0]) < EPS) eye[0] = 0;
     if (std::abs(eye[1]) < EPS) eye[1] = 0;
@@ -351,9 +361,9 @@ main (int argc, char** argv)
           pcl::PointWithViewpoint pt;
           if (object_coordinates)
           {
-            pt.x = static_cast<float> (x[0]);
-            pt.y = static_cast<float> (x[1]);
-            pt.z = static_cast<float> (x[2]);
+            pt.x = static_cast<float> (x[0] * scale);
+            pt.y = static_cast<float> (x[1] * scale);
+            pt.z = static_cast<float> (x[2] * scale);
             pt.vp_x = static_cast<float> (eye[0]);
             pt.vp_y = static_cast<float> (eye[1]);
             pt.vp_z = static_cast<float> (eye[2]);
@@ -363,12 +373,12 @@ main (int argc, char** argv)
             // z axis is the viewray
             // y axis is up
             // x axis is -right (negative because z*y=-x but viewray*up=right)
-            pt.x = static_cast<float> (-right[0]*x[1] + up[0]*x[2] + viewray[0]*x[0] + eye[0]);
-            pt.y = static_cast<float> (-right[1]*x[1] + up[1]*x[2] + viewray[1]*x[0] + eye[1]);
-            pt.z = static_cast<float> (-right[2]*x[1] + up[2]*x[2] + viewray[2]*x[0] + eye[2]);
+            pt.x = static_cast<float> ((-right[0]*x[1] + up[0]*x[2] + viewray[0]*x[0] + eye[0])* scale);
+            pt.y = static_cast<float> ((-right[1]*x[1] + up[1]*x[2] + viewray[1]*x[0] + eye[1]) * scale);
+            pt.z = static_cast<float> ((-right[2]*x[1] + up[2]*x[2] + viewray[2]*x[0] + eye[2]) * scale);
             pt.vp_x = pt.vp_y = pt.vp_z = 0.0f;
           }
-          cloud.points.push_back (pt);
+          cloud.push_back (pt);
         }
         else
           if (organized)
@@ -378,7 +388,7 @@ main (int argc, char** argv)
             pt.vp_x = static_cast<float> (eye[0]);
             pt.vp_y = static_cast<float> (eye[1]);
             pt.vp_z = static_cast<float> (eye[2]);
-            cloud.points.push_back (pt);
+            cloud.push_back (pt);
           }
       } // Horizontal
     } // Vertical
@@ -405,22 +415,20 @@ main (int argc, char** argv)
     boost::trim (filename);
     boost::split (st, filename, boost::is_any_of ("/\\"), boost::token_compress_on);
 
-    std::stringstream ss;
-    std::string output_dir = st.at (st.size () - 1);
-    ss << output_dir << "_output";
+    const std::string output_dir = st.at (st.size () - 1) + "_output";
 
-    boost::filesystem::path outpath (ss.str ());
+    boost::filesystem::path outpath (output_dir);
     if (!boost::filesystem::exists (outpath))
     {
       if (!boost::filesystem::create_directories (outpath))
       {
-        PCL_ERROR ("Error creating directory %s.\n", ss.str ().c_str ());
+        PCL_ERROR ("Error creating directory %s.\n", output_dir.c_str ());
         return (-1);
       }
-      PCL_INFO ("Creating directory %s\n", ss.str ().c_str ());
+      PCL_INFO ("Creating directory %s\n", output_dir.c_str ());
     }
 
-    fname = ss.str () + "/" + seq + ".pcd";
+    fname = output_dir + '/' + seq + ".pcd";
 
     if (organized)
     {
diff --git a/tools/vlp_viewer.cpp b/tools/vlp_viewer.cpp
index 595a7f49..e823ece6 100644
--- a/tools/vlp_viewer.cpp
+++ b/tools/vlp_viewer.cpp
@@ -39,15 +39,12 @@
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl/common/time.h> //fps calculations
-#include <pcl/io/pcd_io.h>
 #include <pcl/io/hdl_grabber.h>
 #include <pcl/io/vlp_grabber.h>
 #include <pcl/visualization/point_cloud_color_handlers.h>
 #include <pcl/visualization/cloud_viewer.h>
 #include <pcl/visualization/image_viewer.h>
-#include <pcl/io/openni_camera/openni_driver.h>
 #include <pcl/console/parse.h>
-#include <pcl/visualization/boost.h>
 
 #include <boost/algorithm/string.hpp>
 
diff --git a/tools/voxel_grid_occlusion_estimation.cpp b/tools/voxel_grid_occlusion_estimation.cpp
index 2db20e31..48ee5de7 100644
--- a/tools/voxel_grid_occlusion_estimation.cpp
+++ b/tools/voxel_grid_occlusion_estimation.cpp
@@ -43,7 +43,6 @@
 #include <pcl/console/print.h>
 #include <pcl/console/parse.h>
 #include <pcl/console/time.h>
-#include <pcl/common/transforms.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/filters/voxel_grid_occlusion_estimation.h>
 #include <vtkCubeSource.h>
diff --git a/tools/xyz2pcd.cpp b/tools/xyz2pcd.cpp
index 2ade40cc..40848038 100644
--- a/tools/xyz2pcd.cpp
+++ b/tools/xyz2pcd.cpp
@@ -38,6 +38,7 @@
 #include <pcl/io/pcd_io.h>
 #include <pcl/console/print.h>
 #include <pcl/console/parse.h>
+#include <boost/algorithm/string/split.hpp> // for split
 
 using namespace pcl;
 using namespace pcl::io;
diff --git a/tracking/include/pcl/tracking/approx_nearest_pair_point_cloud_coherence.h b/tracking/include/pcl/tracking/approx_nearest_pair_point_cloud_coherence.h
index 31b60b51..0a5da5e6 100644
--- a/tracking/include/pcl/tracking/approx_nearest_pair_point_cloud_coherence.h
+++ b/tracking/include/pcl/tracking/approx_nearest_pair_point_cloud_coherence.h
@@ -1,7 +1,6 @@
 #pragma once
 
 #include <pcl/search/octree.h>
-#include <pcl/search/search.h>
 #include <pcl/tracking/nearest_pair_point_cloud_coherence.h>
 
 namespace pcl {
diff --git a/tracking/include/pcl/tracking/boost.h b/tracking/include/pcl/tracking/boost.h
deleted file mode 100644
index 64a8d172..00000000
--- a/tracking/include/pcl/tracking/boost.h
+++ /dev/null
@@ -1,46 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Point Cloud Library (PCL) - www.pointclouds.org
- *  Copyright (c) 2012-, Open Perception, Inc.
- *
- *  All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the copyright holder(s) nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- *
- * $Id: lmeds.h 1370 2011-06-19 01:06:01Z jspricke $
- *
- */
-
-#pragma once
-
-#include <pcl/pcl_macros.h>
-
-PCL_DEPRECATED_HEADER(1, 12, "")
-
-#include <boost/random.hpp>
diff --git a/tracking/include/pcl/tracking/impl/approx_nearest_pair_point_cloud_coherence.hpp b/tracking/include/pcl/tracking/impl/approx_nearest_pair_point_cloud_coherence.hpp
index 7d7bede7..60c85664 100644
--- a/tracking/include/pcl/tracking/impl/approx_nearest_pair_point_cloud_coherence.hpp
+++ b/tracking/include/pcl/tracking/impl/approx_nearest_pair_point_cloud_coherence.hpp
@@ -14,7 +14,7 @@ ApproxNearestPairPointCloudCoherence<PointInT>::computeCoherence(
   double val = 0.0;
   // for (std::size_t i = 0; i < indices->size (); i++)
   for (const auto& point : *cloud) {
-    int k_index = 0;
+    pcl::index_t k_index = 0;
     float k_distance = 0.0;
     // PointInT input_point = cloud->points[(*indices)[i]];
     PointInT input_point = point;
diff --git a/tracking/include/pcl/tracking/impl/distance_coherence.hpp b/tracking/include/pcl/tracking/impl/distance_coherence.hpp
index 972b1baf..601338d3 100644
--- a/tracking/include/pcl/tracking/impl/distance_coherence.hpp
+++ b/tracking/include/pcl/tracking/impl/distance_coherence.hpp
@@ -3,8 +3,6 @@
 
 #include <pcl/tracking/distance_coherence.h>
 
-#include <Eigen/Dense>
-
 namespace pcl {
 namespace tracking {
 template <typename PointInT>
diff --git a/tracking/include/pcl/tracking/impl/hsv_color_coherence.hpp b/tracking/include/pcl/tracking/impl/hsv_color_coherence.hpp
index 63be1395..c01b9e3e 100644
--- a/tracking/include/pcl/tracking/impl/hsv_color_coherence.hpp
+++ b/tracking/include/pcl/tracking/impl/hsv_color_coherence.hpp
@@ -45,8 +45,6 @@
 
 #include <pcl/tracking/hsv_color_coherence.h>
 
-#include <Eigen/Dense>
-
 namespace pcl {
 namespace tracking {
 // utility
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 46716bb9..8b036574 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
@@ -78,7 +78,7 @@ KLDAdaptiveParticleFilterOMPTracker<PointInT, StateT>::weight()
   else {
     std::vector<IndicesPtr> indices_list(particle_num_);
     for (int i = 0; i < particle_num_; i++) {
-      indices_list[i] = IndicesPtr(new std::vector<int>);
+      indices_list[i] = IndicesPtr(new pcl::Indices);
     }
     // clang-format off
 #pragma omp parallel for \
diff --git a/tracking/include/pcl/tracking/impl/nearest_pair_point_cloud_coherence.hpp b/tracking/include/pcl/tracking/impl/nearest_pair_point_cloud_coherence.hpp
index a14173db..5987bfcb 100644
--- a/tracking/include/pcl/tracking/impl/nearest_pair_point_cloud_coherence.hpp
+++ b/tracking/include/pcl/tracking/impl/nearest_pair_point_cloud_coherence.hpp
@@ -2,7 +2,6 @@
 #define PCL_TRACKING_IMPL_NEAREST_PAIR_POINT_CLOUD_COHERENCE_H_
 
 #include <pcl/search/kdtree.h>
-#include <pcl/search/organized.h>
 #include <pcl/tracking/nearest_pair_point_cloud_coherence.h>
 
 namespace pcl {
@@ -16,7 +15,7 @@ NearestPairPointCloudCoherence<PointInT>::computeCoherence(
   // for (std::size_t i = 0; i < indices->size (); i++)
   for (std::size_t i = 0; i < cloud->size(); i++) {
     PointInT input_point = (*cloud)[i];
-    std::vector<int> k_indices(1);
+    pcl::Indices k_indices(1);
     std::vector<float> k_distances(1);
     search_->nearestKSearch(input_point, 1, k_indices, k_distances);
     int k_index = k_indices[0];
diff --git a/tracking/include/pcl/tracking/impl/particle_filter.hpp b/tracking/include/pcl/tracking/impl/particle_filter.hpp
index 27daf315..14c3e85b 100644
--- a/tracking/include/pcl/tracking/impl/particle_filter.hpp
+++ b/tracking/include/pcl/tracking/impl/particle_filter.hpp
@@ -220,7 +220,7 @@ ParticleFilterTracker<PointInT, StateT>::testChangeDetection(
 {
   change_detector_->setInputCloud(input);
   change_detector_->addPointsFromInputCloud();
-  std::vector<int> newPointIdxVector;
+  pcl::Indices newPointIdxVector;
   change_detector_->getPointIndicesFromNewVoxels(newPointIdxVector,
                                                  change_detector_filter_);
   change_detector_->switchBuffers();
@@ -250,7 +250,7 @@ ParticleFilterTracker<PointInT, StateT>::weight()
   }
   else {
     for (std::size_t i = 0; i < particles_->size(); i++) {
-      IndicesPtr indices(new std::vector<int>);
+      IndicesPtr indices(new pcl::Indices);
       computeTransformedPointCloudWithNormal(
           (*particles_)[i], *indices, *transed_reference_vector_[i]);
     }
@@ -261,7 +261,7 @@ ParticleFilterTracker<PointInT, StateT>::weight()
     coherence_->setTargetCloud(coherence_input);
     coherence_->initCompute();
     for (std::size_t i = 0; i < particles_->size(); i++) {
-      IndicesPtr indices(new std::vector<int>);
+      IndicesPtr indices(new pcl::Indices);
       coherence_->compute(
           transed_reference_vector_[i], indices, (*particles_)[i].weight);
     }
@@ -273,7 +273,7 @@ ParticleFilterTracker<PointInT, StateT>::weight()
 template <typename PointInT, typename StateT>
 void
 ParticleFilterTracker<PointInT, StateT>::computeTransformedPointCloud(
-    const StateT& hypothesis, std::vector<int>& indices, PointCloudIn& cloud)
+    const StateT& hypothesis, pcl::Indices& indices, PointCloudIn& cloud)
 {
   if (use_normal_)
     computeTransformedPointCloudWithNormal(hypothesis, indices, cloud);
@@ -293,15 +293,11 @@ ParticleFilterTracker<PointInT, StateT>::computeTransformedPointCloudWithoutNorm
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointInT, typename StateT>
+template <typename PointT, pcl::traits::HasNormal<PointT>>
 void
 ParticleFilterTracker<PointInT, StateT>::computeTransformedPointCloudWithNormal(
-#ifdef PCL_TRACKING_NORMAL_SUPPORTED
-    const StateT& hypothesis, std::vector<int>& indices, PointCloudIn& cloud)
-#else
-    const StateT&, std::vector<int>&, PointCloudIn&)
-#endif
+    const StateT& hypothesis, pcl::Indices& indices, PointCloudIn& cloud)
 {
-#ifdef PCL_TRACKING_NORMAL_SUPPORTED
   const Eigen::Affine3f trans = toEigenMatrix(hypothesis);
   // destructively assigns to cloud
   pcl::transformPointCloudWithNormals<PointInT>(*ref_, cloud, trans);
@@ -318,11 +314,6 @@ ParticleFilterTracker<PointInT, StateT>::computeTransformedPointCloudWithNormal(
     if (theta > occlusion_angle_thr_)
       indices.push_back(i);
   }
-#else
-  PCL_WARN("[pcl::%s::computeTransformedPointCloudWithoutNormal] "
-           "use_normal_ == true is not supported in this Point Type.",
-           getClassName().c_str());
-#endif
 }
 
 template <typename PointInT, typename StateT>
diff --git a/tracking/include/pcl/tracking/impl/particle_filter_omp.hpp b/tracking/include/pcl/tracking/impl/particle_filter_omp.hpp
index 05534e96..a4cd2a1f 100644
--- a/tracking/include/pcl/tracking/impl/particle_filter_omp.hpp
+++ b/tracking/include/pcl/tracking/impl/particle_filter_omp.hpp
@@ -77,7 +77,7 @@ ParticleFilterOMPTracker<PointInT, StateT>::weight()
   else {
     std::vector<IndicesPtr> indices_list(particle_num_);
     for (int i = 0; i < particle_num_; i++) {
-      indices_list[i] = IndicesPtr(new std::vector<int>);
+      indices_list[i] = IndicesPtr(new pcl::Indices);
     }
     // clang-format off
 #pragma omp parallel for \
diff --git a/tracking/include/pcl/tracking/impl/pyramidal_klt.hpp b/tracking/include/pcl/tracking/impl/pyramidal_klt.hpp
index 38eccaf3..ebee0a18 100644
--- a/tracking/include/pcl/tracking/impl/pyramidal_klt.hpp
+++ b/tracking/include/pcl/tracking/impl/pyramidal_klt.hpp
@@ -69,8 +69,8 @@ PyramidalKLTTracker<PointInT, IntensityT>::setPointsToTrack(
     keypoints_ = p;
   }
 
-  keypoints_status_.reset(new pcl::PointIndices);
-  keypoints_status_->indices.resize(keypoints_->size(), 0);
+  keypoints_status_.reset(new std::vector<int>);
+  keypoints_status_->resize(keypoints_->size(), 0);
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////////////
@@ -105,13 +105,13 @@ PyramidalKLTTracker<PointInT, IntensityT>::initCompute()
 
   if (!input_->isOrganized()) {
     PCL_ERROR(
-        "[pcl::tracking::%s::initCompute] Need an organized point cloud to proceed!",
+        "[pcl::tracking::%s::initCompute] Need an organized point cloud to proceed!\n",
         tracker_name_.c_str());
     return (false);
   }
 
   if (!keypoints_ || keypoints_->empty()) {
-    PCL_ERROR("[pcl::tracking::%s::initCompute] No keypoints aborting!",
+    PCL_ERROR("[pcl::tracking::%s::initCompute] No keypoints aborting!\n",
               tracker_name_.c_str());
     return (false);
   }
@@ -144,14 +144,14 @@ PyramidalKLTTracker<PointInT, IntensityT>::initCompute()
 
     if (nb_levels_ < 2) {
       PCL_ERROR("[pcl::tracking::%s::initCompute] Number of pyramid levels should be "
-                "at least 2!",
+                "at least 2!\n",
                 tracker_name_.c_str());
       return (false);
     }
 
     if (nb_levels_ > 5) {
       PCL_ERROR("[pcl::tracking::%s::initCompute] Number of pyramid levels should not "
-                "exceed 5!",
+                "exceed 5!\n",
                 tracker_name_.c_str());
       return (false);
     }
@@ -723,7 +723,7 @@ PyramidalKLTTracker<PointInT, IntensityT>::computeTracking()
   ref_ = input_;
   ref_pyramid_ = pyramid;
   keypoints_ = keypoints;
-  keypoints_status_->indices = status;
+  *keypoints_status_ = status;
 }
 
 } // namespace tracking
diff --git a/tracking/include/pcl/tracking/impl/tracker.hpp b/tracking/include/pcl/tracking/impl/tracker.hpp
index f90a1983..ebebdd6e 100644
--- a/tracking/include/pcl/tracking/impl/tracker.hpp
+++ b/tracking/include/pcl/tracking/impl/tracker.hpp
@@ -1,11 +1,8 @@
 #ifndef PCL_TRACKING_IMPL_TRACKER_H_
 #define PCL_TRACKING_IMPL_TRACKER_H_
 
-#include <pcl/common/eigen.h>
 #include <pcl/tracking/tracker.h>
 
-#include <ctime>
-
 namespace pcl {
 namespace tracking {
 template <typename PointInT, typename StateT>
diff --git a/tracking/include/pcl/tracking/impl/tracking.hpp b/tracking/include/pcl/tracking/impl/tracking.hpp
index 7f442985..2fe9bbfd 100644
--- a/tracking/include/pcl/tracking/impl/tracking.hpp
+++ b/tracking/include/pcl/tracking/impl/tracking.hpp
@@ -6,8 +6,6 @@
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
 
-#include <ctime>
-
 namespace pcl {
 namespace tracking {
 struct _ParticleXYZRPY {
diff --git a/tracking/include/pcl/tracking/particle_filter.h b/tracking/include/pcl/tracking/particle_filter.h
index 9c28d0a5..f4cf99e6 100644
--- a/tracking/include/pcl/tracking/particle_filter.h
+++ b/tracking/include/pcl/tracking/particle_filter.h
@@ -6,8 +6,7 @@
 #include <pcl/tracking/tracker.h>
 #include <pcl/tracking/tracking.h>
 #include <pcl/memory.h>
-
-#include <Eigen/Dense>
+#include <pcl/point_types.h>
 
 namespace pcl {
 namespace tracking {
@@ -294,7 +293,14 @@ public:
   inline void
   setUseNormal(bool use_normal)
   {
-    use_normal_ = use_normal;
+    if (traits::has_normal_v<PointInT> || !use_normal) {
+      use_normal_ = use_normal;
+      return;
+    }
+    PCL_WARN("[pcl::%s::setUseNormal] "
+             "use_normal_ == true is not supported in this Point Type.\n",
+             getClassName().c_str());
+    use_normal_ = false;
   }
 
   /** \brief Get the value of use_normal_. */
@@ -437,9 +443,10 @@ protected:
    **/
   void
   computeTransformedPointCloud(const StateT& hypothesis,
-                               std::vector<int>& indices,
+                               pcl::Indices& indices,
                                PointCloudIn& cloud);
 
+#ifdef DOXYGEN_ONLY
   /** \brief Compute a reference pointcloud transformed to the pose that hypothesis
    * represents and calculate indices taking occlusion into account.
    * \param[in] hypothesis a particle which represents a hypothesis.
@@ -449,8 +456,23 @@ protected:
    **/
   void
   computeTransformedPointCloudWithNormal(const StateT& hypothesis,
-                                         std::vector<int>& indices,
+                                         pcl::Indices& indices,
+                                         PointCloudIn& cloud);
+#else
+  template <typename PointT = PointInT, traits::HasNormal<PointT> = true>
+  void
+  computeTransformedPointCloudWithNormal(const StateT& hypothesis,
+                                         pcl::Indices& indices,
                                          PointCloudIn& cloud);
+  template <typename PointT = PointInT, traits::HasNoNormal<PointT> = true>
+  void
+  computeTransformedPointCloudWithNormal(const StateT&, pcl::Indices&, PointCloudIn&)
+  {
+    PCL_WARN("[pcl::%s::computeTransformedPointCloudWithNormal] "
+             "use_normal_ == true is not supported in this Point Type.\n",
+             getClassName().c_str());
+  }
+#endif
 
   /** \brief Compute a reference pointcloud transformed to the pose that hypothesis
    * represents and calculate indices without taking occlusion into account.
diff --git a/tracking/include/pcl/tracking/pyramidal_klt.h b/tracking/include/pcl/tracking/pyramidal_klt.h
index bdc0e812..6f843616 100644
--- a/tracking/include/pcl/tracking/pyramidal_klt.h
+++ b/tracking/include/pcl/tracking/pyramidal_klt.h
@@ -260,8 +260,24 @@ public:
    * Status == -1 --> point is out of bond;
    * Status == -2 --> optical flow can not be computed for this point.
    */
+  PCL_DEPRECATED(1, 15, "use getStatusOfPointsToTrack instead")
   inline pcl::PointIndicesConstPtr
   getPointsToTrackStatus() const
+  {
+    pcl::PointIndicesPtr res(new pcl::PointIndices);
+    res->indices.insert(
+        res->indices.end(), keypoints_status_->begin(), keypoints_status_->end());
+    return (res);
+  }
+
+  /** \return the status of points to track.
+   * Status == 0  --> points successfully tracked;
+   * Status < 0   --> point is lost;
+   * Status == -1 --> point is out of bond;
+   * Status == -2 --> optical flow can not be computed for this point.
+   */
+  inline pcl::shared_ptr<const std::vector<int>>
+  getStatusOfPointsToTrack() const
   {
     return (keypoints_status_);
   }
@@ -398,7 +414,7 @@ protected:
   /** \brief detected keypoints 2D coordinates */
   pcl::PointCloud<pcl::PointUV>::ConstPtr keypoints_;
   /** \brief status of keypoints of t-1 at t */
-  pcl::PointIndicesPtr keypoints_status_;
+  pcl::shared_ptr<std::vector<int>> keypoints_status_;
   /** \brief number of points to detect */
   std::size_t keypoints_nbr_;
   /** \brief tracking width */
diff --git a/tracking/src/kld_adaptive_particle_filter.cpp b/tracking/src/kld_adaptive_particle_filter.cpp
index b11d00ad..2642364f 100644
--- a/tracking/src/kld_adaptive_particle_filter.cpp
+++ b/tracking/src/kld_adaptive_particle_filter.cpp
@@ -40,24 +40,13 @@
 #ifndef PCL_NO_PRECOMPILE
 #include <pcl/impl/instantiate.hpp>
 #include <pcl/point_types.h>
-#define PCL_TRACKING_NORMAL_SUPPORTED
 
 // clang-format off
 PCL_INSTANTIATE_PRODUCT(KLDAdaptiveParticleFilterTracker,
                         ((pcl::PointNormal)
                          (pcl::PointXYZINormal)
-                         (pcl::PointXYZRGBNormal))
-                        (PCL_STATE_POINT_TYPES))
-PCL_INSTANTIATE_PRODUCT(KLDAdaptiveParticleFilterOMPTracker,
-                        ((pcl::PointNormal)
-                         (pcl::PointXYZINormal)
-                         (pcl::PointXYZRGBNormal))
-                        (PCL_STATE_POINT_TYPES))
-// clang-format on
-#undef PCL_TRACKING_NORMAL_SUPPORTED
-// clang-format off
-PCL_INSTANTIATE_PRODUCT(KLDAdaptiveParticleFilterOMPTracker,
-                        ((pcl::PointXYZ)
+                         (pcl::PointXYZRGBNormal)
+                         (pcl::PointXYZ)
                          (pcl::PointXYZI)
                          (pcl::PointXYZRGBA)
                          (pcl::PointXYZRGB)
@@ -66,8 +55,12 @@ PCL_INSTANTIATE_PRODUCT(KLDAdaptiveParticleFilterOMPTracker,
                          (pcl::PointWithViewpoint)
                          (pcl::PointWithScale))
                         (PCL_STATE_POINT_TYPES))
-PCL_INSTANTIATE_PRODUCT(KLDAdaptiveParticleFilterTracker,
-                        ((pcl::PointXYZ)
+
+PCL_INSTANTIATE_PRODUCT(KLDAdaptiveParticleFilterOMPTracker,
+                        ((pcl::PointNormal)
+                         (pcl::PointXYZINormal)
+                         (pcl::PointXYZRGBNormal)
+                         (pcl::PointXYZ)
                          (pcl::PointXYZI)
                          (pcl::PointXYZRGBA)
                          (pcl::PointXYZRGB)
diff --git a/tracking/src/particle_filter.cpp b/tracking/src/particle_filter.cpp
index 71e26001..96e09f9c 100644
--- a/tracking/src/particle_filter.cpp
+++ b/tracking/src/particle_filter.cpp
@@ -40,24 +40,13 @@
 #ifndef PCL_NO_PRECOMPILE
 #include <pcl/impl/instantiate.hpp>
 #include <pcl/point_types.h>
-#define PCL_TRACKING_NORMAL_SUPPORTED
 
 // clang-format off
 PCL_INSTANTIATE_PRODUCT(ParticleFilterTracker,
                         ((pcl::PointNormal)
                          (pcl::PointXYZINormal)
-                         (pcl::PointXYZRGBNormal))
-                        (PCL_STATE_POINT_TYPES))
-PCL_INSTANTIATE_PRODUCT(ParticleFilterOMPTracker,
-                        ((pcl::PointNormal)
-                         (pcl::PointXYZINormal)
-                         (pcl::PointXYZRGBNormal))
-                        (PCL_STATE_POINT_TYPES))
-// clang-format on
-#undef PCL_TRACKING_NORMAL_SUPPORTED
-// clang-format off
-PCL_INSTANTIATE_PRODUCT(ParticleFilterTracker,
-                        ((pcl::PointXYZ)
+                         (pcl::PointXYZRGBNormal)
+                         (pcl::PointXYZ)
                          (pcl::PointXYZI)
                          (pcl::PointXYZRGBA)
                          (pcl::PointXYZRGB)
@@ -66,8 +55,12 @@ PCL_INSTANTIATE_PRODUCT(ParticleFilterTracker,
                          (pcl::PointWithViewpoint)
                          (pcl::PointWithScale))
                         (PCL_STATE_POINT_TYPES))
+
 PCL_INSTANTIATE_PRODUCT(ParticleFilterOMPTracker,
-                        ((pcl::PointXYZ)
+                        ((pcl::PointNormal)
+                         (pcl::PointXYZINormal)
+                         (pcl::PointXYZRGBNormal)
+                         (pcl::PointXYZ)
                          (pcl::PointXYZI)
                          (pcl::PointXYZRGBA)
                          (pcl::PointXYZRGB)
diff --git a/visualization/CMakeLists.txt b/visualization/CMakeLists.txt
index 990a5f5b..e676a202 100644
--- a/visualization/CMakeLists.txt
+++ b/visualization/CMakeLists.txt
@@ -61,6 +61,20 @@ if(VTK_RENDERING_BACKEND_OPENGL_VERSION VERSION_LESS 2)
     "src/vtk/vtkVertexBufferObject.cxx"
     "src/vtk/vtkVertexBufferObjectMapper.cxx"
   )
+
+  # Code in this module may use deprecated declarations when using OpenGLv1
+  # Add the GCC exclusive rules for -Werror only for OpenGLv1 compile to avoid build interruption
+  if(CMAKE_COMPILER_IS_GNUCXX)
+    add_compile_options("-Wno-error=deprecated-declarations")
+  endif()
+endif()
+
+if(NOT (${VTK_VERSION} VERSION_LESS 9.0))
+  if(NOT (";${VTK_AVAILABLE_COMPONENTS};" MATCHES ";RenderingContextOpenGL2;"))
+    list(REMOVE_ITEM srcs
+      src/pcl_painter2D.cpp
+    )
+  endif()  
 endif()
 
 set(incs
@@ -85,6 +99,7 @@ set(incs
   "include/pcl/${SUBSYS_NAME}/vtk.h"
   "include/pcl/${SUBSYS_NAME}/simple_buffer_visualizer.h"
   "include/pcl/${SUBSYS_NAME}/pcl_plotter.h"
+  "include/pcl/${SUBSYS_NAME}/qvtk_compatibility.h"
 )
 
 set(common_incs
@@ -116,6 +131,7 @@ set(vtk_incs
   "include/pcl/${SUBSYS_NAME}/vtk/pcl_image_canvas_source_2d.h"
   "include/pcl/${SUBSYS_NAME}/vtk/pcl_context_item.h"
   "include/pcl/${SUBSYS_NAME}/vtk/vtkRenderWindowInteractorFix.h"
+  "include/pcl/${SUBSYS_NAME}/vtk/pcl_vtk_compatibility.h"
 )
 
 if(VTK_RENDERING_BACKEND_OPENGL_VERSION VERSION_LESS 2)
@@ -125,6 +141,15 @@ if(VTK_RENDERING_BACKEND_OPENGL_VERSION VERSION_LESS 2)
   )
 endif()
 
+if(NOT (${VTK_VERSION} VERSION_LESS 9.0))
+  if(NOT (";${VTK_AVAILABLE_COMPONENTS};" MATCHES ";RenderingContextOpenGL2;"))
+    
+    list(REMOVE_ITEM incs
+      "include/pcl/${SUBSYS_NAME}/pcl_painter2D.h"
+    )
+  endif()
+endif()
+
 # on apple, a workaround is used for the cocoa render window interactor
 if(APPLE)
   list(APPEND srcs
@@ -134,14 +159,44 @@ endif()
 set(LIB_NAME "pcl_${SUBSYS_NAME}")
 PCL_ADD_LIBRARY(${LIB_NAME} COMPONENT ${SUBSYS_NAME} SOURCES ${srcs} ${incs} ${common_incs} ${impl_incs} ${common_impl_incs} ${vtk_incs})
 
-target_include_directories("${LIB_NAME}" SYSTEM PUBLIC ${VTK_INCLUDE_DIRS})
-
 # apple workaround (continued)
 if(APPLE)
   target_link_libraries("${LIB_NAME}" "-framework Cocoa")
 endif()
 
-target_link_libraries("${LIB_NAME}" pcl_common pcl_io pcl_kdtree ${VTK_LIBRARIES} ${OPENGL_LIBRARIES})
+target_link_libraries("${LIB_NAME}" pcl_common pcl_io pcl_kdtree ${OPENGL_LIBRARIES})
+
+if(${VTK_VERSION} VERSION_LESS 9.0)
+  target_include_directories("${LIB_NAME}" SYSTEM PUBLIC ${VTK_INCLUDE_DIRS})
+  target_link_libraries("${LIB_NAME}" ${VTK_LIBRARIES})
+else()
+  #Some libs are referenced through depending on IO
+  target_link_libraries("${LIB_NAME}"
+                        VTK::ChartsCore
+                        VTK::CommonColor
+                        VTK::CommonComputationalGeometry
+                        VTK::CommonDataModel
+                        VTK::FiltersExtraction
+                        VTK::FiltersGeometry
+                        VTK::FiltersGeneral
+                        VTK::FiltersModeling
+                        VTK::FiltersSources
+                        VTK::IOImage
+                        VTK::IOPLY
+                        VTK::ImagingSources
+                        VTK::InteractionImage
+                        VTK::InteractionStyle
+                        VTK::RenderingAnnotation
+                        VTK::RenderingContext2D
+                        VTK::RenderingFreeType
+                        VTK::RenderingLOD
+                        VTK::RenderingOpenGL2
+                        VTK::ViewsContext2D)
+                        
+  if(";${VTK_AVAILABLE_COMPONENTS};" MATCHES ";RenderingContextOpenGL2;")
+    target_link_libraries("${LIB_NAME}" VTK::RenderingContextOpenGL2)
+  endif()
+endif()
 
 set(EXT_DEPS "")
 if(WITH_OPENNI)
@@ -162,6 +217,7 @@ endif()
 if(WITH_RSSDK)
   list(APPEND EXT_DEPS rssdk)
 endif()
+
 PCL_MAKE_PKGCONFIG(${LIB_NAME} COMPONENT ${SUBSYS_NAME} DESC ${SUBSYS_DESC} PCL_DEPS ${SUBSYS_DEPS} EXT_DEPS ${EXT_DEPS})
 
 # Install include files
@@ -171,6 +227,13 @@ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs})
 PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/common/impl" ${common_impl_incs})
 PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/vtk" ${vtk_incs})
 
+#TODO: Update when CMAKE 3.10 is available
+if(NOT (${VTK_VERSION} VERSION_LESS 9.0))
+  vtk_module_autoinit(TARGETS "${LIB_NAME}" 
+                      MODULES VTK::RenderingOpenGL2
+                              VTK::RenderingFreeType)
+endif()
+
 if(BUILD_TESTS)
   add_subdirectory(test)
 endif()
diff --git a/visualization/include/pcl/visualization/area_picking_event.h b/visualization/include/pcl/visualization/area_picking_event.h
index ae833990..11b0886d 100644
--- a/visualization/include/pcl/visualization/area_picking_event.h
+++ b/visualization/include/pcl/visualization/area_picking_event.h
@@ -40,8 +40,6 @@
 
 #include <pcl/pcl_macros.h>
 
-#include <vector>
-
 namespace pcl
 {
   namespace visualization
@@ -50,7 +48,7 @@ namespace pcl
     class PCL_EXPORTS AreaPickingEvent
     {
       public:
-        AreaPickingEvent (int nb_points, const std::vector<int>& indices)
+        AreaPickingEvent (int nb_points, const pcl::Indices& indices)
           : nb_points_ (nb_points)
           , indices_ (indices)
         {}
@@ -60,7 +58,7 @@ namespace pcl
           * \return true, if the area selected by the user contains points, false otherwise
           */
         inline bool
-        getPointsIndices (std::vector<int>& indices) const
+        getPointsIndices (pcl::Indices& indices) const
         {
           if (nb_points_ <= 0)
             return (false);
@@ -70,7 +68,7 @@ namespace pcl
 
       private:
         int nb_points_;
-        std::vector<int> indices_;
+        pcl::Indices indices_;
     };
   } //namespace visualization
 } //namespace pcl
diff --git a/visualization/include/pcl/visualization/boost.h b/visualization/include/pcl/visualization/boost.h
index 7f7ec385..bd3fae78 100644
--- a/visualization/include/pcl/visualization/boost.h
+++ b/visualization/include/pcl/visualization/boost.h
@@ -43,8 +43,6 @@
 #  pragma GCC system_header 
 #endif
 
-//https://bugreports.qt-project.org/browse/QTBUG-22829
-#ifndef Q_MOC_RUN
 #include <boost/shared_array.hpp>
 #define BOOST_PARAMETER_MAX_ARITY 7
 #include <boost/signals2.hpp>
@@ -52,8 +50,5 @@
 #include <boost/algorithm/string/split.hpp>
 #include <boost/algorithm/string/classification.hpp>
 #include <boost/foreach.hpp>
-#ifndef Q_MOC_RUN
 #include <boost/date_time/posix_time/posix_time.hpp>
-#endif
 #include <boost/filesystem.hpp>
-#endif
diff --git a/visualization/include/pcl/visualization/common/actor_map.h b/visualization/include/pcl/visualization/common/actor_map.h
index 8434d670..68025d91 100644
--- a/visualization/include/pcl/visualization/common/actor_map.h
+++ b/visualization/include/pcl/visualization/common/actor_map.h
@@ -37,14 +37,14 @@
 
 #pragma once
 
-#include <pcl/visualization/boost.h>
-#include <pcl/visualization/point_cloud_handlers.h>
+#include <pcl/visualization/point_cloud_geometry_handlers.h> // for PointCloudGeometryHandler
+#include <pcl/visualization/point_cloud_color_handlers.h> // for PointCloudColorHandler
 #include <pcl/PCLPointCloud2.h>
 
 #include <vtkLODActor.h>
 #include <vtkSmartPointer.h>
+#include <vtkIdTypeArray.h>
 
-#include <map>
 #include <unordered_map>
 #include <vector>
 
diff --git a/visualization/include/pcl/visualization/common/common.h b/visualization/include/pcl/visualization/common/common.h
index 4f3e94d3..67f33740 100644
--- a/visualization/include/pcl/visualization/common/common.h
+++ b/visualization/include/pcl/visualization/common/common.h
@@ -41,8 +41,9 @@
 #pragma GCC system_header
 #endif
 
+#include <Eigen/Core> // for Matrix, ...
+
 #include <pcl/pcl_macros.h>
-#include <pcl/visualization/eigen.h>
 #include <vtkMatrix4x4.h>
 #include <vtkSmartPointer.h>
 #include <vtkLookupTable.h>
diff --git a/visualization/include/pcl/visualization/common/float_image_utils.h b/visualization/include/pcl/visualization/common/float_image_utils.h
index 19bfd39d..bc5de74e 100644
--- a/visualization/include/pcl/visualization/common/float_image_utils.h
+++ b/visualization/include/pcl/visualization/common/float_image_utils.h
@@ -37,8 +37,6 @@
 #include <pcl/pcl_config.h>
 #include <pcl/pcl_macros.h>
 
-#include <iostream>
-#include <cmath>
 #include <limits>
 
 namespace pcl
diff --git a/visualization/include/pcl/visualization/common/io.h b/visualization/include/pcl/visualization/common/io.h
index 059b4d68..f4c4ab4e 100644
--- a/visualization/include/pcl/visualization/common/io.h
+++ b/visualization/include/pcl/visualization/common/io.h
@@ -39,7 +39,6 @@
 #pragma once
 
 #include <pcl/visualization/common/actor_map.h>
-#include <pcl/console/print.h>
 
 class vtkPolyData;
 
@@ -55,7 +54,7 @@ namespace pcl
       * \ingroup visualization
       */
     PCL_EXPORTS void 
-    getCorrespondingPointCloud (vtkPolyData *src, const pcl::PointCloud<pcl::PointXYZ> &tgt, std::vector<int> &indices);
+    getCorrespondingPointCloud (vtkPolyData *src, const pcl::PointCloud<pcl::PointXYZ> &tgt, pcl::Indices &indices);
 
     /** \brief Saves the vtk-formatted Point Cloud data into a set of files, based on whether
       * the data comes from previously loaded PCD files. The PCD files are matched using the 
diff --git a/visualization/include/pcl/visualization/common/ren_win_interact_map.h b/visualization/include/pcl/visualization/common/ren_win_interact_map.h
index 45145661..1841f13a 100644
--- a/visualization/include/pcl/visualization/common/ren_win_interact_map.h
+++ b/visualization/include/pcl/visualization/common/ren_win_interact_map.h
@@ -41,8 +41,9 @@
 #include <map>
 #include <string>
 
+#include <vtkXYPlotActor.h>
+
 template <typename T> class vtkSmartPointer;
-class vtkXYPlotActor;
 class vtkRenderer;
 class vtkRenderWindow;
 class vtkRenderWindowInteractor;
diff --git a/visualization/include/pcl/visualization/common/shapes.h b/visualization/include/pcl/visualization/common/shapes.h
index 2eb2317d..d74389dd 100644
--- a/visualization/include/pcl/visualization/common/shapes.h
+++ b/visualization/include/pcl/visualization/common/shapes.h
@@ -40,7 +40,6 @@
 #pragma once
 
 #include <pcl/point_cloud.h>
-#include <pcl/visualization/eigen.h>
 
 template <typename T> class vtkSmartPointer;
 class vtkDataSet;
@@ -281,6 +280,18 @@ namespace pcl
     createCube (double x_min, double x_max,
                 double y_min, double y_max,
                 double z_min, double z_max);
+
+    /** \brief Create an ellipsoid shape from the given parameters.
+      *
+      * \param[in] transform a transformation to apply to the ellipsoid from 0,0,0
+      * \param[in] radius_x the ellipsoid's radius along its local x-axis
+      * \param[in] radius_y the ellipsoid's radius along its local y-axis
+      * \param[in] radius_z the ellipsoid's radius along its local z-axis
+      * \ingroup visualization
+      */
+    PCL_EXPORTS vtkSmartPointer<vtkDataSet> 
+    createEllipsoid (const Eigen::Isometry3d &transform,
+                     double radius_x, double radius_y, double radius_z);
     
     /** \brief Allocate a new unstructured grid smartpointer. For internal use only.
       * \param[out] polydata the resultant unstructured grid. 
diff --git a/visualization/include/pcl/visualization/eigen.h b/visualization/include/pcl/visualization/eigen.h
index 4c32fbb9..09d57c50 100644
--- a/visualization/include/pcl/visualization/eigen.h
+++ b/visualization/include/pcl/visualization/eigen.h
@@ -42,6 +42,7 @@
 #if defined __GNUC__
 #  pragma GCC system_header 
 #endif
+PCL_DEPRECATED_HEADER(1, 15, "Please include the needed eigen headers directly.")
 
 #include <Eigen/Geometry>
 #include <Eigen/Dense>
diff --git a/visualization/include/pcl/visualization/histogram_visualizer.h b/visualization/include/pcl/visualization/histogram_visualizer.h
index f924f338..ba86848b 100644
--- a/visualization/include/pcl/visualization/histogram_visualizer.h
+++ b/visualization/include/pcl/visualization/histogram_visualizer.h
@@ -116,7 +116,7 @@ namespace pcl
         template <typename PointT> bool 
         addFeatureHistogram (const pcl::PointCloud<PointT> &cloud, 
                              const std::string &field_name, 
-                             const int index,
+                             const pcl::index_t index,
                              const std::string &id = "cloud", int win_width = 640, int win_height = 200);
 
         /** \brief Add a histogram feature to screen as a separate window.
@@ -130,7 +130,7 @@ namespace pcl
         bool 
         addFeatureHistogram (const pcl::PCLPointCloud2 &cloud,
                              const std::string &field_name, 
-                             const int index,
+                             const pcl::index_t index,
                              const std::string &id = "cloud", int win_width = 640, int win_height = 200);
         
         /** \brief Update a histogram feature that is already on screen, with a cloud containing a single histogram.
@@ -161,7 +161,7 @@ namespace pcl
           */
         template <typename PointT> bool 
         updateFeatureHistogram (const pcl::PointCloud<PointT> &cloud, const std::string &field_name,
-        			                   const int index, const std::string &id = "cloud");
+        			                   const pcl::index_t index, const std::string &id = "cloud");
         
                              
         /** \brief Update a histogram feature that is already on screen, with a cloud containing a single histogram.
@@ -172,7 +172,7 @@ namespace pcl
           */
         bool 
         updateFeatureHistogram (const pcl::PCLPointCloud2 &cloud,
-                                const std::string &field_name, const int index,
+                                const std::string &field_name, const pcl::index_t index,
                                 const std::string &id = "cloud");         
 
 
diff --git a/visualization/include/pcl/visualization/impl/histogram_visualizer.hpp b/visualization/include/pcl/visualization/impl/histogram_visualizer.hpp
index be948bdd..c41cdb3f 100644
--- a/visualization/include/pcl/visualization/impl/histogram_visualizer.hpp
+++ b/visualization/include/pcl/visualization/impl/histogram_visualizer.hpp
@@ -86,7 +86,7 @@ template <typename PointT> bool
 PCLHistogramVisualizer::addFeatureHistogram (
     const pcl::PointCloud<PointT> &cloud,
     const std::string &field_name,
-    const int index,
+    const pcl::index_t index,
     const std::string &id, int win_width, int win_height)
 {
   if (index < 0 || index >= cloud.size ())
@@ -118,7 +118,7 @@ PCLHistogramVisualizer::addFeatureHistogram (
 
   // Parse the cloud data and store it in the array
   double xy[2];
-  for (std::uint32_t d = 0; d < fields[field_idx].count; ++d)
+  for (uindex_t d = 0; d < fields[field_idx].count; ++d)
   {
     xy[0] = d;
     //xy[1] = cloud[index].histogram[d];
@@ -168,7 +168,7 @@ PCLHistogramVisualizer::updateFeatureHistogram (
 
 template <typename PointT> bool
 PCLHistogramVisualizer::updateFeatureHistogram (
-    const pcl::PointCloud<PointT> &cloud, const std::string &field_name, const int index,
+    const pcl::PointCloud<PointT> &cloud, const std::string &field_name, const pcl::index_t index,
     const std::string &id)
 {
   if (index < 0 || index >= cloud.size ())
diff --git a/visualization/include/pcl/visualization/impl/pcl_plotter.hpp b/visualization/include/pcl/visualization/impl/pcl_plotter.hpp
index 8a9c3ac2..aca05fab 100644
--- a/visualization/include/pcl/visualization/impl/pcl_plotter.hpp
+++ b/visualization/include/pcl/visualization/impl/pcl_plotter.hpp
@@ -69,7 +69,7 @@ template <typename PointT> bool
 PCLPlotter::addFeatureHistogram (
     const pcl::PointCloud<PointT> &cloud,
     const std::string &field_name,
-    const int index,
+    const pcl::index_t index,
     const std::string &id, int win_width, int win_height)
 {
   if (index < 0 || index >= cloud.size ())
diff --git a/visualization/include/pcl/visualization/impl/pcl_visualizer.hpp b/visualization/include/pcl/visualization/impl/pcl_visualizer.hpp
index a3346cdc..4eca4e2b 100644
--- a/visualization/include/pcl/visualization/impl/pcl_visualizer.hpp
+++ b/visualization/include/pcl/visualization/impl/pcl_visualizer.hpp
@@ -62,6 +62,7 @@
 #include <vtkLODActor.h>
 #include <vtkLineSource.h>
 
+#include <pcl/common/utils.h> // pcl::utils::ignore
 #include <pcl/visualization/common/shapes.h>
 
 // Support for VTK 7.1 upwards
@@ -270,11 +271,40 @@ pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData (
     points->SetNumberOfPoints (nr_points);
   }
 
+#ifdef VTK_CELL_ARRAY_V2
+  // TODO: Remove when VTK 6,7,8 is unsupported
+  pcl::utils::ignore(initcells);
+
+  auto numOfCells = vertices->GetNumberOfCells();
+
+  // If we have less cells than points, add new cells.
+  if (numOfCells < nr_points)
+  {
+    for (int i = numOfCells; i < nr_points; i++)
+    {
+      vertices->InsertNextCell(1);
+      vertices->InsertCellPoint(i);
+    }
+  }
+  // if we too many cells than points, set size (doesn't free excessive memory)
+  else if (numOfCells > nr_points)
+  {
+    vertices->ResizeExact(nr_points, nr_points);
+  }
+
+  polydata->SetPoints(points);
+  polydata->SetVerts(vertices);
+
+#else
   vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
   updateCells (cells, initcells, nr_points);
 
   // 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);
+#endif
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
@@ -304,10 +334,36 @@ pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData (
   if (!vertices)
     vertices = vtkSmartPointer<vtkCellArray>::New ();
 
+#ifdef VTK_CELL_ARRAY_V2
+  // TODO: Remove when VTK 6,7,8 is unsupported
+  pcl::utils::ignore(initcells);
+
+  auto numOfCells = vertices->GetNumberOfCells();
+
+  // If we have less cells than points, add new cells.
+  if (numOfCells < nr_points)
+  {
+    for (int i = numOfCells; i < nr_points; i++)
+    {
+      vertices->InsertNextCell(1);
+      vertices->InsertCellPoint(i);
+    }
+  }
+  // if we too many cells than points, set size (doesn't free excessive memory)
+  else if (numOfCells > nr_points)
+  {
+    vertices->ResizeExact(nr_points, nr_points);
+  }
+
+  polydata->SetPoints(points);
+  polydata->SetVerts(vertices);
+
+#else
   vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
   updateCells (cells, initcells, nr_points);
   // Set the cells and the vertices
   vertices->SetCells (nr_points, cells);
+#endif
 }
 
 ////////////////////////////////////////////////////////////////////////////////////////////
@@ -651,7 +707,7 @@ pcl::visualization::PCLVisualizer::addText3D (
   // If there is no custom viewport and the viewport number is not 0, exit
   if (rens_->GetNumberOfItems () <= viewport)
   {
-    PCL_ERROR ("[addText3D] The viewport [%d] doesn't exist (id <%s>)! ",
+    PCL_ERROR ("[addText3D] The viewport [%d] doesn't exist (id <%s>)! \n",
                viewport,
                tid.c_str ());
     return false;
@@ -664,7 +720,7 @@ pcl::visualization::PCLVisualizer::addText3D (
     const std::string uid = tid + std::string (i, '*');
     if (contains (uid))
     {
-      PCL_ERROR ( "[addText3D] The id <%s> already exists in viewport [%d]! "
+      PCL_ERROR ( "[addText3D] The id <%s> already exists in viewport [%d]! \n"
                   "Please choose a different id and retry.\n",
                   tid.c_str (),
                   i);
@@ -738,7 +794,7 @@ pcl::visualization::PCLVisualizer::addText3D (
   // If there is no custom viewport and the viewport number is not 0, exit
   if (rens_->GetNumberOfItems () <= viewport)
   {
-    PCL_ERROR ("[addText3D] The viewport [%d] doesn't exist (id <%s>)! ",
+    PCL_ERROR ("[addText3D] The viewport [%d] doesn't exist (id <%s>)!\n",
                viewport,
                tid.c_str ());
     return false;
@@ -1199,7 +1255,7 @@ pcl::visualization::PCLVisualizer::addCorrespondences (
   // Draw lines between the best corresponding points
   for (std::size_t i = 0; i < correspondences.size (); i += nth, ++j)
   {
-    if (correspondences[i].index_match == -1)
+    if (correspondences[i].index_match == UNAVAILABLE)
     {
       PCL_WARN ("[addCorrespondences] No valid index_match for correspondence %d\n", i);
       continue;
@@ -1304,7 +1360,6 @@ pcl::visualization::PCLVisualizer::fromHandlersToScreen (
   vtkSmartPointer<vtkIdTypeArray> initcells;
   // Convert the PointCloud to VTK PolyData
   convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
-  // use the given geometry handler
 
   // Get the colors from the handler
   bool has_colors = false;
@@ -1550,45 +1605,8 @@ pcl::visualization::PCLVisualizer::updatePointCloud (const typename pcl::PointCl
   vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
   if (!polydata)
     return (false);
-  vtkSmartPointer<vtkCellArray> vertices = polydata->GetVerts ();
-  vtkSmartPointer<vtkPoints> points      = polydata->GetPoints ();
-  // Copy the new point array in
-  vtkIdType nr_points = cloud->size ();
-  points->SetNumberOfPoints (nr_points);
-
-  // Get a pointer to the beginning of the data array
-  float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
-
-  vtkIdType pts = 0;
-  // If the dataset is dense (no NaNs)
-  if (cloud->is_dense)
-  {
-    for (vtkIdType i = 0; i < nr_points; ++i, pts += 3)
-      std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[pts]);
-  }
-  else
-  {
-    vtkIdType j = 0;    // true point index
-    for (vtkIdType i = 0; i < nr_points; ++i)
-    {
-      // Check if the point is invalid
-      if (!isFinite ((*cloud)[i]))
-        continue;
-      std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[pts]);
-      pts += 3;
-      j++;
-    }
-    nr_points = j;
-    points->SetNumberOfPoints (nr_points);
-  }
 
-  vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
-  updateCells (cells, am_it->second.cells, nr_points);
-
-  // 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);
+  convertPointCloudToVTKPolyData<PointT>(cloud, polydata, am_it->second.cells);
 
   // Get the colors from the handler
   bool has_colors = false;
@@ -1701,32 +1719,9 @@ pcl::visualization::PCLVisualizer::addPolygonMesh (
   {
     // Create polys from polyMesh.polygons
     vtkSmartPointer<vtkCellArray> cell_array = vtkSmartPointer<vtkCellArray>::New ();
-    vtkIdType *cell = cell_array->WritePointer (vertices.size (), vertices.size () * (max_size_of_polygon + 1));
-    int idx = 0;
-    if (!lookup.empty ())
-    {
-      for (std::size_t i = 0; i < vertices.size (); ++i, ++idx)
-      {
-        std::size_t n_points = vertices[i].vertices.size ();
-        *cell++ = n_points;
-        //cell_array->InsertNextCell (n_points);
-        for (std::size_t j = 0; j < n_points; j++, ++idx)
-          *cell++ = lookup[vertices[i].vertices[j]];
-          //cell_array->InsertCellPoint (lookup[vertices[i].vertices[j]]);
-      }
-    }
-    else
-    {
-      for (std::size_t i = 0; i < vertices.size (); ++i, ++idx)
-      {
-        std::size_t n_points = vertices[i].vertices.size ();
-        *cell++ = n_points;
-        //cell_array->InsertNextCell (n_points);
-        for (std::size_t j = 0; j < n_points; j++, ++idx)
-          *cell++ = vertices[i].vertices[j];
-          //cell_array->InsertCellPoint (vertices[i].vertices[j]);
-      }
-    }
+    
+    const auto idx = details::fillCells(lookup,vertices,cell_array, max_size_of_polygon);
+
     vtkSmartPointer<vtkPolyData> polydata;
     allocVtkPolyData (polydata);
     cell_array->GetData ()->SetNumberOfValues (idx);
@@ -1878,28 +1873,9 @@ pcl::visualization::PCLVisualizer::updatePolygonMesh (
 
   // Update the cells
   cells = vtkSmartPointer<vtkCellArray>::New ();
-  vtkIdType *cell = cells->WritePointer (verts.size (), verts.size () * (max_size_of_polygon + 1));
-  int idx = 0;
-  if (!lookup.empty ())
-  {
-    for (std::size_t i = 0; i < verts.size (); ++i, ++idx)
-    {
-      std::size_t n_points = verts[i].vertices.size ();
-      *cell++ = n_points;
-      for (std::size_t j = 0; j < n_points; j++, cell++, ++idx)
-        *cell = lookup[verts[i].vertices[j]];
-    }
-  }
-  else
-  {
-    for (std::size_t i = 0; i < verts.size (); ++i, ++idx)
-    {
-      std::size_t n_points = verts[i].vertices.size ();
-      *cell++ = n_points;
-      for (std::size_t j = 0; j < n_points; j++, cell++, ++idx)
-        *cell = verts[i].vertices[j];
-    }
-  }
+  
+  const auto idx = details::fillCells(lookup, verts, cells, max_size_of_polygon);
+
   cells->GetData ()->SetNumberOfValues (idx);
   cells->Squeeze ();
   // Set the the vertices
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 73a053f2..a91317ce 100644
--- a/visualization/include/pcl/visualization/impl/point_cloud_color_handlers.hpp
+++ b/visualization/include/pcl/visualization/impl/point_cloud_color_handlers.hpp
@@ -42,6 +42,7 @@
 
 #include <pcl/pcl_macros.h>
 #include <pcl/common/colors.h>
+#include <pcl/common/io.h> // for getFieldIndex
 #include <pcl/common/point_tests.h> // for pcl::isFinite
 
 
diff --git a/visualization/include/pcl/visualization/impl/registration_visualizer.hpp b/visualization/include/pcl/visualization/impl/registration_visualizer.hpp
index 2b076c48..85b7776e 100644
--- a/visualization/include/pcl/visualization/impl/registration_visualizer.hpp
+++ b/visualization/include/pcl/visualization/impl/registration_visualizer.hpp
@@ -143,10 +143,9 @@ RegistrationVisualizer<PointSource, PointTarget>::runDisplay ()
     std::size_t correspondences_new_size = cloud_intermediate_indices_.size ();
 
 
-    std::stringstream stream_;
-    stream_ << "Random -> correspondences " << correspondences_new_size;
+    const std::string correspondences_text = "Random -> correspondences " + std::to_string(correspondences_new_size);
     viewer_->removeShape ("correspondences_size", 0);
-    viewer_->addText (stream_.str(), 10, 70, 0.0, 1.0, 0.0, "correspondences_size", v2);
+    viewer_->addText (correspondences_text, 10, 70, 0.0, 1.0, 0.0, "correspondences_size", v2);
 
     // Display entire set of correspondece lines if no maximum displayed correspondences is set
     if( ( 0 < maximum_displayed_correspondences_ ) &&
@@ -188,9 +187,9 @@ RegistrationVisualizer<PointSource, PointTarget>::runDisplay ()
 template<typename PointSource, typename PointTarget> void
 RegistrationVisualizer<PointSource, PointTarget>::updateIntermediateCloud (
     const pcl::PointCloud<PointSource> &cloud_src,
-    const std::vector<int> &indices_src,
+    const pcl::Indices &indices_src,
     const pcl::PointCloud<PointTarget> &cloud_tgt,
-    const std::vector<int> &indices_tgt)
+    const pcl::Indices &indices_tgt)
 {
   // Lock local buffers
   visualizer_updating_mutex_.lock ();
diff --git a/visualization/include/pcl/visualization/interactor_style.h b/visualization/include/pcl/visualization/interactor_style.h
index 0f166b75..6c4d6195 100644
--- a/visualization/include/pcl/visualization/interactor_style.h
+++ b/visualization/include/pcl/visualization/interactor_style.h
@@ -40,17 +40,16 @@
 
 #pragma once
 
-#include <pcl/console/print.h>
 #include <pcl/visualization/common/actor_map.h>
 #include <pcl/visualization/common/ren_win_interact_map.h>
 #include <pcl/visualization/keyboard_event.h>
 #include <pcl/visualization/mouse_event.h>
 #include <pcl/visualization/point_picking_event.h>
 #include <pcl/visualization/area_picking_event.h>
-#ifndef Q_MOC_RUN
 #include <boost/signals2/signal.hpp>
-#endif
 #include <vtkInteractorStyleRubberBandPick.h>
+#include <vtkRendererCollection.h>
+#include <vtkRenderWindow.h>
 
 class vtkRendererCollection;
 class vtkLegendScaleActor;
diff --git a/visualization/include/pcl/visualization/pcl_painter2D.h b/visualization/include/pcl/visualization/pcl_painter2D.h
index f4180e94..4b34cda9 100644
--- a/visualization/include/pcl/visualization/pcl_painter2D.h
+++ b/visualization/include/pcl/visualization/pcl_painter2D.h
@@ -38,8 +38,6 @@
 
 #pragma once
 
-#include <iostream>
-#include <map>
 #include <vector>
 #include <pcl/pcl_exports.h>
 #include <vtkRenderer.h>
diff --git a/visualization/include/pcl/visualization/pcl_plotter.h b/visualization/include/pcl/visualization/pcl_plotter.h
index 3aeeb585..93f78695 100644
--- a/visualization/include/pcl/visualization/pcl_plotter.h
+++ b/visualization/include/pcl/visualization/pcl_plotter.h
@@ -41,7 +41,6 @@
 #include <iostream>
 #include <vector>
 #include <utility>
-#include <cfloat>
 
 #include <pcl/visualization/common/common.h>
 #include <pcl/point_types.h>
@@ -49,16 +48,16 @@
 #include <pcl/point_cloud.h>
 #include <pcl/common/io.h>
 
-class vtkRenderWindow;
-class vtkRenderWindowInteractor;
-class vtkContextView;
-class vtkChartXY;
-class vtkColorSeries;
-
+#include <vtkContextView.h>
+#include <vtkChartXY.h>
+#include <vtkColorSeries.h>
 #include <vtkSmartPointer.h>
 #include <vtkCommand.h>
 #include <vtkChart.h>
 
+class vtkRenderWindow;
+class vtkRenderWindowInteractor;
+
 namespace pcl
 {
   namespace visualization
@@ -244,7 +243,7 @@ namespace pcl
         template <typename PointT> bool 
         addFeatureHistogram (const pcl::PointCloud<PointT> &cloud, 
                              const std::string &field_name, 
-                             const int index,
+                             const pcl::index_t index,
                              const std::string &id = "cloud", int win_width = 640, int win_height = 200);
         
         /** \brief Add a histogram feature to screen as a separate window.
@@ -258,7 +257,7 @@ namespace pcl
         bool 
         addFeatureHistogram (const pcl::PCLPointCloud2 &cloud,
                              const std::string &field_name, 
-                             const int index,
+                             const pcl::index_t index,
                              const std::string &id = "cloud", int win_width = 640, int win_height = 200);
         
         /** \brief Draws all the plots added by addPlotData() or addHistogramData() till now */
diff --git a/visualization/include/pcl/visualization/pcl_visualizer.h b/visualization/include/pcl/visualization/pcl_visualizer.h
index 908b9de9..c847ac9a 100644
--- a/visualization/include/pcl/visualization/pcl_visualizer.h
+++ b/visualization/include/pcl/visualization/pcl_visualizer.h
@@ -53,14 +53,15 @@
 #include <pcl/visualization/area_picking_event.h>
 #include <pcl/visualization/interactor_style.h>
 
+#include <vtkOrientationMarkerWidget.h>
+#include <vtkRenderWindowInteractor.h>
+
 // VTK includes
 class vtkPolyData;
 class vtkTextActor;
 class vtkRenderWindow;
-class vtkOrientationMarkerWidget;
 class vtkAppendPolyData;
 class vtkRenderWindow;
-class vtkRenderWindowInteractor;
 class vtkTransform;
 class vtkInteractorStyle;
 class vtkLODActor;
@@ -68,6 +69,7 @@ class vtkProp;
 class vtkActor;
 class vtkDataSet;
 class vtkUnstructuredGrid;
+class vtkCellArray;
 
 namespace pcl
 {
@@ -76,6 +78,11 @@ namespace pcl
 
   namespace visualization
   {
+    namespace details
+    {
+      PCL_EXPORTS vtkIdType fillCells(std::vector<int>& lookup, const std::vector<pcl::Vertices>& vertices, vtkSmartPointer<vtkCellArray> cell_array, int max_size_of_polygon);
+    }
+
     /** \brief PCL Visualizer main class.
       * \author Radu B. Rusu
       * \ingroup visualization
@@ -1687,6 +1694,20 @@ namespace pcl
         addCube (float x_min, float x_max, float y_min, float y_max, float z_min, float z_max,
                  double r = 1.0, double g = 1.0, double b = 1.0, const std::string &id = "cube", int viewport = 0);
 
+        /** \brief Add an ellipsoid from the given parameters
+          * \param[in] transform a transformation to apply to the ellipsoid from 0,0,0
+          * \param[in] radius_x the ellipsoid's radius along its local x-axis
+          * \param[in] radius_y the ellipsoid's radius along its local y-axis
+          * \param[in] radius_z the ellipsoid's radius along its local z-axis
+          * \param[in] id the ellipsoid id/name (default: "ellipsoid")
+          * \param[in] viewport (optional) the id of the new viewport (default: 0)
+          */
+        bool
+        addEllipsoid (const Eigen::Isometry3d &transform,
+                      double radius_x, double radius_y, double radius_z,
+                      const std::string &id = "ellipsoid",
+                      int viewport = 0);
+
         /** \brief Changes the visual representation for all actors to surface representation. */
         void
         setRepresentationToSurfaceForAllActors ();
@@ -1809,7 +1830,7 @@ namespace pcl
           * \param[in] view_z the z component of the view point of the camera
           * \param[in] up_x the x component of the view up direction of the camera
           * \param[in] up_y the y component of the view up direction of the camera
-          * \param[in] up_z the y component of the view up direction of the camera
+          * \param[in] up_z the z component of the view up direction of the camera
           * \param[in] viewport the viewport to modify camera of (0 modifies all cameras)
           */
         void
@@ -2170,7 +2191,7 @@ namespace pcl
                                         vtkSmartPointer<vtkPolyData> &polydata,
                                         vtkSmartPointer<vtkIdTypeArray> &initcells);
 
-        /** \brief Converts a PCL templated PointCloud object to a vtk polydata object.
+        /** \brief Converts a PCL object to a vtk polydata object.
           * \param[in] geometry_handler the geometry handler object used to extract the XYZ data
           * \param[out] polydata the resultant polydata containing the cloud
           * \param[out] initcells a list of cell indices used for the conversion. This can be set once and then passed
diff --git a/visualization/include/pcl/visualization/point_cloud_color_handlers.h b/visualization/include/pcl/visualization/point_cloud_color_handlers.h
index 62bb8ce1..9cbc9af8 100644
--- a/visualization/include/pcl/visualization/point_cloud_color_handlers.h
+++ b/visualization/include/pcl/visualization/point_cloud_color_handlers.h
@@ -44,7 +44,7 @@
 // PCL includes
 #include <pcl/pcl_macros.h>
 #include <pcl/point_cloud.h>
-#include <pcl/common/io.h>
+#include <pcl/PCLPointCloud2.h> // for PCLPointCloud2
 #include <pcl/visualization/common/common.h>
 // VTK includes
 #include <vtkSmartPointer.h>
@@ -98,28 +98,11 @@ namespace pcl
         getFieldName () const = 0;
 
         /** Obtain the actual color for the input dataset as a VTK data array.
-          * Deriving handlers should override this method. 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.
+          * Deriving handlers should override this method.
           * \return smart pointer to VTK array if the operation was successful (the
           * handler is capable and the input cloud was given), a null pointer otherwise */
         virtual vtkSmartPointer<vtkDataArray>
-        getColor () const {
-          vtkSmartPointer<vtkDataArray> scalars;
-          getColor (scalars);
-          return scalars;
-        }
-
-        /** Obtain the actual color for the input dataset as a VTK data array.
-          * 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(1, 12, "use getColor() without parameters instead")
-        virtual bool
-        getColor (vtkSmartPointer<vtkDataArray> &scalars) const {
-          scalars = getColor ();
-          return scalars.Get() != nullptr;
-        }
+        getColor () const = 0;
 
         /** \brief Set the input cloud to be used.
           * \param[in] cloud the input cloud to be used by the handler
@@ -187,8 +170,6 @@ namespace pcl
         vtkSmartPointer<vtkDataArray>
         getColor () const override;
 
-        using PointCloudColorHandler<PointT>::getColor;
-
       protected:
         // Members derived from the base class
         using PointCloudColorHandler<PointT>::cloud_;
@@ -247,8 +228,6 @@ namespace pcl
         vtkSmartPointer<vtkDataArray>
         getColor () const override;
 
-        using PointCloudColorHandler<PointT>::getColor;
-
       protected:
         // Members derived from the base class
         using PointCloudColorHandler<PointT>::cloud_;
@@ -298,8 +277,6 @@ namespace pcl
         vtkSmartPointer<vtkDataArray>
         getColor () const override;
 
-        using PointCloudColorHandler<PointT>::getColor;
-
         /** \brief Set the input cloud to be used.
           * \param[in] cloud the input cloud to be used by the handler
           */
@@ -348,8 +325,6 @@ namespace pcl
         vtkSmartPointer<vtkDataArray>
         getColor () const override;
 
-        using PointCloudColorHandler<PointT>::getColor;
-
       protected:
         /** \brief Class getName method. */
         virtual std::string
@@ -410,8 +385,6 @@ namespace pcl
         vtkSmartPointer<vtkDataArray>
         getColor () const override;
 
-        using PointCloudColorHandler<PointT>::getColor;
-
         /** \brief Set the input cloud to be used.
           * \param[in] cloud the input cloud to be used by the handler
           */
@@ -474,8 +447,6 @@ namespace pcl
         vtkSmartPointer<vtkDataArray>
         getColor () const override;
 
-        using PointCloudColorHandler<PointT>::getColor;
-
         /** \brief Set the input cloud to be used.
           * \param[in] cloud the input cloud to be used by the handler
           */
@@ -606,22 +577,7 @@ namespace pcl
           * \return smart pointer to VTK array if the operation was successful (the
           * handler is capable and the input cloud was given), a null pointer otherwise */
         virtual vtkSmartPointer<vtkDataArray>
-        getColor () const {
-          vtkSmartPointer<vtkDataArray> scalars;
-          getColor (scalars);
-          return scalars;
-        }
-
-        /** Obtain the actual color for the input dataset as a VTK data array.
-          * 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(1, 12, "use getColor() without parameters instead")
-        virtual bool
-        getColor (vtkSmartPointer<vtkDataArray> &scalars) const {
-          scalars = getColor ();
-          return scalars.Get() != nullptr;
-        }
+          getColor() const = 0;
 
         /** \brief Set the input cloud to be used.
           * \param[in] cloud the input cloud to be used by the handler
@@ -681,8 +637,6 @@ namespace pcl
 
         vtkSmartPointer<vtkDataArray>
         getColor () const override;
-
-        using PointCloudColorHandler<pcl::PCLPointCloud2>::getColor;
     };
 
     //////////////////////////////////////////////////////////////////////////////////////
@@ -722,8 +676,6 @@ namespace pcl
         vtkSmartPointer<vtkDataArray>
         getColor () const override;
 
-        using PointCloudColorHandler<pcl::PCLPointCloud2>::getColor;
-
       protected:
         /** \brief Internal R, G, B holding the values given by the user. */
         double r_, g_, b_;
@@ -755,8 +707,6 @@ namespace pcl
         vtkSmartPointer<vtkDataArray>
         getColor () const override;
 
-        using PointCloudColorHandler<pcl::PCLPointCloud2>::getColor;
-
       protected:
         /** \brief Get the name of the class. */
         virtual std::string
@@ -792,8 +742,6 @@ namespace pcl
         vtkSmartPointer<vtkDataArray>
         getColor () const override;
 
-        using PointCloudColorHandler<pcl::PCLPointCloud2>::getColor;
-
       protected:
         /** \brief Get the name of the class. */
         virtual std::string
@@ -837,8 +785,6 @@ namespace pcl
         vtkSmartPointer<vtkDataArray>
         getColor () const override;
 
-        using PointCloudColorHandler<pcl::PCLPointCloud2>::getColor;
-
       protected:
         /** \brief Get the name of the class. */
         virtual std::string
@@ -879,8 +825,6 @@ namespace pcl
         vtkSmartPointer<vtkDataArray>
         getColor () const override;
 
-        using PointCloudColorHandler<pcl::PCLPointCloud2>::getColor;
-
       protected:
         /** \brief Get the name of the class. */
         virtual std::string
@@ -919,8 +863,6 @@ namespace pcl
         vtkSmartPointer<vtkDataArray>
         getColor () const override;
 
-        using PointCloudColorHandler<pcl::PCLPointCloud2>::getColor;
-
       protected:
         /** \brief Get the name of the class. */
         virtual std::string
diff --git a/visualization/include/pcl/visualization/point_cloud_geometry_handlers.h b/visualization/include/pcl/visualization/point_cloud_geometry_handlers.h
index efd8ea93..d173b6c0 100644
--- a/visualization/include/pcl/visualization/point_cloud_geometry_handlers.h
+++ b/visualization/include/pcl/visualization/point_cloud_geometry_handlers.h
@@ -42,6 +42,7 @@
 #endif
 
 // PCL includes
+#include <pcl/pcl_base.h> // for UNAVAILABLE
 #include <pcl/point_cloud.h>
 #include <pcl/common/io.h>
 // VTK includes
diff --git a/visualization/include/pcl/visualization/point_picking_event.h b/visualization/include/pcl/visualization/point_picking_event.h
index 38eee9e2..0d598050 100644
--- a/visualization/include/pcl/visualization/point_picking_event.h
+++ b/visualization/include/pcl/visualization/point_picking_event.h
@@ -39,7 +39,7 @@
 #pragma once
 
 #include <pcl/pcl_macros.h>
-#include <vector>
+#include <pcl/types.h> // for pcl::Indices
 
 #include <vtkCommand.h>
 class vtkRenderWindowInteractor;
@@ -71,7 +71,7 @@ namespace pcl
         performSinglePick (vtkRenderWindowInteractor *iren, float &x, float &y, float &z);
 
         int
-        performAreaPick (vtkRenderWindowInteractor *iren, std::vector<int> &indices) const;
+        performAreaPick (vtkRenderWindowInteractor *iren, pcl::Indices &indices) const;
 
       private:
         float x_, y_, z_;
diff --git a/visualization/include/pcl/visualization/qvtk_compatibility.h b/visualization/include/pcl/visualization/qvtk_compatibility.h
new file mode 100644
index 00000000..b6ac0e72
--- /dev/null
+++ b/visualization/include/pcl/visualization/qvtk_compatibility.h
@@ -0,0 +1,51 @@
+/*
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2020-, Open Perception
+ *
+ *  All rights reserved
+ */
+#include <pcl/pcl_macros.h>
+#include <pcl/pcl_config.h>
+
+#if HAVE_QVTK
+#include <vtkVersion.h>
+#include <vtkRenderWindow.h>
+
+#if VTK_MAJOR_VERSION > 8
+  #include <QVTKOpenGLNativeWidget.h>
+  using PCLQVTKWidget = QVTKOpenGLNativeWidget;
+#else 
+  #include <QVTKWidget.h>
+  using PCLQVTKWidget = QVTKWidget;
+#endif // VTK_MAJOR_VERSION > 8
+
+
+inline auto PCL_EXPORTS getInteractorCompat(PCLQVTKWidget& qvtk) {
+#if VTK_MAJOR_VERSION > 8
+  return qvtk.interactor();
+#else
+  return qvtk.GetInteractor();
+#endif // VTK_MAJOR_VERSION > 8
+}
+
+inline auto PCL_EXPORTS getRenderWindowCompat(PCLQVTKWidget& qvtk) {
+#if VTK_MAJOR_VERSION > 8
+  return qvtk.renderWindow();
+#else
+  return qvtk.GetRenderWindow();
+#endif // VTK_MAJOR_VERSION > 8
+}
+
+inline auto PCL_EXPORTS setRenderWindowCompat(PCLQVTKWidget& qvtk, vtkRenderWindow& window) {
+#if VTK_MAJOR_VERSION > 8
+  return qvtk.setRenderWindow(&window);
+#else
+  return qvtk.SetRenderWindow(&window);
+#endif // VTK_MAJOR_VERSION > 8
+}
+
+#else
+#error PCL is not compiled with QVTK.
+#endif
diff --git a/visualization/include/pcl/visualization/registration_visualizer.h b/visualization/include/pcl/visualization/registration_visualizer.h
index dd1ccb8b..418be69e 100644
--- a/visualization/include/pcl/visualization/registration_visualizer.h
+++ b/visualization/include/pcl/visualization/registration_visualizer.h
@@ -83,8 +83,8 @@ namespace pcl
 
         // Create the local callback function and bind it to the local function responsible for updating
         // the local buffers
-        update_visualizer_ = [this] (const pcl::PointCloud<PointSource>& cloud_src, const std::vector<int>& indices_src,
-                                     const pcl::PointCloud<PointTarget>& cloud_tgt, const std::vector<int>& indices_tgt)
+        update_visualizer_ = [this] (const pcl::PointCloud<PointSource>& cloud_src, const pcl::Indices& indices_src,
+                                     const pcl::PointCloud<PointTarget>& cloud_tgt, const pcl::Indices& indices_tgt)
         {
           updateIntermediateCloud (cloud_src, indices_src, cloud_tgt, indices_tgt);
         };
@@ -121,8 +121,8 @@ namespace pcl
        * \param indices_tgt represents the indices of the target points used for the estimation of rigid transformation
        */
       void
-      updateIntermediateCloud (const pcl::PointCloud<PointSource> &cloud_src, const std::vector<int> &indices_src,
-                               const pcl::PointCloud<PointTarget> &cloud_tgt, const std::vector<int> &indices_tgt);
+      updateIntermediateCloud (const pcl::PointCloud<PointSource> &cloud_src, const pcl::Indices &indices_src,
+                               const pcl::PointCloud<PointTarget> &cloud_tgt, const pcl::Indices &indices_tgt);
 
       /** \brief Set maximum number of correspondence lines which will be rendered. */
       inline void
@@ -157,10 +157,7 @@ namespace pcl
       inline std::string
       getIndexedName (std::string &root_name, std::size_t &id)
       {
-        std::stringstream id_stream_;
-        id_stream_ << id;
-        std::string indexed_name_ = root_name + id_stream_.str ();
-        return indexed_name_;
+        return root_name + std::to_string(id);
       }
 
       /** \brief The registration viewer. */
@@ -174,8 +171,8 @@ namespace pcl
 
       /** \brief Callback function linked to pcl::Registration::update_visualizer_ */
       std::function<void
-      (const pcl::PointCloud<PointSource> &cloud_src, const std::vector<int> &indices_src, const pcl::PointCloud<
-          PointTarget> &cloud_tgt, const std::vector<int> &indices_tgt)> update_visualizer_;
+      (const pcl::PointCloud<PointSource> &cloud_src, const pcl::Indices &indices_src, const pcl::PointCloud<
+          PointTarget> &cloud_tgt, const pcl::Indices &indices_tgt)> update_visualizer_;
 
       /** \brief Updates source and target point clouds only for the first update call. */
       bool first_update_flag_;
@@ -193,10 +190,10 @@ namespace pcl
       pcl::PointCloud<PointSource> cloud_intermediate_;
 
       /** \brief The indices of intermediate points used for computation of rigid transformation. */
-      std::vector<int> cloud_intermediate_indices_;
+      pcl::Indices cloud_intermediate_indices_;
 
       /** \brief The indices of target points used for computation of rigid transformation. */
-      std::vector<int> cloud_target_indices_;
+      pcl::Indices cloud_target_indices_;
 
       /** \brief The maximum number of displayed correspondences. */
       std::size_t maximum_displayed_correspondences_;
diff --git a/visualization/include/pcl/visualization/simple_buffer_visualizer.h b/visualization/include/pcl/visualization/simple_buffer_visualizer.h
index f4c143a6..f11ab723 100644
--- a/visualization/include/pcl/visualization/simple_buffer_visualizer.h
+++ b/visualization/include/pcl/visualization/simple_buffer_visualizer.h
@@ -160,7 +160,7 @@ namespace pcl
         void
         initValuesAndVisualization ()
         {
-          cloud_.points.resize(1);
+          cloud_.resize(1);
           
           PCL_WARN("Setting buffer size to %d entries.\n", nb_values_);
           values_.resize(nb_values_);
diff --git a/visualization/include/pcl/visualization/vtk.h b/visualization/include/pcl/visualization/vtk.h
index 9f6df75a..aa944316 100644
--- a/visualization/include/pcl/visualization/vtk.h
+++ b/visualization/include/pcl/visualization/vtk.h
@@ -1,172 +1,10 @@
 /*
- * Software License Agreement (BSD License)
+ * SPDX-License-Identifier: BSD-3-Clause
  *
  *  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
- *     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$
+ *  Copyright (c) 2020-, Open Perception
  *
+ *  All rights reserved
  */
 
-#pragma once
-
-#if defined __GNUC__
-#pragma GCC system_header
-#ifdef __DEPRECATED
-#undef __DEPRECATED
-#define __DEPRECATED_DISABLED__
-#endif
-#endif
-
-#include <vtkVersion.h>
-#include <vtkAppendPolyData.h>
-#include <vtkAssemblyPath.h>
-#include <vtkAxesActor.h>
-#include <vtkActor.h>
-#include <vtkBoxRepresentation.h>
-#include <vtkBoxWidget.h>
-#include <vtkBoxWidget2.h>
-#include <vtkCellData.h>
-#include <vtkMath.h>
-#include <vtkLoopSubdivisionFilter.h>
-#include <vtkLineSource.h>
-#include <vtkLegendScaleActor.h>
-#include <vtkLightKit.h>
-#include <vtkPlatonicSolidSource.h>
-#include <vtkPropPicker.h>
-#include <vtkGeneralTransform.h>
-#include <vtkSmartPointer.h>
-#include <vtkDataSet.h>
-#include <vtkDataSetSurfaceFilter.h>
-#include <vtkExecutive.h>
-#include <vtkPolygon.h>
-#include <vtkPointPicker.h>
-#include <vtkUnstructuredGrid.h>
-#include <vtkConeSource.h>
-#include <vtkDiskSource.h>
-#include <vtkPlaneSource.h>
-#include <vtkSphereSource.h>
-#include <vtkIdentityTransform.h>
-#include <vtkTransform.h>
-#include <vtkTransformPolyDataFilter.h>
-#include <vtkTubeFilter.h>
-#include <vtkCubeSource.h>
-#include <vtkAxes.h>
-#include <vtkFloatArray.h>
-#include <vtkPointData.h>
-#include <vtkPolyData.h>
-#include <vtkPolyDataReader.h>
-#include <vtkPolyDataMapper.h>
-#include <vtkDataSetMapper.h>
-#include <vtkCellArray.h>
-#include <vtkCommand.h>
-#include <vtkCellLocator.h>
-#include <vtkPLYReader.h>
-#include <vtkTransformFilter.h>
-#include <vtkPolyLine.h>
-#include <vtkVectorText.h>
-#include <vtkFollower.h>
-#include <vtkCallbackCommand.h>
-#include <vtkInteractorStyle.h>
-#include <vtkInformationVector.h>
-#include <vtkDataArray.h>
-#include <vtkUnsignedCharArray.h>
-#include <vtkPoints.h>
-#include <vtkRendererCollection.h>
-#include <vtkPNGWriter.h>
-#include <vtkWindowToImageFilter.h>
-#include <vtkInteractorStyleTrackballCamera.h>
-#include <vtkProperty.h>
-#include <vtkCamera.h>
-#include <vtkObjectFactory.h>
-#include <vtkScalarBarActor.h>
-#include <vtkScalarsToColors.h>
-#include <vtkClipPolyData.h>
-#include <vtkPlanes.h>
-#include <vtkImageImport.h>
-#include <vtkImageViewer.h>
-#include <vtkInteractorStyleImage.h>
-#include <vtkImageFlip.h>
-#include <vtkTIFFWriter.h>
-#include <vtkBMPWriter.h>
-#include <vtkJPEGWriter.h>
-#include <vtkImageViewer2.h>
-#include <vtkRenderWindow.h>
-#include <vtkXYPlotActor.h>
-#include <vtkTextProperty.h>
-#include <vtkProperty2D.h>
-#include <vtkFieldData.h>
-#include <vtkDoubleArray.h>
-#include <vtkLODActor.h>
-#include <vtkPolyDataWriter.h>
-#include <vtkTextActor.h>
-#include <vtkCleanPolyData.h>
-#include <vtkRenderer.h>
-#include <vtkObject.h>
-#include <vtkOrientationMarkerWidget.h>
-#include <vtkImageReslice.h>
-#include <vtkImageChangeInformation.h>
-#include <vtkImageCanvasSource2D.h>
-#include <vtkImageBlend.h>
-#include <vtkImageStencilData.h>
-#include <vtkRenderWindowInteractor.h>
-#include <vtkChartXY.h>
-#include <vtkPlot.h>
-#include <vtkTable.h>
-#include <vtkContextView.h>
-#include <vtkContextScene.h>
-#include <vtkColorSeries.h>
-#include <vtkAxis.h>
-#include <vtkSelection.h>
-
-#include <vtkHardwareSelector.h>
-
-#include <vtkTriangle.h>
-#include <vtkWorldPointPicker.h>
-
-#include <vtkInteractorStyleRubberBandPick.h>
-#include <vtkInteractorStyleTrackballActor.h>
-#include <vtkAreaPicker.h>
-#include <vtkExtractGeometry.h>
-#include <vtkExtractPolyDataGeometry.h>
-#include <vtkVertexGlyphFilter.h>
-#include <vtkIdFilter.h>
-#include <vtkIdTypeArray.h>
-#include <vtkImageReader2Factory.h>
-#include <vtkImageReader2.h>
-#include <vtkImageData.h>
-
-#if defined __GNUC__ && defined __DEPRECATED_DISABLED__
-#define __DEPRECATED
-#undef __DEPRECATED_DISABLED__
-#endif
+PCL_DEPRECATED_HEADER(1, 14, "Use required vtk includes instead.")
diff --git a/visualization/include/pcl/visualization/vtk/pcl_vtk_compatibility.h b/visualization/include/pcl/visualization/vtk/pcl_vtk_compatibility.h
new file mode 100644
index 00000000..6eb6598e
--- /dev/null
+++ b/visualization/include/pcl/visualization/vtk/pcl_vtk_compatibility.h
@@ -0,0 +1,17 @@
+/*
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2020-, Open Perception
+ *
+ *  All rights reserved
+ */
+
+#include <vtkCellArray.h>
+
+#ifdef VTK_CELL_ARRAY_V2
+  using vtkCellPtsPtr = vtkIdType const*;
+#else
+  using vtkCellPtsPtr = vtkIdType*;
+#endif
+
diff --git a/visualization/include/pcl/visualization/vtk/vtkVertexBufferObject.h b/visualization/include/pcl/visualization/vtk/vtkVertexBufferObject.h
index 895f2f9d..ee0239cc 100644
--- a/visualization/include/pcl/visualization/vtk/vtkVertexBufferObject.h
+++ b/visualization/include/pcl/visualization/vtk/vtkVertexBufferObject.h
@@ -26,8 +26,6 @@
 
 #pragma once
 
-#include <vector>
-
 #include "vtkObject.h"
 #include "vtkWeakPointer.h"
 
diff --git a/visualization/src/cloud_viewer.cpp b/visualization/src/cloud_viewer.cpp
index 79e3506e..002134f6 100644
--- a/visualization/src/cloud_viewer.cpp
+++ b/visualization/src/cloud_viewer.cpp
@@ -37,9 +37,11 @@
 
 #include <pcl/visualization/cloud_viewer.h>
 #include <pcl/visualization/pcl_visualizer.h>
-#include <pcl/visualization/boost.h>
 #include <pcl/memory.h>
 
+#include <vtkOrientationMarkerWidget.h>
+#include <vtkRenderWindowInteractor.h>
+
 #include <mutex>
 #include <thread>
 
diff --git a/visualization/src/common/common.cpp b/visualization/src/common/common.cpp
index 6ff42a64..9dd3777e 100644
--- a/visualization/src/common/common.cpp
+++ b/visualization/src/common/common.cpp
@@ -38,6 +38,8 @@
 #include <vtkCamera.h>
 #include <vtkRenderWindow.h>
 
+#include <Eigen/Geometry> // for cross
+
 #include <pcl/point_types.h>
 #include <pcl/visualization/common/common.h>
 #include <pcl/console/print.h>
diff --git a/visualization/src/common/io.cpp b/visualization/src/common/io.cpp
index 4b043778..5ec44eb3 100644
--- a/visualization/src/common/io.cpp
+++ b/visualization/src/common/io.cpp
@@ -44,20 +44,19 @@
 #include <pcl/visualization/common/io.h>
 #include <pcl/kdtree/kdtree_flann.h>
 #include <pcl/io/pcd_io.h>
-#include <pcl/visualization/eigen.h>
 #include <pcl/memory.h>
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
 pcl::visualization::getCorrespondingPointCloud (vtkPolyData *src, 
                                                 const pcl::PointCloud<pcl::PointXYZ> &tgt, 
-                                                std::vector<int> &indices)
+                                                pcl::Indices &indices)
 {
   // Iterate through the points and copy the data in a pcl::PointCloud
   pcl::PointCloud<pcl::PointXYZ> cloud;
   cloud.height = 1; cloud.width = static_cast<std::uint32_t> (src->GetNumberOfPoints ());
   cloud.is_dense = false;
-  cloud.points.resize (cloud.width * cloud.height);
+  cloud.resize (cloud.width * cloud.height);
   for (vtkIdType i = 0; i < src->GetNumberOfPoints (); i++)
   {
     double p[3];
@@ -71,7 +70,7 @@ pcl::visualization::getCorrespondingPointCloud (vtkPolyData *src,
   pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
   kdtree.setInputCloud (make_shared<PointCloud<PointXYZ>> (tgt));
 
-  std::vector<int> nn_indices (1);
+  pcl::Indices nn_indices (1);
   std::vector<float> nn_dists (1);
   // For each point on screen, find its correspondent in the target
   for (const auto &point : cloud.points)
@@ -132,16 +131,15 @@ pcl::visualization::savePointData (vtkPolyData* data, const std::string &out_fil
     pcl::PointCloud<pcl::PointXYZ> cloud_xyz;
     pcl::fromPCLPointCloud2 (cloud, cloud_xyz);
     // Get the corresponding indices that we need to save from this point cloud
-    std::vector<int> indices;
+    pcl::Indices indices;
     getCorrespondingPointCloud (cleaner->GetOutput (), cloud_xyz, indices);
 
     // Copy the indices and save the file
     pcl::PCLPointCloud2 cloud_out;
     pcl::copyPointCloud (cloud, indices, cloud_out);
-    std::stringstream ss;
-    ss << out_file << i++ << ".pcd";
-    pcl::console::print_debug ("  Save: %s ... ", ss.str ().c_str ());
-    if (pcl::io::savePCDFile (ss.str (), cloud_out, Eigen::Vector4f::Zero (),
+    const std::string out_filename = out_file + std::to_string(i++) + ".pcd";
+    pcl::console::print_debug ("  Save: %s ... ", out_filename.c_str ());
+    if (pcl::io::savePCDFile (out_filename, cloud_out, Eigen::Vector4f::Zero (),
                               Eigen::Quaternionf::Identity (), true) == -1)
     {
       pcl::console::print_error (stdout, "[failed]\n");
diff --git a/visualization/src/common/shapes.cpp b/visualization/src/common/shapes.cpp
index 445db9f5..8893ea96 100644
--- a/visualization/src/common/shapes.cpp
+++ b/visualization/src/common/shapes.cpp
@@ -47,6 +47,8 @@
 #include <vtkDiskSource.h>
 #include <vtkPlaneSource.h>
 #include <vtkCubeSource.h>
+#include <vtkParametricEllipsoid.h>
+#include <vtkParametricFunctionSource.h>
 
 ////////////////////////////////////////////////////////////////////////////////////////////
 vtkSmartPointer<vtkDataSet> 
@@ -304,6 +306,31 @@ pcl::visualization::createLine (const Eigen::Vector4f &pt1, const Eigen::Vector4
 
   return (line->GetOutput ());
 }
+//////////////////////////////////////////////////////////////////////////////////////////////
+vtkSmartPointer<vtkDataSet>
+pcl::visualization::createEllipsoid (const Eigen::Isometry3d &transform,
+                                     double radius_x, double radius_y, double radius_z)
+{
+  const vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
+  const Eigen::Matrix4d trMatrix = transform.matrix ().transpose (); // Eigen is col-major while vtk is row-major, so transpose it.
+  t->SetMatrix (trMatrix.data ());
+
+  vtkSmartPointer<vtkParametricEllipsoid> ellipsoid = vtkSmartPointer<vtkParametricEllipsoid>::New ();
+  ellipsoid->SetXRadius (radius_x);
+  ellipsoid->SetYRadius (radius_y);
+  ellipsoid->SetZRadius (radius_z);
+
+  vtkSmartPointer<vtkParametricFunctionSource> s_ellipsoid = vtkSmartPointer<vtkParametricFunctionSource>::New ();
+  s_ellipsoid->SetParametricFunction (ellipsoid);
+
+  vtkSmartPointer<vtkTransformPolyDataFilter> tf = vtkSmartPointer<vtkTransformPolyDataFilter>::New ();
+  tf->SetTransform (t);
+  tf->SetInputConnection (s_ellipsoid->GetOutputPort ());
+  tf->Update ();
+
+  return (tf->GetOutput ());
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
 pcl::visualization::allocVtkUnstructuredGrid (vtkSmartPointer<vtkUnstructuredGrid> &polydata)
diff --git a/visualization/src/histogram_visualizer.cpp b/visualization/src/histogram_visualizer.cpp
index cdbbb294..9d7b7ef7 100644
--- a/visualization/src/histogram_visualizer.cpp
+++ b/visualization/src/histogram_visualizer.cpp
@@ -42,10 +42,8 @@
 #include <pcl/visualization/common/common.h>
 #include <vtkRenderWindowInteractor.h>
 #include <pcl/visualization/histogram_visualizer.h>
-#include <pcl/visualization/boost.h>
 
 #include <vtkVersion.h>
-#include <vtkXYPlotActor.h>
 #include <vtkDoubleArray.h>
 #include <vtkTextProperty.h>
 #include <vtkRenderWindow.h>
@@ -287,7 +285,7 @@ pcl::visualization::PCLHistogramVisualizer::addFeatureHistogram (
   int field_idx = pcl::getFieldIndex (cloud, field_name);
   if (field_idx == -1)
   {
-    PCL_ERROR ("[addFeatureHistogram] Invalid field (%s) given!", field_name.c_str ());
+    PCL_ERROR ("[addFeatureHistogram] Invalid field (%s) given!\n", field_name.c_str ());
     return (false);
   }
 
@@ -304,7 +302,7 @@ pcl::visualization::PCLHistogramVisualizer::addFeatureHistogram (
 
   // Parse the cloud data and store it in the array
   double xy[2];
-  for (unsigned int d = 0; d < cloud.fields[field_idx].count; ++d)
+  for (uindex_t d = 0; d < cloud.fields[field_idx].count; ++d)
   {
     xy[0] = d;
     float data;
@@ -326,10 +324,10 @@ bool
 pcl::visualization::PCLHistogramVisualizer::addFeatureHistogram (
     const pcl::PCLPointCloud2 &cloud,
     const std::string &field_name, 
-    const int index,
+    const pcl::index_t index,
     const std::string &id, int win_width, int win_height)
 {
-  if (index < 0 || index >= static_cast<int> (cloud.width * cloud.height))
+  if (index < 0 || index >= static_cast<pcl::index_t> (cloud.width * cloud.height))
   {
     PCL_ERROR ("[addFeatureHistogram] Invalid point index (%d) given!\n", index);
     return (false);
@@ -361,7 +359,7 @@ pcl::visualization::PCLHistogramVisualizer::addFeatureHistogram (
 
   // Parse the cloud data and store it in the array
   double xy[2];
-  for (unsigned int d = 0; d < cloud.fields[field_idx].count; ++d)
+  for (uindex_t d = 0; d < cloud.fields[field_idx].count; ++d)
   {
     xy[0] = d;
     float data;
@@ -405,7 +403,7 @@ pcl::visualization::PCLHistogramVisualizer::updateFeatureHistogram (
 
   // Parse the cloud data and store it in the array
   double xy[2];
-  for (unsigned int d = 0; d < cloud.fields[field_idx].count; ++d)
+  for (uindex_t d = 0; d < cloud.fields[field_idx].count; ++d)
   {
     xy[0] = d;
     float data;
@@ -422,10 +420,10 @@ bool
 pcl::visualization::PCLHistogramVisualizer::updateFeatureHistogram (
     const pcl::PCLPointCloud2 &cloud,
     const std::string &field_name, 
-    const int index,
+    const pcl::index_t index,
     const std::string &id)
 {
-  if (index < 0 || index >= static_cast<int> (cloud.width * cloud.height))
+  if (index < 0 || index >= static_cast<pcl::index_t> (cloud.width * cloud.height))
   {
     PCL_ERROR ("[updateFeatureHistogram] Invalid point index (%d) given!\n", index);
     return (false);
@@ -456,7 +454,7 @@ pcl::visualization::PCLHistogramVisualizer::updateFeatureHistogram (
 
   // Parse the cloud data and store it in the array
   double xy[2];
-  for (unsigned int d = 0; d < cloud.fields[field_idx].count; ++d)
+  for (uindex_t d = 0; d < cloud.fields[field_idx].count; ++d)
   {
     xy[0] = d;
     float data;
diff --git a/visualization/src/interactor_style.cpp b/visualization/src/interactor_style.cpp
index 4740577c..db17786b 100644
--- a/visualization/src/interactor_style.cpp
+++ b/visualization/src/interactor_style.cpp
@@ -36,6 +36,7 @@
  *
  */
 
+#include <fstream>
 #include <list>
 #include <pcl/common/angles.h>
 #include <pcl/visualization/common/io.h>
@@ -65,6 +66,10 @@
 #include <vtkPointPicker.h>
 #include <vtkAreaPicker.h>
 
+#include <boost/algorithm/string/classification.hpp> // for is_any_of
+#include <boost/algorithm/string/split.hpp> // for split
+#include <boost/filesystem.hpp> // for exists
+
 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
 #include <pcl/visualization/vtk/vtkVertexBufferObjectMapper.h>
 #endif
@@ -137,7 +142,7 @@ pcl::visualization::PCLVisualizerInteractorStyle::saveCameraParameters (const st
 {
   FindPokedRenderer (Interactor->GetEventPosition ()[0], Interactor->GetEventPosition ()[1]);
 
-  ofstream ofs_cam (file.c_str ());
+  std::ofstream ofs_cam (file.c_str ());
   if (!ofs_cam.is_open ())
   {
     return (false);
@@ -1063,7 +1068,7 @@ pcl::visualization::PCLVisualizerInteractorStyle::OnKeyDown ()
         if(CurrentRenderer)
           CurrentRenderer->ResetCamera ();
         else
-          PCL_WARN ("no current renderer on the interactor style.");
+          PCL_WARN ("no current renderer on the interactor style.\n");
 
         CurrentRenderer->Render ();
         break;
diff --git a/visualization/src/pcl_plotter.cpp b/visualization/src/pcl_plotter.cpp
index 7274d206..e9c0fe95 100644
--- a/visualization/src/pcl_plotter.cpp
+++ b/visualization/src/pcl_plotter.cpp
@@ -303,7 +303,7 @@ pcl::visualization::PCLPlotter::addFeatureHistogram (
   int field_idx = pcl::getFieldIndex (cloud, field_name);
   if (field_idx == -1)
   {
-    PCL_ERROR ("[addFeatureHistogram] Invalid field (%s) given!", field_name.c_str ());
+    PCL_ERROR ("[addFeatureHistogram] Invalid field (%s) given!\n", field_name.c_str ());
     return (false);
   }
 
@@ -330,10 +330,10 @@ bool
 pcl::visualization::PCLPlotter::addFeatureHistogram (
     const pcl::PCLPointCloud2 &cloud,
     const std::string &field_name, 
-    const int index,
+    const pcl::index_t index,
     const std::string &id, int win_width, int win_height)
 {
-  if (index < 0 || index >= static_cast<int> (cloud.width * cloud.height))
+  if (index < 0 || index >= static_cast<pcl::index_t> (cloud.width * cloud.height))
   {
     PCL_ERROR ("[addFeatureHistogram] Invalid point index (%d) given!\n", index);
     return (false);
@@ -343,7 +343,7 @@ pcl::visualization::PCLPlotter::addFeatureHistogram (
   int field_idx = pcl::getFieldIndex (cloud, field_name);
   if (field_idx == -1)
   {
-    PCL_ERROR ("[addFeatureHistogram] Invalid field (%s) given!", field_name.c_str ());
+    PCL_ERROR ("[addFeatureHistogram] Invalid field (%s) given!\n", field_name.c_str ());
     return (false);
   }
 
diff --git a/visualization/src/pcl_visualizer.cpp b/visualization/src/pcl_visualizer.cpp
index 8d2c7d7e..370d5ed2 100644
--- a/visualization/src/pcl_visualizer.cpp
+++ b/visualization/src/pcl_visualizer.cpp
@@ -51,15 +51,15 @@
 #include <vtkPolyDataNormals.h>
 #include <vtkMapper.h>
 #include <vtkDataSetMapper.h>
-
+#include <vtkCellArray.h>
 #include <vtkHardwareSelector.h>
 #include <vtkSelectionNode.h>
 
 #include <vtkSelection.h>
 #include <vtkPointPicker.h>
 
-#include <pcl/visualization/boost.h>
 #include <pcl/visualization/vtk/vtkRenderWindowInteractorFix.h>
+#include <pcl/visualization/vtk/pcl_vtk_compatibility.h>
 
 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
 #include <pcl/visualization/vtk/vtkVertexBufferObjectMapper.h>
@@ -82,7 +82,6 @@
 #include <vtkAxesActor.h>
 #include <vtkRenderWindowInteractor.h>
 #include <vtkAreaPicker.h>
-#include <vtkXYPlotActor.h>
 #include <vtkOpenGLRenderWindow.h>
 #include <vtkJPEGReader.h>
 #include <vtkBMPReader.h>
@@ -100,12 +99,14 @@
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/visualization/common/common.h>
 #include <pcl/common/time.h>
+#include <boost/version.hpp> // for BOOST_VERSION
 #if (BOOST_VERSION >= 106600)
 #include <boost/uuid/detail/sha1.hpp>
 #else
 #include <boost/uuid/sha1.hpp>
 #endif
 #include <boost/filesystem.hpp>
+#include <boost/algorithm/string.hpp> // for split
 #include <pcl/common/utils.h> // pcl::utils::ignore
 #include <pcl/console/parse.h>
 
@@ -122,6 +123,63 @@
   #undef far
 #endif
 
+vtkIdType
+pcl::visualization::details::fillCells(std::vector<int>& lookup, const std::vector<pcl::Vertices>& vertices, vtkSmartPointer<vtkCellArray> cell_array, int max_size_of_polygon)
+{
+#ifdef VTK_CELL_ARRAY_V2
+  pcl::utils::ignore(max_size_of_polygon);
+
+  if (!lookup.empty())
+  {
+    for (const auto& verti : vertices)
+    {
+      std::size_t n_points = verti.vertices.size();
+      cell_array->InsertNextCell(n_points);
+      for (const auto& vertj : verti.vertices)
+        cell_array->InsertCellPoint(lookup[vertj]);
+    }
+  }
+  else
+  {
+    for (const auto& verti : vertices)
+    {
+      std::size_t n_points = verti.vertices.size();
+      cell_array->InsertNextCell(n_points);
+      for (const auto& vertj : verti.vertices)
+        cell_array->InsertCellPoint(vertj);
+    }
+  }
+#else
+  vtkIdType* cell = cell_array->WritePointer(vertices.size(), vertices.size() * (max_size_of_polygon + 1));
+
+  if (!lookup.empty())
+  {
+    for (const auto& verti : vertices)
+    {
+      std::size_t n_points = verti.vertices.size();
+      *cell++ = n_points;
+      for (const auto& vertj : verti.vertices)
+        *cell++ = lookup[vertj];
+    }
+  }
+  else
+  {
+    for (const auto& verti : vertices)
+    {
+      std::size_t n_points = verti.vertices.size();
+      *cell++ = n_points;
+      for (const auto& vertj : verti.vertices)
+        *cell++ = vertj;
+    }
+  }
+#endif
+
+  const auto idx = vertices.size() + std::accumulate(vertices.begin(), vertices.end(), static_cast<vtkIdType>(0),
+    [](const auto& sum, const auto& vertex) { return sum + vertex.vertices.size(); });
+
+  return idx;
+}
+
 /////////////////////////////////////////////////////////////////////////////////////////////
 pcl::visualization::PCLVisualizer::PCLVisualizer (const std::string &name, const bool create_interactor)
   : update_fps_ (vtkSmartPointer<FPSCallback>::New ())
@@ -335,7 +393,7 @@ pcl::visualization::PCLVisualizer::setupInteractor (
 void pcl::visualization::PCLVisualizer::setupRenderer (vtkSmartPointer<vtkRenderer> ren)
 {
   if (!ren)
-    PCL_ERROR ("Passed pointer to renderer is null");
+    PCL_ERROR ("Passed pointer to renderer is null\n");
 
   ren->AddObserver (vtkCommand::EndEvent, update_fps_);
   // Add it to the list of renderers
@@ -346,7 +404,7 @@ void pcl::visualization::PCLVisualizer::setupRenderer (vtkSmartPointer<vtkRender
 void pcl::visualization::PCLVisualizer::setupFPSCallback (const vtkSmartPointer<vtkRenderer>& ren)
 {
   if (!ren)
-    PCL_ERROR ("Passed pointer to renderer is null");
+    PCL_ERROR ("Passed pointer to renderer is null\n");
   // FPS callback
   vtkSmartPointer<vtkTextActor> txt = vtkSmartPointer<vtkTextActor>::New ();
   update_fps_->actor = txt;
@@ -360,7 +418,7 @@ void pcl::visualization::PCLVisualizer::setupFPSCallback (const vtkSmartPointer<
 void pcl::visualization::PCLVisualizer::setupRenderWindow (const std::string& name)
 {
   if (!win_)
-    PCL_ERROR ("Pointer to render window is null");
+    PCL_ERROR ("Pointer to render window is null\n");
 
   // This is temporary measures for disable display of deprecated warnings
   #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
@@ -383,7 +441,7 @@ void pcl::visualization::PCLVisualizer::setupRenderWindow (const std::string& na
 void pcl::visualization::PCLVisualizer::setupStyle ()
 {
   if (!style_)
-    PCL_ERROR ("Pointer to style is null");
+    PCL_ERROR ("Pointer to style is null\n");
 
   // Set rend erer window in case no interactor is created
   style_->setRenderWindow (win_);
@@ -401,7 +459,7 @@ void pcl::visualization::PCLVisualizer::setupStyle ()
 void pcl::visualization::PCLVisualizer::setDefaultWindowSizeAndPos ()
 {
   if (!win_)
-    PCL_ERROR ("Pointer to render window is null");
+    PCL_ERROR ("Pointer to render window is null\n");
   int scr_size_x = win_->GetScreenSize ()[0];
   int scr_size_y = win_->GetScreenSize ()[1];
   win_->SetSize (scr_size_x / 2, scr_size_y / 2);
@@ -521,13 +579,14 @@ void
 pcl::visualization::PCLVisualizer::spinOnce (int time, bool force_redraw)
 {
   resetStoppedFlag ();
-  #if (defined (__APPLE__))
-    if (!win_->IsDrawable ())
-    {
-      close ();
-      return;
-    }
-  #endif
+
+#if VTK_MAJOR_VERSION < 9 && defined (__APPLE__)
+  if (!win_->IsDrawable ())
+  {
+    close ();
+    return;
+  }
+#endif
 
   if (!interactor_)
     return;
@@ -855,7 +914,7 @@ pcl::visualization::PCLVisualizer::removeText3D (const std::string &id, int view
   // If there is no custom viewport and the viewport number is not 0, exit
   if (rens_->GetNumberOfItems () <= viewport)
   {
-    PCL_ERROR ("[removeText3D] The viewport [%d] doesn't exist (id <%s>)! ",
+    PCL_ERROR ("[removeText3D] The viewport [%d] doesn't exist (id <%s>)!\n",
                viewport,
                id.c_str ());
     return false;
@@ -1262,7 +1321,6 @@ pcl::visualization::PCLVisualizer::createActorFromVTKDataSet (const vtkSmartPoin
   //actor->SetNumberOfCloudPoints (std::max<vtkIdType> (1, data->GetNumberOfPoints () / 10));
   actor->GetProperty ()->SetInterpolationToFlat ();
 }
-
 /////////////////////////////////////////////////////////////////////////////////////////////
 void
 pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData (
@@ -1291,10 +1349,36 @@ pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData (
   if (!vertices)
     vertices = vtkSmartPointer<vtkCellArray>::New ();
 
+#ifdef VTK_CELL_ARRAY_V2
+  // TODO: Remove when VTK 6,7,8 is unsupported
+  pcl::utils::ignore(initcells);
+
+  auto numOfCells = vertices->GetNumberOfCells();
+
+  // If we have less cells than points, add new cells.
+  if (numOfCells < nr_points)
+  {
+    for (int i = numOfCells; i < nr_points; i++)
+    {
+      vertices->InsertNextCell(1);
+      vertices->InsertCellPoint(i);
+    }
+  }
+  // if we too many cells than points, set size (doesn't free excessive memory)
+  else if (numOfCells > nr_points)
+  {
+    vertices->ResizeExact(nr_points, nr_points);
+  }
+
+  polydata->SetPoints(points);
+  polydata->SetVerts(vertices);
+
+#else
   vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
   updateCells (cells, initcells, nr_points);
   // Set the cells and the vertices
   vertices->SetCells (nr_points, cells);
+#endif
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
@@ -1926,16 +2010,6 @@ pcl::visualization::PCLVisualizer::getCameraFile () const
   return (style_->getCameraFile ());
 }
 
-/////////////////////////////////////////////////////////////////////////////////////////////
-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 ()
-{
-  rens_->InitTraversal ();
-  // Update the camera parameters
-  win_->Render ();
-}
-
 /////////////////////////////////////////////////////////////////////////////////////////////
 bool
 pcl::visualization::PCLVisualizer::updateShapePose (const std::string &id, const Eigen::Affine3f& pose)
@@ -2362,6 +2436,34 @@ pcl::visualization::PCLVisualizer::addCube (float x_min, float x_max,
   return (true);
 }
 
+////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::visualization::PCLVisualizer::addEllipsoid (
+  const Eigen::Isometry3d &transform,
+  double radius_x, double radius_y, double radius_z,
+  const std::string &id, int viewport)
+{
+  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+  ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+  if (am_it != shape_actor_map_->end ())
+  {
+    pcl::console::print_warn (stderr, "[addEllipsoid] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
+    return (false);
+  }
+
+  vtkSmartPointer<vtkDataSet> data = createEllipsoid (transform, radius_x, radius_y, radius_z);
+
+  // Create an Actor
+  vtkSmartPointer<vtkLODActor> actor;
+  createActorFromVTKDataSet (data, actor);
+  actor->GetProperty ()->SetRepresentationToSurface ();
+  addActorToRenderer (actor, viewport);
+
+  // Save the pointer/ID pair to the global actor map
+  (*shape_actor_map_)[id] = actor;
+  return (true);
+}
+
 ////////////////////////////////////////////////////////////////////////////////////////////
 bool
 pcl::visualization::PCLVisualizer::addSphere (const pcl::ModelCoefficients &coefficients,
@@ -3160,28 +3262,9 @@ pcl::visualization::PCLVisualizer::updatePolygonMesh (
 
   // Update the cells
   cells = vtkSmartPointer<vtkCellArray>::New ();
-  vtkIdType *cell = cells->WritePointer (verts.size (), verts.size () * (max_size_of_polygon + 1));
-  int idx = 0;
-  if (!lookup.empty ())
-  {
-    for (std::size_t i = 0; i < verts.size (); ++i, ++idx)
-    {
-      std::size_t n_points = verts[i].vertices.size ();
-      *cell++ = n_points;
-      for (std::size_t j = 0; j < n_points; j++, cell++, ++idx)
-        *cell = lookup[verts[i].vertices[j]];
-    }
-  }
-  else
-  {
-    for (std::size_t i = 0; i < verts.size (); ++i, ++idx)
-    {
-      std::size_t n_points = verts[i].vertices.size ();
-      *cell++ = n_points;
-      for (std::size_t j = 0; j < n_points; j++, cell++, ++idx)
-        *cell = verts[i].vertices[j];
-    }
-  }
+  
+  const auto idx = details::fillCells(lookup, verts, cells, max_size_of_polygon);
+
   cells->GetData ()->SetNumberOfValues (idx);
   cells->Squeeze ();
   // Set the the vertices
@@ -3318,7 +3401,7 @@ pcl::visualization::PCLVisualizer::addTextureMesh (const pcl::TextureMesh &mesh,
   {
     pcl::PointCloud<pcl::PointXYZRGB> cloud;
     pcl::fromPCLPointCloud2 (mesh.cloud, cloud);
-    if (cloud.points.empty ())
+    if (cloud.empty ())
     {
       PCL_ERROR ("[PCLVisualizer::addTextureMesh] Cloud is empty!\n");
       return (false);
@@ -3410,8 +3493,7 @@ pcl::visualization::PCLVisualizer::addTextureMesh (const pcl::TextureMesh &mesh,
     // add a texture coordinates array per texture
     vtkSmartPointer<vtkFloatArray> coordinates = vtkSmartPointer<vtkFloatArray>::New ();
     coordinates->SetNumberOfComponents (2);
-    std::stringstream ss; ss << "TCoords" << tex_id;
-    std::string this_coordinates_name = ss.str ();
+    const std::string this_coordinates_name = "TCoords" + std::to_string(tex_id);
     coordinates->SetName (this_coordinates_name.c_str ());
 
     for (std::size_t t = 0; t < mesh.tex_coordinates.size (); ++t)
@@ -3552,7 +3634,8 @@ pcl::visualization::PCLVisualizer::renderViewTesselatedSphere (
 
   //center object
   double CoM[3];
-  vtkIdType npts_com = 0, *ptIds_com = nullptr;
+  vtkIdType npts_com = 0;
+  vtkCellPtsPtr ptIds_com = nullptr;
   vtkSmartPointer<vtkCellArray> cells_com = polydata->GetPolys ();
 
   double center[3], p1_com[3], p2_com[3], p3_com[3], totalArea_com = 0;
@@ -3611,10 +3694,11 @@ pcl::visualization::PCLVisualizer::renderViewTesselatedSphere (
   // * Compute area of the mesh
   //////////////////////////////
   vtkSmartPointer<vtkCellArray> cells = mapper->GetInput ()->GetPolys ();
-  vtkIdType npts = 0, *ptIds = nullptr;
+  vtkIdType npts = 0;
+  vtkCellPtsPtr ptIds = nullptr;
 
   double p1[3], p2[3], p3[3], totalArea = 0;
-  for (cells->InitTraversal (); cells->GetNextCell (npts, ptIds);)
+  for (cells->InitTraversal (); cells->GetNextCell(npts, ptIds);)
   {
     polydata->GetPoint (ptIds[0], p1);
     polydata->GetPoint (ptIds[1], p2);
@@ -3830,7 +3914,8 @@ pcl::visualization::PCLVisualizer::renderViewTesselatedSphere (
     polydata->BuildCells ();
 
     vtkSmartPointer<vtkCellArray> cells = polydata->GetPolys ();
-    vtkIdType npts = 0, *ptIds = nullptr;
+    vtkIdType npts = 0;
+    vtkCellPtsPtr ptIds = nullptr;
 
     double p1[3], p2[3], p3[3], area, totalArea = 0;
     for (cells->InitTraversal (); cells->GetNextCell (npts, ptIds);)
@@ -4040,9 +4125,9 @@ pcl::visualization::PCLVisualizer::fromHandlersToScreen (
 
   vtkSmartPointer<vtkPolyData> polydata;
   vtkSmartPointer<vtkIdTypeArray> initcells;
+
   // Convert the PointCloud to VTK PolyData
   convertPointCloudToVTKPolyData (geometry_handler, polydata, initcells);
-  // use the given geometry handler
 
   // Get the colors from the handler
   bool has_colors = false;
diff --git a/visualization/src/point_cloud_handlers.cpp b/visualization/src/point_cloud_handlers.cpp
index 026266b9..fa8c1df7 100644
--- a/visualization/src/point_cloud_handlers.cpp
+++ b/visualization/src/point_cloud_handlers.cpp
@@ -691,7 +691,7 @@ pcl::visualization::PointCloudGeometryHandler<pcl::PCLPointCloud2>::getGeometry
   
   if (!data->Resize(nr_points))
   {
-    PCL_ERROR("[point_cloud_handlers::getGeometry] Failed to allocate space for points in VTK array.");
+    PCL_ERROR("[point_cloud_handlers::getGeometry] Failed to allocate space for points in VTK array.\n");
     throw std::bad_alloc();
   }
     
diff --git a/visualization/src/point_picking_event.cpp b/visualization/src/point_picking_event.cpp
index e9348cc6..c0f342ed 100644
--- a/visualization/src/point_picking_event.cpp
+++ b/visualization/src/point_picking_event.cpp
@@ -49,7 +49,6 @@
 #include <vtkPointData.h>
 #include <vtkVertexGlyphFilter.h>
 #include <vtkPlanes.h>
-#include <vtkXYPlotActor.h>
 #include <vtkRenderer.h>
 #include <vtkRenderWindow.h>
 
@@ -102,7 +101,7 @@ pcl::visualization::PointPickingCallback::Execute (vtkObject *caller, unsigned l
     else if (eventid == vtkCommand::LeftButtonReleaseEvent)
     {
       style->OnLeftButtonUp ();
-      std::vector<int> indices;
+      pcl::Indices indices;
       int nb_points = performAreaPick (iren, indices);
       AreaPickingEvent event (nb_points, indices);
       style->area_picking_signal_ (event);
@@ -167,7 +166,7 @@ pcl::visualization::PointPickingCallback::performSinglePick (
 /////////////////////////////////////////////////////////////////////////////////////////////
 int
 pcl::visualization::PointPickingCallback::performAreaPick (vtkRenderWindowInteractor *iren,
-                                                           std::vector<int> &indices) const
+                                                           pcl::Indices &indices) const
 {
   vtkAreaPicker *picker = static_cast<vtkAreaPicker*> (iren->GetPicker ());
   vtkRenderer *ren = iren->FindPokedRenderer (iren->GetEventPosition ()[0], iren->GetEventPosition ()[1]);
@@ -207,7 +206,7 @@ pcl::visualization::PointPickingCallback::performAreaPick (vtkRenderWindowIntera
     indices.reserve (ids->GetNumberOfTuples ());
 
     for(vtkIdType i = 0; i < ids->GetNumberOfTuples (); i++)
-      indices.push_back (static_cast<int> (ids->GetValue (i)));
+      indices.push_back (static_cast<index_t> (ids->GetValue (i)));
 
     return (static_cast<int> (selected->GetNumberOfPoints ()));
   }