├── data ├── 251370668.pcd ├── 251371071.pcd └── screenshot.png ├── src └── pclomp │ ├── voxel_grid_covariance_omp.cpp │ ├── ndt_omp.cpp │ └── gicp_omp.cpp ├── docker ├── foxy │ └── Dockerfile ├── galactic │ └── Dockerfile ├── noetic │ └── Dockerfile ├── melodic │ └── Dockerfile ├── foxy_llvm │ └── Dockerfile ├── galactic_llvm │ └── Dockerfile ├── noetic_llvm │ └── Dockerfile └── melodic_llvm │ └── Dockerfile ├── .github └── workflows │ └── build.yml ├── package.xml ├── LICENSE ├── README.md ├── CMakeLists.txt ├── .clang-format ├── apps └── align.cpp └── include └── pclomp ├── gicp_omp.h ├── voxel_grid_covariance_omp_impl.hpp ├── voxel_grid_covariance_omp.h ├── gicp_omp_impl.hpp ├── ndt_omp.h └── ndt_omp_impl.hpp /data/251370668.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/ndt_omp/HEAD/data/251370668.pcd -------------------------------------------------------------------------------- /data/251371071.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/ndt_omp/HEAD/data/251371071.pcd -------------------------------------------------------------------------------- /data/screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/ndt_omp/HEAD/data/screenshot.png -------------------------------------------------------------------------------- /src/pclomp/voxel_grid_covariance_omp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | template class pclomp::VoxelGridCovariance; 5 | template class pclomp::VoxelGridCovariance; 6 | template class pclomp::VoxelGridCovariance; -------------------------------------------------------------------------------- /src/pclomp/ndt_omp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | template class pclomp::NormalDistributionsTransform; 5 | template class pclomp::NormalDistributionsTransform; 6 | template class pclomp::NormalDistributionsTransform; -------------------------------------------------------------------------------- /src/pclomp/gicp_omp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | template class pclomp::GeneralizedIterativeClosestPoint; 5 | template class pclomp::GeneralizedIterativeClosestPoint; 6 | template class pclomp::GeneralizedIterativeClosestPoint; -------------------------------------------------------------------------------- /docker/foxy/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:foxy 2 | 3 | RUN apt-get update && apt-get install -y --no-install-recommends \ 4 | wget nano build-essential libomp-dev clang lld git\ 5 | ros-foxy-geodesy ros-foxy-pcl-ros ros-foxy-nmea-msgs \ 6 | ros-foxy-rviz2 ros-foxy-libg2o \ 7 | && apt-get clean \ 8 | && rm -rf /var/lib/apt/lists/* 9 | 10 | RUN mkdir -p /root/colcon_ws/src 11 | WORKDIR /root/colcon_ws/src 12 | # Comment it out until the repository supports ROS2 builds. 13 | # RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive --depth=1 14 | # RUN git clone https://github.com/koide3/hdl_graph_slam.git --depth=1 15 | 16 | COPY . /root/colcon_ws/src/ndt_omp/ 17 | 18 | WORKDIR /root/colcon_ws 19 | RUN /bin/bash -c '. /opt/ros/foxy/setup.bash; colcon build' 20 | RUN sed -i "6i source \"/root/colcon_ws/devel/setup.bash\"" /ros_entrypoint.sh 21 | 22 | WORKDIR / 23 | 24 | ENTRYPOINT ["/ros_entrypoint.sh"] 25 | CMD ["bash"] 26 | -------------------------------------------------------------------------------- /.github/workflows/build.yml: -------------------------------------------------------------------------------- 1 | name: Build 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | pull_request: 7 | 8 | # Allows you to run this workflow manually from the Actions tab 9 | workflow_dispatch: 10 | 11 | jobs: 12 | build: 13 | runs-on: ubuntu-latest 14 | strategy: 15 | matrix: 16 | ROS_DISTRO: [melodic, melodic_llvm, noetic, noetic_llvm, foxy, foxy_llvm, galactic, galactic_llvm] 17 | 18 | steps: 19 | - uses: actions/checkout@v2 20 | 21 | - name: Docker login 22 | continue-on-error: true 23 | uses: docker/login-action@v1 24 | with: 25 | username: ${{ secrets.DOCKER_USERNAME }} 26 | password: ${{ secrets.DOCKER_TOKEN }} 27 | 28 | - name: Docker build 29 | uses: docker/build-push-action@v2 30 | with: 31 | file: ${{github.workspace}}/docker/${{ matrix.ROS_DISTRO }}/Dockerfile 32 | context: . 33 | push: false 34 | -------------------------------------------------------------------------------- /docker/galactic/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:galactic 2 | 3 | RUN apt-get update && apt-get install -y --no-install-recommends \ 4 | wget nano build-essential libomp-dev clang lld git\ 5 | ros-galactic-geodesy ros-galactic-pcl-ros ros-galactic-nmea-msgs \ 6 | ros-galactic-rviz2 ros-galactic-libg2o \ 7 | && apt-get clean \ 8 | && rm -rf /var/lib/apt/lists/* 9 | 10 | RUN mkdir -p /root/colcon_ws/src 11 | WORKDIR /root/colcon_ws/src 12 | # Comment it out until the repository supports ROS2 builds. 13 | # RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive --depth=1 14 | # RUN git clone https://github.com/koide3/hdl_graph_slam.git --depth=1 15 | 16 | COPY . /root/colcon_ws/src/ndt_omp/ 17 | 18 | WORKDIR /root/colcon_ws 19 | RUN /bin/bash -c '. /opt/ros/galactic/setup.bash; colcon build' 20 | RUN sed -i "6i source \"/root/colcon_ws/devel/setup.bash\"" /ros_entrypoint.sh 21 | 22 | WORKDIR / 23 | 24 | ENTRYPOINT ["/ros_entrypoint.sh"] 25 | CMD ["bash"] 26 | -------------------------------------------------------------------------------- /docker/noetic/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:noetic 2 | 3 | RUN apt-get update && apt-get install -y --no-install-recommends \ 4 | wget nano build-essential libomp-dev clang lld git\ 5 | ros-noetic-geodesy ros-noetic-pcl-ros ros-noetic-nmea-msgs \ 6 | ros-noetic-rviz ros-noetic-tf-conversions ros-noetic-libg2o \ 7 | && apt-get clean \ 8 | && rm -rf /var/lib/apt/lists/* 9 | 10 | RUN mkdir -p /root/catkin_ws/src 11 | WORKDIR /root/catkin_ws/src 12 | RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_init_workspace' 13 | RUN git config --global http.postBuffer 2M 14 | RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive --depth=1 15 | RUN git clone https://github.com/koide3/hdl_graph_slam.git --depth=1 16 | 17 | COPY . /root/catkin_ws/src/ndt_omp/ 18 | 19 | WORKDIR /root/catkin_ws 20 | RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_make' 21 | RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh 22 | 23 | WORKDIR / 24 | 25 | ENTRYPOINT ["/ros_entrypoint.sh"] 26 | CMD ["bash"] 27 | -------------------------------------------------------------------------------- /docker/melodic/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:melodic 2 | 3 | RUN apt-get update && apt-get install -y --no-install-recommends \ 4 | wget nano build-essential libomp-dev clang lld\ 5 | ros-melodic-geodesy ros-melodic-pcl-ros ros-melodic-nmea-msgs \ 6 | ros-melodic-rviz ros-melodic-tf-conversions ros-melodic-libg2o \ 7 | && apt-get clean \ 8 | && rm -rf /var/lib/apt/lists/* 9 | 10 | RUN mkdir -p /root/catkin_ws/src 11 | WORKDIR /root/catkin_ws/src 12 | RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; catkin_init_workspace' 13 | RUN git config --global http.postBuffer 2M 14 | RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive --depth=1 15 | RUN git clone https://github.com/koide3/hdl_graph_slam.git --depth=1 16 | 17 | COPY . /root/catkin_ws/src/ndt_omp/ 18 | 19 | WORKDIR /root/catkin_ws 20 | RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; catkin_make' 21 | RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh 22 | 23 | WORKDIR / 24 | 25 | ENTRYPOINT ["/ros_entrypoint.sh"] 26 | CMD ["bash"] 27 | -------------------------------------------------------------------------------- /docker/foxy_llvm/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:foxy 2 | 3 | RUN apt-get update && apt-get install -y --no-install-recommends \ 4 | wget nano build-essential libomp-dev clang lld git\ 5 | ros-foxy-geodesy ros-foxy-pcl-ros ros-foxy-nmea-msgs \ 6 | ros-foxy-rviz2 ros-foxy-libg2o \ 7 | && apt-get clean \ 8 | && rm -rf /var/lib/apt/lists/* 9 | 10 | RUN mkdir -p /root/colcon_ws/src 11 | WORKDIR /root/colcon_ws/src 12 | # Comment it out until the repository supports ROS2 builds. 13 | # RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive --depth=1 14 | # RUN git clone https://github.com/koide3/hdl_graph_slam.git --depth=1 15 | 16 | COPY . /root/colcon_ws/src/ndt_omp/ 17 | 18 | RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld 50 19 | 20 | WORKDIR /root/colcon_ws 21 | RUN /bin/bash -c '. /opt/ros/foxy/setup.bash; CC=clang CXX=clang++ colcon build' 22 | RUN sed -i "6i source \"/root/colcon_ws/devel/setup.bash\"" /ros_entrypoint.sh 23 | 24 | WORKDIR / 25 | 26 | ENTRYPOINT ["/ros_entrypoint.sh"] 27 | CMD ["bash"] 28 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ndt_omp 5 | 0.0.0 6 | OpenMP boosted NDT and GICP algorithms 7 | 8 | koide 9 | 10 | BSD 11 | 12 | catkin 13 | ament_cmake_auto 14 | ros_environment 15 | 16 | pcl_ros 17 | roscpp 18 | 19 | libpcl-all-dev 20 | 21 | 22 | catkin 23 | ament_cmake 24 | 25 | 26 | -------------------------------------------------------------------------------- /docker/galactic_llvm/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:galactic 2 | 3 | RUN apt-get update && apt-get install -y --no-install-recommends \ 4 | wget nano build-essential libomp-dev clang lld git\ 5 | ros-galactic-geodesy ros-galactic-pcl-ros ros-galactic-nmea-msgs \ 6 | ros-galactic-rviz2 ros-galactic-libg2o \ 7 | && apt-get clean \ 8 | && rm -rf /var/lib/apt/lists/* 9 | 10 | RUN mkdir -p /root/colcon_ws/src 11 | WORKDIR /root/colcon_ws/src 12 | # Comment it out until the repository supports ROS2 builds. 13 | # RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive --depth=1 14 | # RUN git clone https://github.com/koide3/hdl_graph_slam.git --depth=1 15 | 16 | COPY . /root/colcon_ws/src/ndt_omp/ 17 | 18 | RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld 50 19 | 20 | WORKDIR /root/colcon_ws 21 | RUN /bin/bash -c '. /opt/ros/galactic/setup.bash; CC=clang CXX=clang++ colcon build' 22 | RUN sed -i "6i source \"/root/colcon_ws/devel/setup.bash\"" /ros_entrypoint.sh 23 | 24 | WORKDIR / 25 | 26 | ENTRYPOINT ["/ros_entrypoint.sh"] 27 | CMD ["bash"] 28 | -------------------------------------------------------------------------------- /docker/noetic_llvm/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:noetic 2 | 3 | RUN apt-get update && apt-get install -y --no-install-recommends \ 4 | wget nano build-essential libomp-dev clang lld git\ 5 | ros-noetic-geodesy ros-noetic-pcl-ros ros-noetic-nmea-msgs \ 6 | ros-noetic-rviz ros-noetic-tf-conversions ros-noetic-libg2o \ 7 | && apt-get clean \ 8 | && rm -rf /var/lib/apt/lists/* 9 | 10 | RUN mkdir -p /root/catkin_ws/src 11 | WORKDIR /root/catkin_ws/src 12 | RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_init_workspace' 13 | RUN git config --global http.postBuffer 2M 14 | RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive --depth=1 15 | RUN git clone https://github.com/koide3/hdl_graph_slam.git --depth=1 16 | 17 | COPY . /root/catkin_ws/src/ndt_omp/ 18 | 19 | RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld 50 20 | 21 | WORKDIR /root/catkin_ws 22 | RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; CC=clang CXX=clang++ catkin_make' 23 | RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh 24 | 25 | WORKDIR / 26 | 27 | ENTRYPOINT ["/ros_entrypoint.sh"] 28 | CMD ["bash"] 29 | -------------------------------------------------------------------------------- /docker/melodic_llvm/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:melodic 2 | 3 | RUN apt-get update && apt-get install -y --no-install-recommends \ 4 | wget nano build-essential libomp-dev clang lld\ 5 | ros-melodic-geodesy ros-melodic-pcl-ros ros-melodic-nmea-msgs \ 6 | ros-melodic-rviz ros-melodic-tf-conversions ros-melodic-libg2o \ 7 | && apt-get clean \ 8 | && rm -rf /var/lib/apt/lists/* 9 | 10 | RUN mkdir -p /root/catkin_ws/src 11 | WORKDIR /root/catkin_ws/src 12 | RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; catkin_init_workspace' 13 | RUN git config --global http.postBuffer 2M 14 | RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive --depth=1 15 | RUN git clone https://github.com/koide3/hdl_graph_slam.git --depth=1 16 | 17 | COPY . /root/catkin_ws/src/ndt_omp/ 18 | 19 | RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld 50 20 | 21 | WORKDIR /root/catkin_ws 22 | RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; CC=clang CXX=clang++ catkin_make' 23 | RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh 24 | 25 | WORKDIR / 26 | 27 | ENTRYPOINT ["/ros_entrypoint.sh"] 28 | CMD ["bash"] 29 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2019, k.koide 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ndt_omp 2 | 3 | This package provides an OpenMP-boosted Normal Distributions Transform (and GICP) algorithm derived from pcl. The NDT algorithm is modified to be SSE-friendly and multi-threaded. It can run up to 10 times faster than its original version in pcl. 4 | 5 | For using this package in non-ROS1 projects (ROS2 or without ROS), see forked repositories: [dfki-ric/pclomp](https://github.com/dfki-ric/pclomp) [tier4/ndt_omp](https://github.com/tier4/ndt_omp). 6 | 7 | [![Build](https://github.com/koide3/ndt_omp/actions/workflows/build.yml/badge.svg)](https://github.com/koide3/ndt_omp/actions/workflows/build.yml) 8 | 9 | ### Benchmark (on Core i7-6700K) 10 | ``` 11 | $ roscd ndt_omp/data 12 | $ rosrun ndt_omp align 251370668.pcd 251371071.pcd 13 | --- pcl::NDT --- 14 | single : 282.222[msec] 15 | 10times: 2921.92[msec] 16 | fitness: 0.213937 17 | 18 | --- pclomp::NDT (KDTREE, 1 threads) --- 19 | single : 207.697[msec] 20 | 10times: 2059.19[msec] 21 | fitness: 0.213937 22 | 23 | --- pclomp::NDT (DIRECT7, 1 threads) --- 24 | single : 139.433[msec] 25 | 10times: 1356.79[msec] 26 | fitness: 0.214205 27 | 28 | --- pclomp::NDT (DIRECT1, 1 threads) --- 29 | single : 34.6418[msec] 30 | 10times: 317.03[msec] 31 | fitness: 0.208511 32 | 33 | --- pclomp::NDT (KDTREE, 8 threads) --- 34 | single : 54.9903[msec] 35 | 10times: 500.51[msec] 36 | fitness: 0.213937 37 | 38 | --- pclomp::NDT (DIRECT7, 8 threads) --- 39 | single : 63.1442[msec] 40 | 10times: 343.336[msec] 41 | fitness: 0.214205 42 | 43 | --- pclomp::NDT (DIRECT1, 8 threads) --- 44 | single : 17.2353[msec] 45 | 10times: 100.025[msec] 46 | fitness: 0.208511 47 | ``` 48 | 49 | Several methods for neighbor voxel search are implemented. If you select pclomp::KDTREE, results will be completely the same as that of the original pcl::NDT. We recommend using pclomp::DIRECT7 which is faster and stable. If you need extremely fast registration, choose pclomp::DIRECT1, but it might be a bit unstable. 50 | 51 |
52 | Red: target, Green: source, Blue: aligned 53 | 54 | ## Related packages 55 | - [ndt_omp](https://github.com/koide3/ndt_omp) 56 | - [fast_gicp](https://github.com/SMRT-AIST/fast_gicp) 57 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(ndt_omp) 3 | 4 | add_definitions(-std=c++14) 5 | set(CMAKE_CXX_FLAGS "-std=c++14") 6 | 7 | if (BUILD_WITH_MARCH_NATIVE) 8 | add_compile_options(-march=native) 9 | else() 10 | add_definitions(-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2) 11 | set(CMAKE_CXX_FLAGS "-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2") 12 | endif() 13 | 14 | # pcl 1.7 causes a segfault when it is built with debug mode 15 | set(CMAKE_BUILD_TYPE "RELEASE") 16 | 17 | find_package(PCL 1.7 REQUIRED) 18 | include_directories(${PCL_INCLUDE_DIRS}) 19 | link_directories(${PCL_LIBRARY_DIRS}) 20 | add_definitions(${PCL_DEFINITIONS}) 21 | 22 | message(STATUS "PCL_INCLUDE_DIRS:" ${PCL_INCLUDE_DIRS}) 23 | message(STATUS "PCL_LIBRARY_DIRS:" ${PCL_LIBRARY_DIRS}) 24 | message(STATUS "PCL_DEFINITIONS:" ${PCL_DEFINITIONS}) 25 | 26 | find_package(OpenMP) 27 | if (OPENMP_FOUND) 28 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 29 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 30 | endif() 31 | 32 | if($ENV{ROS_VERSION} EQUAL 1) 33 | # ROS1 34 | find_package(catkin REQUIRED COMPONENTS 35 | roscpp 36 | pcl_ros 37 | ) 38 | 39 | ################################### 40 | ## catkin specific configuration ## 41 | ################################### 42 | catkin_package( 43 | INCLUDE_DIRS include 44 | LIBRARIES ndt_omp 45 | ) 46 | 47 | ########### 48 | ## Build ## 49 | ########### 50 | include_directories(include) 51 | include_directories( 52 | ${catkin_INCLUDE_DIRS} 53 | ) 54 | 55 | add_library(ndt_omp 56 | src/pclomp/voxel_grid_covariance_omp.cpp 57 | src/pclomp/ndt_omp.cpp 58 | src/pclomp/gicp_omp.cpp 59 | ) 60 | 61 | add_executable(align 62 | apps/align.cpp 63 | ) 64 | add_dependencies(align 65 | ndt_omp 66 | ) 67 | target_link_libraries(align 68 | ${catkin_LIBRARIES} 69 | ${PCL_LIBRARIES} 70 | ndt_omp 71 | ) 72 | 73 | ############# 74 | ## INSTALL ## 75 | ############# 76 | 77 | install( 78 | TARGETS 79 | ndt_omp 80 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 81 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 82 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 83 | ) 84 | 85 | # install headers 86 | install(DIRECTORY include/pclomp 87 | DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) 88 | else() 89 | 90 | # ROS2 91 | find_package(ament_cmake_auto REQUIRED) 92 | ament_auto_find_build_dependencies() 93 | 94 | ########### 95 | ## Build ## 96 | ########### 97 | include_directories(include) 98 | include_directories( 99 | ${catkin_INCLUDE_DIRS} 100 | ) 101 | 102 | add_library(ndt_omp 103 | SHARED 104 | src/pclomp/voxel_grid_covariance_omp.cpp 105 | src/pclomp/ndt_omp.cpp 106 | src/pclomp/gicp_omp.cpp 107 | ) 108 | 109 | ament_export_targets(ndt_omp HAS_LIBRARY_TARGET) 110 | 111 | target_link_libraries(ndt_omp ${PCL_LIBRARIES}) 112 | 113 | if(OpenMP_CXX_FOUND) 114 | target_link_libraries(ndt_omp OpenMP::OpenMP_CXX) 115 | else() 116 | message(WARNING "OpenMP not found") 117 | endif() 118 | 119 | install( 120 | TARGETS 121 | ndt_omp 122 | EXPORT ndt_omp 123 | LIBRARY DESTINATION lib 124 | ARCHIVE DESTINATION lib 125 | RUNTIME DESTINATION bin 126 | INCLUDES DESTINATION include 127 | ) 128 | 129 | ament_auto_package() 130 | endif() 131 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: Google 4 | AccessModifierOffset: -2 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveAssignments: false 7 | AlignConsecutiveDeclarations: false 8 | AlignEscapedNewlines: Left 9 | AlignOperands: true 10 | AlignTrailingComments: true 11 | AllowAllParametersOfDeclarationOnNextLine: true 12 | AllowShortBlocksOnASingleLine: false 13 | AllowShortCaseLabelsOnASingleLine: false 14 | AllowShortFunctionsOnASingleLine: Empty 15 | AllowShortIfStatementsOnASingleLine: true 16 | AllowShortLoopsOnASingleLine: true 17 | AlwaysBreakAfterDefinitionReturnType: None 18 | AlwaysBreakAfterReturnType: None 19 | AlwaysBreakBeforeMultilineStrings: true 20 | AlwaysBreakTemplateDeclarations: true 21 | BinPackArguments: true 22 | BinPackParameters: false 23 | BraceWrapping: 24 | AfterClass: false 25 | AfterControlStatement: false 26 | AfterEnum: false 27 | AfterFunction: false 28 | AfterNamespace: false 29 | AfterObjCDeclaration: false 30 | AfterStruct: false 31 | AfterUnion: false 32 | AfterExternBlock: false 33 | BeforeCatch: false 34 | BeforeElse: false 35 | IndentBraces: false 36 | SplitEmptyFunction: true 37 | SplitEmptyRecord: true 38 | SplitEmptyNamespace: true 39 | BreakBeforeBinaryOperators: None 40 | BreakBeforeBraces: Attach 41 | BreakBeforeInheritanceComma: false 42 | BreakBeforeTernaryOperators: true 43 | BreakConstructorInitializersBeforeComma: false 44 | BreakConstructorInitializers: BeforeColon 45 | BreakAfterJavaFieldAnnotations: false 46 | BreakStringLiterals: false 47 | ColumnLimit: 256 48 | CommentPragmas: '^ IWYU pragma:' 49 | CompactNamespaces: false 50 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 51 | ConstructorInitializerIndentWidth: 4 52 | ContinuationIndentWidth: 4 53 | Cpp11BracedListStyle: true 54 | DerivePointerAlignment: true 55 | DisableFormat: false 56 | ExperimentalAutoDetectBinPacking: false 57 | FixNamespaceComments: true 58 | ForEachMacros: 59 | - foreach 60 | - Q_FOREACH 61 | - BOOST_FOREACH 62 | IncludeBlocks: Preserve 63 | IncludeCategories: 64 | - Regex: '^' 65 | Priority: 2 66 | - Regex: '^<.*\.h>' 67 | Priority: 1 68 | - Regex: '^<.*' 69 | Priority: 2 70 | - Regex: '.*' 71 | Priority: 3 72 | IncludeIsMainRegex: '([-_](test|unittest))?$' 73 | IndentCaseLabels: true 74 | IndentPPDirectives: None 75 | IndentWidth: 2 76 | IndentWrappedFunctionNames: false 77 | JavaScriptQuotes: Leave 78 | JavaScriptWrapImports: true 79 | KeepEmptyLinesAtTheStartOfBlocks: false 80 | MacroBlockBegin: '' 81 | MacroBlockEnd: '' 82 | MaxEmptyLinesToKeep: 1 83 | NamespaceIndentation: None 84 | ObjCBlockIndentWidth: 2 85 | ObjCSpaceAfterProperty: false 86 | ObjCSpaceBeforeProtocolList: false 87 | PenaltyBreakAssignment: 2 88 | PenaltyBreakBeforeFirstCallParameter: 1 89 | PenaltyBreakComment: 300 90 | PenaltyBreakFirstLessLess: 120 91 | PenaltyBreakString: 1000 92 | PenaltyExcessCharacter: 0 93 | PenaltyReturnTypeOnItsOwnLine: 200 94 | PointerAlignment: Left 95 | RawStringFormats: 96 | - Language: TextProto 97 | Delimiters: 98 | - pb 99 | BasedOnStyle: google 100 | ReflowComments: true 101 | SortIncludes: false 102 | SortUsingDeclarations: false 103 | SpaceAfterCStyleCast: false 104 | SpaceAfterTemplateKeyword: false 105 | SpaceBeforeAssignmentOperators: true 106 | SpaceBeforeParens: Never 107 | SpaceInEmptyParentheses: false 108 | SpacesBeforeTrailingComments: 2 109 | SpacesInAngles: false 110 | SpacesInContainerLiterals: true 111 | SpacesInCStyleCastParentheses: false 112 | SpacesInParentheses: false 113 | SpacesInSquareBrackets: false 114 | Standard: Auto 115 | TabWidth: 8 116 | UseTab: Never 117 | ... 118 | 119 | -------------------------------------------------------------------------------- /apps/align.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | // align point clouds and measure processing time 15 | pcl::PointCloud::Ptr align(pcl::Registration::Ptr registration, const pcl::PointCloud::Ptr& target_cloud, const pcl::PointCloud::Ptr& source_cloud ) { 16 | registration->setInputTarget(target_cloud); 17 | registration->setInputSource(source_cloud); 18 | pcl::PointCloud::Ptr aligned(new pcl::PointCloud()); 19 | 20 | auto t1 = ros::WallTime::now(); 21 | registration->align(*aligned); 22 | auto t2 = ros::WallTime::now(); 23 | std::cout << "single : " << (t2 - t1).toSec() * 1000 << "[msec]" << std::endl; 24 | 25 | for(int i=0; i<10; i++) { 26 | registration->align(*aligned); 27 | } 28 | auto t3 = ros::WallTime::now(); 29 | std::cout << "10times: " << (t3 - t2).toSec() * 1000 << "[msec]" << std::endl; 30 | std::cout << "fitness: " << registration->getFitnessScore() << std::endl << std::endl; 31 | 32 | return aligned; 33 | } 34 | 35 | 36 | int main(int argc, char** argv) { 37 | if(argc != 3) { 38 | std::cout << "usage: align target.pcd source.pcd" << std::endl; 39 | return 0; 40 | } 41 | 42 | std::string target_pcd = argv[1]; 43 | std::string source_pcd = argv[2]; 44 | 45 | pcl::PointCloud::Ptr target_cloud(new pcl::PointCloud()); 46 | pcl::PointCloud::Ptr source_cloud(new pcl::PointCloud()); 47 | 48 | if(pcl::io::loadPCDFile(target_pcd, *target_cloud)) { 49 | std::cerr << "failed to load " << target_pcd << std::endl; 50 | return 0; 51 | } 52 | if(pcl::io::loadPCDFile(source_pcd, *source_cloud)) { 53 | std::cerr << "failed to load " << source_pcd << std::endl; 54 | return 0; 55 | } 56 | 57 | // downsampling 58 | pcl::PointCloud::Ptr downsampled(new pcl::PointCloud()); 59 | 60 | pcl::VoxelGrid voxelgrid; 61 | voxelgrid.setLeafSize(0.1f, 0.1f, 0.1f); 62 | 63 | voxelgrid.setInputCloud(target_cloud); 64 | voxelgrid.filter(*downsampled); 65 | *target_cloud = *downsampled; 66 | 67 | voxelgrid.setInputCloud(source_cloud); 68 | voxelgrid.filter(*downsampled); 69 | source_cloud = downsampled; 70 | 71 | ros::Time::init(); 72 | 73 | // benchmark 74 | std::cout << "--- pcl::GICP ---" << std::endl; 75 | pcl::GeneralizedIterativeClosestPoint::Ptr gicp(new pcl::GeneralizedIterativeClosestPoint()); 76 | pcl::PointCloud::Ptr aligned = align(gicp, target_cloud, source_cloud); 77 | 78 | std::cout << "--- pclomp::GICP ---" << std::endl; 79 | pclomp::GeneralizedIterativeClosestPoint::Ptr gicp_omp(new pclomp::GeneralizedIterativeClosestPoint()); 80 | aligned = align(gicp_omp, target_cloud, source_cloud); 81 | 82 | 83 | std::cout << "--- pcl::NDT ---" << std::endl; 84 | pcl::NormalDistributionsTransform::Ptr ndt(new pcl::NormalDistributionsTransform()); 85 | ndt->setResolution(1.0); 86 | aligned = align(ndt, target_cloud, source_cloud); 87 | 88 | std::vector num_threads = {1, omp_get_max_threads()}; 89 | std::vector> search_methods = { 90 | {"KDTREE", pclomp::KDTREE}, 91 | {"DIRECT7", pclomp::DIRECT7}, 92 | {"DIRECT1", pclomp::DIRECT1} 93 | }; 94 | 95 | pclomp::NormalDistributionsTransform::Ptr ndt_omp(new pclomp::NormalDistributionsTransform()); 96 | ndt_omp->setResolution(1.0); 97 | 98 | for(int n : num_threads) { 99 | for(const auto& search_method : search_methods) { 100 | std::cout << "--- pclomp::NDT (" << search_method.first << ", " << n << " threads) ---" << std::endl; 101 | ndt_omp->setNumThreads(n); 102 | ndt_omp->setNeighborhoodSearchMethod(search_method.second); 103 | aligned = align(ndt_omp, target_cloud, source_cloud); 104 | } 105 | } 106 | 107 | // visualization 108 | pcl::visualization::PCLVisualizer vis("vis"); 109 | pcl::visualization::PointCloudColorHandlerCustom target_handler(target_cloud, 255.0, 0.0, 0.0); 110 | pcl::visualization::PointCloudColorHandlerCustom source_handler(source_cloud, 0.0, 255.0, 0.0); 111 | pcl::visualization::PointCloudColorHandlerCustom aligned_handler(aligned, 0.0, 0.0, 255.0); 112 | vis.addPointCloud(target_cloud, target_handler, "target"); 113 | vis.addPointCloud(source_cloud, source_handler, "source"); 114 | vis.addPointCloud(aligned, aligned_handler, "aligned"); 115 | vis.spin(); 116 | 117 | return 0; 118 | } 119 | -------------------------------------------------------------------------------- /include/pclomp/gicp_omp.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2010, Willow Garage, Inc. 6 | * Copyright (c) 2012-, Open Perception, Inc. 7 | * 8 | * All rights reserved. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions 12 | * are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * * Neither the name of the copyright holder(s) nor the names of its 21 | * contributors may be used to endorse or promote products derived 22 | * from this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | * POSSIBILITY OF SUCH DAMAGE. 36 | * 37 | * $Id$ 38 | * 39 | */ 40 | 41 | #ifndef PCL_GICP_OMP_H_ 42 | #define PCL_GICP_OMP_H_ 43 | 44 | #include 45 | #include 46 | 47 | namespace pclomp 48 | { 49 | /** \brief GeneralizedIterativeClosestPoint is an ICP variant that implements the 50 | * generalized iterative closest point algorithm as described by Alex Segal et al. in 51 | * http://www.robots.ox.ac.uk/~avsegal/resources/papers/Generalized_ICP.pdf 52 | * The approach is based on using anisotropic cost functions to optimize the alignment 53 | * after closest point assignments have been made. 54 | * The original code uses GSL and ANN while in ours we use an eigen mapped BFGS and 55 | * FLANN. 56 | * \author Nizar Sallem 57 | * \ingroup registration 58 | */ 59 | template 60 | class GeneralizedIterativeClosestPoint : public pcl::IterativeClosestPoint 61 | { 62 | public: 63 | using pcl::IterativeClosestPoint::reg_name_; 64 | using pcl::IterativeClosestPoint::getClassName; 65 | using pcl::IterativeClosestPoint::indices_; 66 | using pcl::IterativeClosestPoint::target_; 67 | using pcl::IterativeClosestPoint::input_; 68 | using pcl::IterativeClosestPoint::tree_; 69 | using pcl::IterativeClosestPoint::tree_reciprocal_; 70 | using pcl::IterativeClosestPoint::nr_iterations_; 71 | using pcl::IterativeClosestPoint::max_iterations_; 72 | using pcl::IterativeClosestPoint::previous_transformation_; 73 | using pcl::IterativeClosestPoint::final_transformation_; 74 | using pcl::IterativeClosestPoint::transformation_; 75 | using pcl::IterativeClosestPoint::transformation_epsilon_; 76 | using pcl::IterativeClosestPoint::converged_; 77 | using pcl::IterativeClosestPoint::corr_dist_threshold_; 78 | using pcl::IterativeClosestPoint::inlier_threshold_; 79 | using pcl::IterativeClosestPoint::min_number_correspondences_; 80 | using pcl::IterativeClosestPoint::update_visualizer_; 81 | 82 | using PointCloudSource = pcl::PointCloud; 83 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 84 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 85 | 86 | using PointCloudTarget = pcl::PointCloud; 87 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 88 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 89 | 90 | using PointIndicesPtr = pcl::PointIndices::Ptr; 91 | using PointIndicesConstPtr = pcl::PointIndices::ConstPtr; 92 | 93 | using InputKdTree = typename pcl::Registration::KdTree; 94 | using InputKdTreePtr = typename pcl::Registration::KdTreePtr; 95 | 96 | using MatricesVector = std::vector >; 97 | 98 | #if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0) 99 | using MatricesVectorPtr = pcl::shared_ptr; 100 | using MatricesVectorConstPtr = pcl::shared_ptr; 101 | 102 | using Ptr = pcl::shared_ptr >; 103 | using ConstPtr = pcl::shared_ptr >; 104 | #else 105 | using MatricesVectorPtr = boost::shared_ptr; 106 | using MatricesVectorConstPtr = boost::shared_ptr; 107 | 108 | using Ptr = boost::shared_ptr >; 109 | using ConstPtr = boost::shared_ptr >; 110 | #endif 111 | 112 | 113 | using Vector6d = Eigen::Matrix; 114 | 115 | /** \brief Empty constructor. */ 116 | GeneralizedIterativeClosestPoint () 117 | : k_correspondences_(20) 118 | , gicp_epsilon_(0.001) 119 | , rotation_epsilon_(2e-3) 120 | , mahalanobis_(0) 121 | , max_inner_iterations_(20) 122 | { 123 | min_number_correspondences_ = 4; 124 | reg_name_ = "GeneralizedIterativeClosestPoint"; 125 | max_iterations_ = 200; 126 | transformation_epsilon_ = 5e-4; 127 | corr_dist_threshold_ = 5.; 128 | rigid_transformation_estimation_ = [this] (const PointCloudSource& cloud_src, 129 | const std::vector& indices_src, 130 | const PointCloudTarget& cloud_tgt, 131 | const std::vector& indices_tgt, 132 | Eigen::Matrix4f& transformation_matrix) 133 | { 134 | estimateRigidTransformationBFGS (cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix); 135 | }; 136 | } 137 | 138 | /** \brief Provide a pointer to the input dataset 139 | * \param cloud the const boost shared pointer to a PointCloud message 140 | */ 141 | inline void 142 | setInputSource (const PointCloudSourceConstPtr &cloud) override 143 | { 144 | 145 | if (cloud->points.empty ()) 146 | { 147 | PCL_ERROR ("[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ()); 148 | return; 149 | } 150 | PointCloudSource input = *cloud; 151 | // Set all the point.data[3] values to 1 to aid the rigid transformation 152 | for (size_t i = 0; i < input.size (); ++i) 153 | input[i].data[3] = 1.0; 154 | 155 | pcl::IterativeClosestPoint::setInputSource (cloud); 156 | input_covariances_.reset (); 157 | } 158 | 159 | /** \brief Provide a pointer to the covariances of the input source (if computed externally!). 160 | * If not set, GeneralizedIterativeClosestPoint will compute the covariances itself. 161 | * Make sure to set the covariances AFTER setting the input source point cloud (setting the input source point cloud will reset the covariances). 162 | * \param[in] target the input point cloud target 163 | */ 164 | inline void 165 | setSourceCovariances (const MatricesVectorPtr& covariances) 166 | { 167 | input_covariances_ = covariances; 168 | } 169 | 170 | /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to) 171 | * \param[in] target the input point cloud target 172 | */ 173 | inline void 174 | setInputTarget (const PointCloudTargetConstPtr &target) override 175 | { 176 | pcl::IterativeClosestPoint::setInputTarget(target); 177 | target_covariances_.reset (); 178 | } 179 | 180 | /** \brief Provide a pointer to the covariances of the input target (if computed externally!). 181 | * If not set, GeneralizedIterativeClosestPoint will compute the covariances itself. 182 | * Make sure to set the covariances AFTER setting the input source point cloud (setting the input source point cloud will reset the covariances). 183 | * \param[in] target the input point cloud target 184 | */ 185 | inline void 186 | setTargetCovariances (const MatricesVectorPtr& covariances) 187 | { 188 | target_covariances_ = covariances; 189 | } 190 | 191 | /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using an iterative 192 | * non-linear Levenberg-Marquardt approach. 193 | * \param[in] cloud_src the source point cloud dataset 194 | * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src 195 | * \param[in] cloud_tgt the target point cloud dataset 196 | * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from \a indices_src 197 | * \param[out] transformation_matrix the resultant transformation matrix 198 | */ 199 | void 200 | estimateRigidTransformationBFGS (const PointCloudSource &cloud_src, 201 | const std::vector &indices_src, 202 | const PointCloudTarget &cloud_tgt, 203 | const std::vector &indices_tgt, 204 | Eigen::Matrix4f &transformation_matrix); 205 | 206 | /** \brief \return Mahalanobis distance matrix for the given point index */ 207 | inline const Eigen::Matrix4f& mahalanobis(size_t index) const 208 | { 209 | assert(index < mahalanobis_.size()); 210 | return mahalanobis_[index]; 211 | } 212 | 213 | /** \brief Computes rotation matrix derivative. 214 | * rotation matrix is obtained from rotation angles x[3], x[4] and x[5] 215 | * \return d/d_rx, d/d_ry and d/d_rz respectively in g[3], g[4] and g[5] 216 | * param x array representing 3D transformation 217 | * param R rotation matrix 218 | * param g gradient vector 219 | */ 220 | void 221 | computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const; 222 | 223 | /** \brief Set the rotation epsilon (maximum allowable difference between two 224 | * consecutive rotations) in order for an optimization to be considered as having 225 | * converged to the final solution. 226 | * \param epsilon the rotation epsilon 227 | */ 228 | inline void 229 | setRotationEpsilon (double epsilon) { rotation_epsilon_ = epsilon; } 230 | 231 | /** \brief Get the rotation epsilon (maximum allowable difference between two 232 | * consecutive rotations) as set by the user. 233 | */ 234 | inline double 235 | getRotationEpsilon () { return (rotation_epsilon_); } 236 | 237 | /** \brief Set the number of neighbors used when selecting a point neighbourhood 238 | * to compute covariances. 239 | * A higher value will bring more accurate covariance matrix but will make 240 | * covariances computation slower. 241 | * \param k the number of neighbors to use when computing covariances 242 | */ 243 | void 244 | setCorrespondenceRandomness (int k) { k_correspondences_ = k; } 245 | 246 | /** \brief Get the number of neighbors used when computing covariances as set by 247 | * the user 248 | */ 249 | int 250 | getCorrespondenceRandomness () { return (k_correspondences_); } 251 | 252 | /** set maximum number of iterations at the optimization step 253 | * \param[in] max maximum number of iterations for the optimizer 254 | */ 255 | void 256 | setMaximumOptimizerIterations (int max) { max_inner_iterations_ = max; } 257 | 258 | ///\return maximum number of iterations at the optimization step 259 | int 260 | getMaximumOptimizerIterations () { return (max_inner_iterations_); } 261 | 262 | protected: 263 | 264 | /** \brief The number of neighbors used for covariances computation. 265 | * default: 20 266 | */ 267 | int k_correspondences_; 268 | 269 | /** \brief The epsilon constant for gicp paper; this is NOT the convergence 270 | * tolerance 271 | * default: 0.001 272 | */ 273 | double gicp_epsilon_; 274 | 275 | /** The epsilon constant for rotation error. (In GICP the transformation epsilon 276 | * is split in rotation part and translation part). 277 | * default: 2e-3 278 | */ 279 | double rotation_epsilon_; 280 | 281 | /** \brief base transformation */ 282 | Eigen::Matrix4f base_transformation_; 283 | 284 | /** \brief Temporary pointer to the source dataset. */ 285 | const PointCloudSource *tmp_src_; 286 | 287 | /** \brief Temporary pointer to the target dataset. */ 288 | const PointCloudTarget *tmp_tgt_; 289 | 290 | /** \brief Temporary pointer to the source dataset indices. */ 291 | const std::vector *tmp_idx_src_; 292 | 293 | /** \brief Temporary pointer to the target dataset indices. */ 294 | const std::vector *tmp_idx_tgt_; 295 | 296 | 297 | /** \brief Input cloud points covariances. */ 298 | MatricesVectorPtr input_covariances_; 299 | 300 | /** \brief Target cloud points covariances. */ 301 | MatricesVectorPtr target_covariances_; 302 | 303 | /** \brief Mahalanobis matrices holder. */ 304 | std::vector mahalanobis_; 305 | 306 | /** \brief maximum number of optimizations */ 307 | int max_inner_iterations_; 308 | 309 | /** \brief compute points covariances matrices according to the K nearest 310 | * neighbors. K is set via setCorrespondenceRandomness() method. 311 | * \param cloud pointer to point cloud 312 | * \param tree KD tree performer for nearest neighbors search 313 | * \param[out] cloud_covariances covariances matrices for each point in the cloud 314 | */ 315 | template 316 | void computeCovariances(typename pcl::PointCloud::ConstPtr cloud, 317 | const typename pcl::search::KdTree::ConstPtr tree, 318 | MatricesVector& cloud_covariances); 319 | 320 | /** \return trace of mat1^t . mat2 321 | * \param mat1 matrix of dimension nxm 322 | * \param mat2 matrix of dimension nxp 323 | */ 324 | inline double 325 | matricesInnerProd(const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2) const 326 | { 327 | double r = 0.; 328 | size_t n = mat1.rows(); 329 | // tr(mat1^t.mat2) 330 | for(size_t i = 0; i < n; i++) 331 | for(size_t j = 0; j < n; j++) 332 | r += mat1 (j, i) * mat2 (i,j); 333 | return r; 334 | } 335 | 336 | /** \brief Rigid transformation computation method with initial guess. 337 | * \param output the transformed input point cloud dataset using the rigid transformation found 338 | * \param guess the initial guess of the transformation to compute 339 | */ 340 | void 341 | computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) override; 342 | 343 | /** \brief Search for the closest nearest neighbor of a given point. 344 | * \param query the point to search a nearest neighbour for 345 | * \param index vector of size 1 to store the index of the nearest neighbour found 346 | * \param distance vector of size 1 to store the distance to nearest neighbour found 347 | */ 348 | inline bool 349 | searchForNeighbors (const PointSource &query, std::vector& index, std::vector& distance) const 350 | { 351 | int k = tree_->nearestKSearch (query, 1, index, distance); 352 | if (k == 0) 353 | return (false); 354 | return (true); 355 | } 356 | 357 | /// \brief compute transformation matrix from transformation matrix 358 | void applyState(Eigen::Matrix4f &t, const Vector6d& x) const; 359 | 360 | /// \brief optimization functor structure 361 | struct OptimizationFunctorWithIndices : public BFGSDummyFunctor 362 | { 363 | OptimizationFunctorWithIndices (const GeneralizedIterativeClosestPoint* gicp) 364 | : BFGSDummyFunctor (), gicp_(gicp) {} 365 | double operator() (const Vector6d& x) override; 366 | void df(const Vector6d &x, Vector6d &df) override; 367 | void fdf(const Vector6d &x, double &f, Vector6d &df) override; 368 | 369 | const GeneralizedIterativeClosestPoint *gicp_; 370 | }; 371 | 372 | std::function &cloud_src, 373 | const std::vector &src_indices, 374 | const pcl::PointCloud &cloud_tgt, 375 | const std::vector &tgt_indices, 376 | Eigen::Matrix4f &transformation_matrix)> rigid_transformation_estimation_; 377 | }; 378 | } 379 | 380 | #endif //#ifndef PCL_GICP_H_ 381 | -------------------------------------------------------------------------------- /include/pclomp/voxel_grid_covariance_omp_impl.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2010-2011, Willow Garage, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the copyright holder(s) nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | */ 37 | 38 | #ifndef PCL_VOXEL_GRID_COVARIANCE_IMPL_OMP_H_ 39 | #define PCL_VOXEL_GRID_COVARIANCE_IMPL_OMP_H_ 40 | 41 | #include 42 | #include 43 | #include "voxel_grid_covariance_omp.h" 44 | #include 45 | #include 46 | 47 | ////////////////////////////////////////////////////////////////////////////////////////// 48 | template void 49 | pclomp::VoxelGridCovariance::applyFilter (PointCloud &output) 50 | { 51 | voxel_centroids_leaf_indices_.clear (); 52 | 53 | // Has the input dataset been set already? 54 | if (!input_) 55 | { 56 | PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ()); 57 | output.width = output.height = 0; 58 | output.points.clear (); 59 | return; 60 | } 61 | 62 | // Copy the header (and thus the frame_id) + allocate enough space for points 63 | output.height = 1; // downsampling breaks the organized structure 64 | output.is_dense = true; // we filter out invalid points 65 | output.points.clear (); 66 | 67 | Eigen::Vector4f min_p, max_p; 68 | // Get the minimum and maximum dimensions 69 | if (!filter_field_name_.empty ()) // If we don't want to process the entire cloud... 70 | pcl::getMinMax3D (input_, filter_field_name_, static_cast (filter_limit_min_), static_cast (filter_limit_max_), min_p, max_p, filter_limit_negative_); 71 | else 72 | pcl::getMinMax3D (*input_, min_p, max_p); 73 | 74 | // Check that the leaf size is not too small, given the size of the data 75 | int64_t dx = static_cast((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1; 76 | int64_t dy = static_cast((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1; 77 | int64_t dz = static_cast((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1; 78 | 79 | if((dx*dy*dz) > std::numeric_limits::max()) 80 | { 81 | PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName().c_str()); 82 | output.clear(); 83 | return; 84 | } 85 | 86 | // Compute the minimum and maximum bounding box values 87 | min_b_[0] = static_cast (floor (min_p[0] * inverse_leaf_size_[0])); 88 | max_b_[0] = static_cast (floor (max_p[0] * inverse_leaf_size_[0])); 89 | min_b_[1] = static_cast (floor (min_p[1] * inverse_leaf_size_[1])); 90 | max_b_[1] = static_cast (floor (max_p[1] * inverse_leaf_size_[1])); 91 | min_b_[2] = static_cast (floor (min_p[2] * inverse_leaf_size_[2])); 92 | max_b_[2] = static_cast (floor (max_p[2] * inverse_leaf_size_[2])); 93 | 94 | // Compute the number of divisions needed along all axis 95 | div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones (); 96 | div_b_[3] = 0; 97 | 98 | // Clear the leaves 99 | leaves_.clear (); 100 | // leaves_.reserve(8192); 101 | 102 | // Set up the division multiplier 103 | divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0); 104 | 105 | int centroid_size = 4; 106 | 107 | if (downsample_all_data_) 108 | centroid_size = boost::mpl::size::value; 109 | 110 | // ---[ RGB special case 111 | std::vector fields; 112 | int rgba_index = -1; 113 | rgba_index = pcl::getFieldIndex ("rgb", fields); 114 | if (rgba_index == -1) 115 | rgba_index = pcl::getFieldIndex ("rgba", fields); 116 | if (rgba_index >= 0) 117 | { 118 | rgba_index = fields[rgba_index].offset; 119 | centroid_size += 3; 120 | } 121 | 122 | // If we don't want to process the entire cloud, but rather filter points far away from the viewpoint first... 123 | if (!filter_field_name_.empty ()) 124 | { 125 | // Get the distance field index 126 | std::vector fields; 127 | int distance_idx = pcl::getFieldIndex (filter_field_name_, fields); 128 | if (distance_idx == -1) 129 | PCL_WARN ("[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().c_str (), distance_idx); 130 | 131 | // First pass: go over all points and insert them into the right leaf 132 | for (size_t cp = 0; cp < input_->points.size (); ++cp) 133 | { 134 | if (!input_->is_dense) 135 | // Check if the point is invalid 136 | if (!std::isfinite (input_->points[cp].x) || 137 | !std::isfinite (input_->points[cp].y) || 138 | !std::isfinite (input_->points[cp].z)) 139 | continue; 140 | 141 | // Get the distance value 142 | const uint8_t* pt_data = reinterpret_cast (&input_->points[cp]); 143 | float distance_value = 0; 144 | memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float)); 145 | 146 | if (filter_limit_negative_) 147 | { 148 | // Use a threshold for cutting out points which inside the interval 149 | if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_)) 150 | continue; 151 | } 152 | else 153 | { 154 | // Use a threshold for cutting out points which are too close/far away 155 | if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_)) 156 | continue; 157 | } 158 | 159 | int ijk0 = static_cast (floor (input_->points[cp].x * inverse_leaf_size_[0]) - static_cast (min_b_[0])); 160 | int ijk1 = static_cast (floor (input_->points[cp].y * inverse_leaf_size_[1]) - static_cast (min_b_[1])); 161 | int ijk2 = static_cast (floor (input_->points[cp].z * inverse_leaf_size_[2]) - static_cast (min_b_[2])); 162 | 163 | // Compute the centroid leaf index 164 | int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; 165 | 166 | Leaf& leaf = leaves_[idx]; 167 | if (leaf.nr_points == 0) 168 | { 169 | leaf.centroid.resize (centroid_size); 170 | leaf.centroid.setZero (); 171 | } 172 | 173 | Eigen::Vector3d pt3d (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z); 174 | // Accumulate point sum for centroid calculation 175 | leaf.mean_ += pt3d; 176 | // Accumulate x*xT for single pass covariance calculation 177 | leaf.cov_ += pt3d * pt3d.transpose (); 178 | 179 | // Do we need to process all the fields? 180 | if (!downsample_all_data_) 181 | { 182 | Eigen::Vector4f pt (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0); 183 | leaf.centroid.template head<4> () += pt; 184 | } 185 | else 186 | { 187 | // Copy all the fields 188 | Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size); 189 | // ---[ RGB special case 190 | if (rgba_index >= 0) 191 | { 192 | // fill r/g/b data 193 | int rgb; 194 | memcpy (&rgb, reinterpret_cast (&input_->points[cp]) + rgba_index, sizeof (int)); 195 | centroid[centroid_size - 3] = static_cast ((rgb >> 16) & 0x0000ff); 196 | centroid[centroid_size - 2] = static_cast ((rgb >> 8) & 0x0000ff); 197 | centroid[centroid_size - 1] = static_cast ((rgb) & 0x0000ff); 198 | } 199 | pcl::for_each_type (pcl::NdCopyPointEigenFunctor (input_->points[cp], centroid)); 200 | leaf.centroid += centroid; 201 | } 202 | ++leaf.nr_points; 203 | } 204 | } 205 | // No distance filtering, process all data 206 | else 207 | { 208 | // First pass: go over all points and insert them into the right leaf 209 | for (size_t cp = 0; cp < input_->points.size (); ++cp) 210 | { 211 | if (!input_->is_dense) 212 | // Check if the point is invalid 213 | if (!std::isfinite (input_->points[cp].x) || 214 | !std::isfinite (input_->points[cp].y) || 215 | !std::isfinite (input_->points[cp].z)) 216 | continue; 217 | 218 | int ijk0 = static_cast (floor (input_->points[cp].x * inverse_leaf_size_[0]) - static_cast (min_b_[0])); 219 | int ijk1 = static_cast (floor (input_->points[cp].y * inverse_leaf_size_[1]) - static_cast (min_b_[1])); 220 | int ijk2 = static_cast (floor (input_->points[cp].z * inverse_leaf_size_[2]) - static_cast (min_b_[2])); 221 | 222 | // Compute the centroid leaf index 223 | int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; 224 | 225 | //int idx = (((input_->points[cp].getArray4fmap () * inverse_leaf_size_).template cast ()).matrix () - min_b_).dot (divb_mul_); 226 | Leaf& leaf = leaves_[idx]; 227 | if (leaf.nr_points == 0) 228 | { 229 | leaf.centroid.resize (centroid_size); 230 | leaf.centroid.setZero (); 231 | } 232 | 233 | Eigen::Vector3d pt3d (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z); 234 | // Accumulate point sum for centroid calculation 235 | leaf.mean_ += pt3d; 236 | // Accumulate x*xT for single pass covariance calculation 237 | leaf.cov_ += pt3d * pt3d.transpose (); 238 | 239 | // Do we need to process all the fields? 240 | if (!downsample_all_data_) 241 | { 242 | Eigen::Vector4f pt (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0); 243 | leaf.centroid.template head<4> () += pt; 244 | } 245 | else 246 | { 247 | // Copy all the fields 248 | Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size); 249 | // ---[ RGB special case 250 | if (rgba_index >= 0) 251 | { 252 | // Fill r/g/b data, assuming that the order is BGRA 253 | int rgb; 254 | memcpy (&rgb, reinterpret_cast (&input_->points[cp]) + rgba_index, sizeof (int)); 255 | centroid[centroid_size - 3] = static_cast ((rgb >> 16) & 0x0000ff); 256 | centroid[centroid_size - 2] = static_cast ((rgb >> 8) & 0x0000ff); 257 | centroid[centroid_size - 1] = static_cast ((rgb) & 0x0000ff); 258 | } 259 | pcl::for_each_type (pcl::NdCopyPointEigenFunctor (input_->points[cp], centroid)); 260 | leaf.centroid += centroid; 261 | } 262 | ++leaf.nr_points; 263 | } 264 | } 265 | 266 | // Second pass: go over all leaves and compute centroids and covariance matrices 267 | output.points.reserve (leaves_.size ()); 268 | if (searchable_) 269 | voxel_centroids_leaf_indices_.reserve (leaves_.size ()); 270 | int cp = 0; 271 | if (save_leaf_layout_) 272 | leaf_layout_.resize (div_b_[0] * div_b_[1] * div_b_[2], -1); 273 | 274 | // Eigen values and vectors calculated to prevent near singular matrices 275 | Eigen::SelfAdjointEigenSolver eigensolver; 276 | Eigen::Matrix3d eigen_val; 277 | Eigen::Vector3d pt_sum; 278 | 279 | // Eigen values less than a threshold of max eigen value are inflated to a set fraction of the max eigen value. 280 | double min_covar_eigvalue; 281 | 282 | for (auto it = leaves_.begin (); it != leaves_.end (); ++it) 283 | { 284 | 285 | // Normalize the centroid 286 | Leaf& leaf = it->second; 287 | 288 | // Normalize the centroid 289 | leaf.centroid /= static_cast (leaf.nr_points); 290 | // Point sum used for single pass covariance calculation 291 | pt_sum = leaf.mean_; 292 | // Normalize mean 293 | leaf.mean_ /= leaf.nr_points; 294 | 295 | // If the voxel contains sufficient points, its covariance is calculated and is added to the voxel centroids and output clouds. 296 | // Points with less than the minimum points will have a can not be accurately approximated using a normal distribution. 297 | if (leaf.nr_points >= min_points_per_voxel_) 298 | { 299 | if (save_leaf_layout_) 300 | leaf_layout_[it->first] = cp++; 301 | 302 | output.push_back (PointT ()); 303 | 304 | // Do we need to process all the fields? 305 | if (!downsample_all_data_) 306 | { 307 | output.points.back ().x = leaf.centroid[0]; 308 | output.points.back ().y = leaf.centroid[1]; 309 | output.points.back ().z = leaf.centroid[2]; 310 | } 311 | else 312 | { 313 | pcl::for_each_type (pcl::NdCopyEigenPointFunctor (leaf.centroid, output.back ())); 314 | // ---[ RGB special case 315 | if (rgba_index >= 0) 316 | { 317 | // pack r/g/b into rgb 318 | float r = leaf.centroid[centroid_size - 3], g = leaf.centroid[centroid_size - 2], b = leaf.centroid[centroid_size - 1]; 319 | int rgb = (static_cast (r)) << 16 | (static_cast (g)) << 8 | (static_cast (b)); 320 | memcpy (reinterpret_cast (&output.points.back ()) + rgba_index, &rgb, sizeof (float)); 321 | } 322 | } 323 | 324 | // Stores the voxel indices for fast access searching 325 | if (searchable_) 326 | voxel_centroids_leaf_indices_.push_back (static_cast (it->first)); 327 | 328 | // Single pass covariance calculation 329 | leaf.cov_ = (leaf.cov_ - 2 * (pt_sum * leaf.mean_.transpose ())) / leaf.nr_points + leaf.mean_ * leaf.mean_.transpose (); 330 | leaf.cov_ *= (leaf.nr_points - 1.0) / leaf.nr_points; 331 | 332 | //Normalize Eigen Val such that max no more than 100x min. 333 | eigensolver.compute (leaf.cov_); 334 | eigen_val = eigensolver.eigenvalues ().asDiagonal (); 335 | leaf.evecs_ = eigensolver.eigenvectors (); 336 | 337 | if (eigen_val (0, 0) < 0 || eigen_val (1, 1) < 0 || eigen_val (2, 2) <= 0) 338 | { 339 | leaf.nr_points = -1; 340 | continue; 341 | } 342 | 343 | // Avoids matrices near singularities (eq 6.11)[Magnusson 2009] 344 | 345 | min_covar_eigvalue = min_covar_eigvalue_mult_ * eigen_val (2, 2); 346 | if (eigen_val (0, 0) < min_covar_eigvalue) 347 | { 348 | eigen_val (0, 0) = min_covar_eigvalue; 349 | 350 | if (eigen_val (1, 1) < min_covar_eigvalue) 351 | { 352 | eigen_val (1, 1) = min_covar_eigvalue; 353 | } 354 | 355 | leaf.cov_ = leaf.evecs_ * eigen_val * leaf.evecs_.inverse (); 356 | } 357 | leaf.evals_ = eigen_val.diagonal (); 358 | 359 | leaf.icov_ = leaf.cov_.inverse (); 360 | if (leaf.icov_.maxCoeff () == std::numeric_limits::infinity ( ) 361 | || leaf.icov_.minCoeff () == -std::numeric_limits::infinity ( ) ) 362 | { 363 | leaf.nr_points = -1; 364 | } 365 | 366 | } 367 | } 368 | 369 | output.width = static_cast (output.points.size ()); 370 | } 371 | 372 | ////////////////////////////////////////////////////////////////////////////////////////// 373 | template int 374 | pclomp::VoxelGridCovariance::getNeighborhoodAtPoint(const Eigen::MatrixXi& relative_coordinates, const PointT& reference_point, std::vector &neighbors) const 375 | { 376 | neighbors.clear(); 377 | 378 | // Find displacement coordinates 379 | Eigen::Vector4i ijk(static_cast (floor(reference_point.x / leaf_size_[0])), 380 | static_cast (floor(reference_point.y / leaf_size_[1])), 381 | static_cast (floor(reference_point.z / leaf_size_[2])), 0); 382 | Eigen::Array4i diff2min = min_b_ - ijk; 383 | Eigen::Array4i diff2max = max_b_ - ijk; 384 | neighbors.reserve(relative_coordinates.cols()); 385 | 386 | // Check each neighbor to see if it is occupied and contains sufficient points 387 | // Slower than radius search because needs to check 26 indices 388 | for (int ni = 0; ni < relative_coordinates.cols(); ni++) 389 | { 390 | Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished(); 391 | // Checking if the specified cell is in the grid 392 | if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all()) 393 | { 394 | auto leaf_iter = leaves_.find(((ijk + displacement - min_b_).dot(divb_mul_))); 395 | if (leaf_iter != leaves_.end() && leaf_iter->second.nr_points >= min_points_per_voxel_) 396 | { 397 | LeafConstPtr leaf = &(leaf_iter->second); 398 | neighbors.push_back(leaf); 399 | } 400 | } 401 | } 402 | 403 | return (static_cast (neighbors.size())); 404 | } 405 | 406 | ////////////////////////////////////////////////////////////////////////////////////////// 407 | template int 408 | pclomp::VoxelGridCovariance::getNeighborhoodAtPoint(const PointT& reference_point, std::vector &neighbors) const 409 | { 410 | neighbors.clear(); 411 | 412 | // Find displacement coordinates 413 | Eigen::MatrixXi relative_coordinates = pcl::getAllNeighborCellIndices(); 414 | return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors); 415 | } 416 | 417 | ////////////////////////////////////////////////////////////////////////////////////////// 418 | template int 419 | pclomp::VoxelGridCovariance::getNeighborhoodAtPoint7(const PointT& reference_point, std::vector &neighbors) const 420 | { 421 | neighbors.clear(); 422 | 423 | Eigen::MatrixXi relative_coordinates(3, 7); 424 | relative_coordinates.setZero(); 425 | relative_coordinates(0, 1) = 1; 426 | relative_coordinates(0, 2) = -1; 427 | relative_coordinates(1, 3) = 1; 428 | relative_coordinates(1, 4) = -1; 429 | relative_coordinates(2, 5) = 1; 430 | relative_coordinates(2, 6) = -1; 431 | 432 | return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors); 433 | } 434 | 435 | 436 | ////////////////////////////////////////////////////////////////////////////////////////// 437 | template int 438 | pclomp::VoxelGridCovariance::getNeighborhoodAtPoint1(const PointT& reference_point, std::vector &neighbors) const 439 | { 440 | neighbors.clear(); 441 | return getNeighborhoodAtPoint(Eigen::MatrixXi::Zero(3,1), reference_point, neighbors); 442 | } 443 | 444 | 445 | 446 | ////////////////////////////////////////////////////////////////////////////////////////// 447 | template void 448 | pclomp::VoxelGridCovariance::getDisplayCloud (pcl::PointCloud& cell_cloud) 449 | { 450 | cell_cloud.clear (); 451 | 452 | int pnt_per_cell = 1000; 453 | boost::mt19937 rng; 454 | boost::normal_distribution<> nd (0.0, leaf_size_.head (3).norm ()); 455 | boost::variate_generator > var_nor (rng, nd); 456 | 457 | Eigen::LLT llt_of_cov; 458 | Eigen::Matrix3d cholesky_decomp; 459 | Eigen::Vector3d cell_mean; 460 | Eigen::Vector3d rand_point; 461 | Eigen::Vector3d dist_point; 462 | 463 | // Generate points for each occupied voxel with sufficient points. 464 | for (auto it = leaves_.begin (); it != leaves_.end (); ++it) 465 | { 466 | Leaf& leaf = it->second; 467 | 468 | if (leaf.nr_points >= min_points_per_voxel_) 469 | { 470 | cell_mean = leaf.mean_; 471 | llt_of_cov.compute (leaf.cov_); 472 | cholesky_decomp = llt_of_cov.matrixL (); 473 | 474 | // Random points generated by sampling the normal distribution given by voxel mean and covariance matrix 475 | for (int i = 0; i < pnt_per_cell; i++) 476 | { 477 | rand_point = Eigen::Vector3d (var_nor (), var_nor (), var_nor ()); 478 | dist_point = cell_mean + cholesky_decomp * rand_point; 479 | cell_cloud.push_back (pcl::PointXYZ (static_cast (dist_point (0)), static_cast (dist_point (1)), static_cast (dist_point (2)))); 480 | } 481 | } 482 | } 483 | } 484 | 485 | #define PCL_INSTANTIATE_VoxelGridCovariance(T) template class PCL_EXPORTS pcl::VoxelGridCovariance; 486 | 487 | #endif // PCL_VOXEL_GRID_COVARIANCE_IMPL_H_ 488 | -------------------------------------------------------------------------------- /include/pclomp/voxel_grid_covariance_omp.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2010-2011, Willow Garage, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the copyright holder(s) nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | */ 37 | 38 | #ifndef PCL_VOXEL_GRID_COVARIANCE_OMP_H_ 39 | #define PCL_VOXEL_GRID_COVARIANCE_OMP_H_ 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | namespace pclomp 50 | { 51 | /** \brief A searchable voxel structure containing the mean and covariance of the data. 52 | * \note For more information please see 53 | * Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform — 54 | * an Efficient Representation for Registration, Surface Analysis, and Loop Detection. 55 | * PhD thesis, Orebro University. Orebro Studies in Technology 36 56 | * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific) 57 | */ 58 | template 59 | class VoxelGridCovariance : public pcl::VoxelGrid 60 | { 61 | protected: 62 | using pcl::VoxelGrid::filter_name_; 63 | using pcl::VoxelGrid::getClassName; 64 | using pcl::VoxelGrid::input_; 65 | using pcl::VoxelGrid::indices_; 66 | using pcl::VoxelGrid::filter_limit_negative_; 67 | using pcl::VoxelGrid::filter_limit_min_; 68 | using pcl::VoxelGrid::filter_limit_max_; 69 | using pcl::VoxelGrid::filter_field_name_; 70 | 71 | using pcl::VoxelGrid::downsample_all_data_; 72 | using pcl::VoxelGrid::leaf_layout_; 73 | using pcl::VoxelGrid::save_leaf_layout_; 74 | using pcl::VoxelGrid::leaf_size_; 75 | using pcl::VoxelGrid::min_b_; 76 | using pcl::VoxelGrid::max_b_; 77 | using pcl::VoxelGrid::inverse_leaf_size_; 78 | using pcl::VoxelGrid::div_b_; 79 | using pcl::VoxelGrid::divb_mul_; 80 | 81 | typedef typename pcl::traits::fieldList::type FieldList; 82 | typedef typename pcl::Filter::PointCloud PointCloud; 83 | typedef typename PointCloud::Ptr PointCloudPtr; 84 | typedef typename PointCloud::ConstPtr PointCloudConstPtr; 85 | 86 | public: 87 | 88 | #if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0) 89 | typedef pcl::shared_ptr< pcl::VoxelGrid > Ptr; 90 | typedef pcl::shared_ptr< const pcl::VoxelGrid > ConstPtr; 91 | #else 92 | typedef boost::shared_ptr< pcl::VoxelGrid > Ptr; 93 | typedef boost::shared_ptr< const pcl::VoxelGrid > ConstPtr; 94 | #endif 95 | 96 | /** \brief Simple structure to hold a centroid, covariance and the number of points in a leaf. 97 | * Inverse covariance, eigen vectors and eigen values are precomputed. */ 98 | struct Leaf 99 | { 100 | /** \brief Constructor. 101 | * Sets \ref nr_points, \ref icov_, \ref mean_ and \ref evals_ to 0 and \ref cov_ and \ref evecs_ to the identity matrix 102 | */ 103 | Leaf () : 104 | nr_points (0), 105 | mean_ (Eigen::Vector3d::Zero ()), 106 | centroid (), 107 | cov_ (Eigen::Matrix3d::Identity ()), 108 | icov_ (Eigen::Matrix3d::Zero ()), 109 | evecs_ (Eigen::Matrix3d::Identity ()), 110 | evals_ (Eigen::Vector3d::Zero ()) 111 | { 112 | } 113 | 114 | /** \brief Get the voxel covariance. 115 | * \return covariance matrix 116 | */ 117 | Eigen::Matrix3d 118 | getCov () const 119 | { 120 | return (cov_); 121 | } 122 | 123 | /** \brief Get the inverse of the voxel covariance. 124 | * \return inverse covariance matrix 125 | */ 126 | Eigen::Matrix3d 127 | getInverseCov () const 128 | { 129 | return (icov_); 130 | } 131 | 132 | /** \brief Get the voxel centroid. 133 | * \return centroid 134 | */ 135 | Eigen::Vector3d 136 | getMean () const 137 | { 138 | return (mean_); 139 | } 140 | 141 | /** \brief Get the eigen vectors of the voxel covariance. 142 | * \note Order corresponds with \ref getEvals 143 | * \return matrix whose columns contain eigen vectors 144 | */ 145 | Eigen::Matrix3d 146 | getEvecs () const 147 | { 148 | return (evecs_); 149 | } 150 | 151 | /** \brief Get the eigen values of the voxel covariance. 152 | * \note Order corresponds with \ref getEvecs 153 | * \return vector of eigen values 154 | */ 155 | Eigen::Vector3d 156 | getEvals () const 157 | { 158 | return (evals_); 159 | } 160 | 161 | /** \brief Get the number of points contained by this voxel. 162 | * \return number of points 163 | */ 164 | int 165 | getPointCount () const 166 | { 167 | return (nr_points); 168 | } 169 | 170 | /** \brief Number of points contained by voxel */ 171 | int nr_points; 172 | 173 | /** \brief 3D voxel centroid */ 174 | Eigen::Vector3d mean_; 175 | 176 | /** \brief Nd voxel centroid 177 | * \note Differs from \ref mean_ when color data is used 178 | */ 179 | Eigen::VectorXf centroid; 180 | 181 | /** \brief Voxel covariance matrix */ 182 | Eigen::Matrix3d cov_; 183 | 184 | /** \brief Inverse of voxel covariance matrix */ 185 | Eigen::Matrix3d icov_; 186 | 187 | /** \brief Eigen vectors of voxel covariance matrix */ 188 | Eigen::Matrix3d evecs_; 189 | 190 | /** \brief Eigen values of voxel covariance matrix */ 191 | Eigen::Vector3d evals_; 192 | 193 | }; 194 | 195 | /** \brief Pointer to VoxelGridCovariance leaf structure */ 196 | typedef Leaf* LeafPtr; 197 | 198 | /** \brief Const pointer to VoxelGridCovariance leaf structure */ 199 | typedef const Leaf* LeafConstPtr; 200 | 201 | typedef std::map Map; 202 | 203 | public: 204 | 205 | /** \brief Constructor. 206 | * Sets \ref leaf_size_ to 0 and \ref searchable_ to false. 207 | */ 208 | VoxelGridCovariance () : 209 | searchable_ (true), 210 | min_points_per_voxel_ (6), 211 | min_covar_eigvalue_mult_ (0.01), 212 | leaves_ (), 213 | voxel_centroids_ (), 214 | voxel_centroids_leaf_indices_ (), 215 | kdtree_ () 216 | { 217 | downsample_all_data_ = false; 218 | save_leaf_layout_ = false; 219 | leaf_size_.setZero (); 220 | min_b_.setZero (); 221 | max_b_.setZero (); 222 | filter_name_ = "VoxelGridCovariance"; 223 | } 224 | 225 | /** \brief Set the minimum number of points required for a cell to be used (must be 3 or greater for covariance calculation). 226 | * \param[in] min_points_per_voxel the minimum number of points for required for a voxel to be used 227 | */ 228 | inline void 229 | setMinPointPerVoxel (int min_points_per_voxel) 230 | { 231 | if(min_points_per_voxel > 2) 232 | { 233 | min_points_per_voxel_ = min_points_per_voxel; 234 | } 235 | else 236 | { 237 | PCL_WARN ("%s: Covariance calculation requires at least 3 points, setting Min Point per Voxel to 3 ", this->getClassName ().c_str ()); 238 | min_points_per_voxel_ = 3; 239 | } 240 | } 241 | 242 | /** \brief Get the minimum number of points required for a cell to be used. 243 | * \return the minimum number of points for required for a voxel to be used 244 | */ 245 | inline int 246 | getMinPointPerVoxel () 247 | { 248 | return min_points_per_voxel_; 249 | } 250 | 251 | /** \brief Set the minimum allowable ratio between eigenvalues to prevent singular covariance matrices. 252 | * \param[in] min_covar_eigvalue_mult the minimum allowable ratio between eigenvalues 253 | */ 254 | inline void 255 | setCovEigValueInflationRatio (double min_covar_eigvalue_mult) 256 | { 257 | min_covar_eigvalue_mult_ = min_covar_eigvalue_mult; 258 | } 259 | 260 | /** \brief Get the minimum allowable ratio between eigenvalues to prevent singular covariance matrices. 261 | * \return the minimum allowable ratio between eigenvalues 262 | */ 263 | inline double 264 | getCovEigValueInflationRatio () 265 | { 266 | return min_covar_eigvalue_mult_; 267 | } 268 | 269 | /** \brief Filter cloud and initializes voxel structure. 270 | * \param[out] output cloud containing centroids of voxels containing a sufficient number of points 271 | * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built 272 | */ 273 | inline void 274 | filter (PointCloud &output, bool searchable = false) 275 | { 276 | searchable_ = searchable; 277 | applyFilter (output); 278 | 279 | voxel_centroids_ = PointCloudPtr (new PointCloud (output)); 280 | 281 | if (searchable_ && voxel_centroids_->size() > 0) 282 | { 283 | // Initiates kdtree of the centroids of voxels containing a sufficient number of points 284 | kdtree_.setInputCloud (voxel_centroids_); 285 | } 286 | } 287 | 288 | /** \brief Initializes voxel structure. 289 | * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built 290 | */ 291 | inline void 292 | filter (bool searchable = false) 293 | { 294 | searchable_ = searchable; 295 | voxel_centroids_ = PointCloudPtr (new PointCloud); 296 | applyFilter (*voxel_centroids_); 297 | 298 | if (searchable_ && voxel_centroids_->size() > 0) 299 | { 300 | // Initiates kdtree of the centroids of voxels containing a sufficient number of points 301 | kdtree_.setInputCloud (voxel_centroids_); 302 | } 303 | } 304 | 305 | /** \brief Get the voxel containing point p. 306 | * \param[in] index the index of the leaf structure node 307 | * \return const pointer to leaf structure 308 | */ 309 | inline LeafConstPtr 310 | getLeaf (int index) 311 | { 312 | auto leaf_iter = leaves_.find (index); 313 | if (leaf_iter != leaves_.end ()) 314 | { 315 | LeafConstPtr ret (&(leaf_iter->second)); 316 | return ret; 317 | } 318 | else 319 | return NULL; 320 | } 321 | 322 | /** \brief Get the voxel containing point p. 323 | * \param[in] p the point to get the leaf structure at 324 | * \return const pointer to leaf structure 325 | */ 326 | inline LeafConstPtr 327 | getLeaf (PointT &p) 328 | { 329 | // Generate index associated with p 330 | int ijk0 = static_cast (floor (p.x * inverse_leaf_size_[0]) - min_b_[0]); 331 | int ijk1 = static_cast (floor (p.y * inverse_leaf_size_[1]) - min_b_[1]); 332 | int ijk2 = static_cast (floor (p.z * inverse_leaf_size_[2]) - min_b_[2]); 333 | 334 | // Compute the centroid leaf index 335 | int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; 336 | 337 | // Find leaf associated with index 338 | auto leaf_iter = leaves_.find (idx); 339 | if (leaf_iter != leaves_.end ()) 340 | { 341 | // If such a leaf exists return the pointer to the leaf structure 342 | LeafConstPtr ret (&(leaf_iter->second)); 343 | return ret; 344 | } 345 | else 346 | return NULL; 347 | } 348 | 349 | /** \brief Get the voxel containing point p. 350 | * \param[in] p the point to get the leaf structure at 351 | * \return const pointer to leaf structure 352 | */ 353 | inline LeafConstPtr 354 | getLeaf (Eigen::Vector3f &p) 355 | { 356 | // Generate index associated with p 357 | int ijk0 = static_cast (floor (p[0] * inverse_leaf_size_[0]) - min_b_[0]); 358 | int ijk1 = static_cast (floor (p[1] * inverse_leaf_size_[1]) - min_b_[1]); 359 | int ijk2 = static_cast (floor (p[2] * inverse_leaf_size_[2]) - min_b_[2]); 360 | 361 | // Compute the centroid leaf index 362 | int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; 363 | 364 | // Find leaf associated with index 365 | auto leaf_iter = leaves_.find (idx); 366 | if (leaf_iter != leaves_.end ()) 367 | { 368 | // If such a leaf exists return the pointer to the leaf structure 369 | LeafConstPtr ret (&(leaf_iter->second)); 370 | return ret; 371 | } 372 | else 373 | return NULL; 374 | 375 | } 376 | 377 | /** \brief Get the voxels surrounding point p, not including the voxel containing point p. 378 | * \note Only voxels containing a sufficient number of points are used (slower than radius search in practice). 379 | * \param[in] reference_point the point to get the leaf structure at 380 | * \param[out] neighbors 381 | * \return number of neighbors found 382 | */ 383 | int getNeighborhoodAtPoint(const Eigen::MatrixXi&, const PointT& reference_point, std::vector &neighbors) const ; 384 | int getNeighborhoodAtPoint(const PointT& reference_point, std::vector &neighbors) const ; 385 | int getNeighborhoodAtPoint7(const PointT& reference_point, std::vector &neighbors) const ; 386 | int getNeighborhoodAtPoint1(const PointT& reference_point, std::vector &neighbors) const ; 387 | 388 | /** \brief Get the leaf structure map 389 | * \return a map containing all leaves 390 | */ 391 | inline const Map& 392 | getLeaves () 393 | { 394 | return leaves_; 395 | } 396 | 397 | /** \brief Get a pointcloud containing the voxel centroids 398 | * \note Only voxels containing a sufficient number of points are used. 399 | * \return a map containing all leaves 400 | */ 401 | inline PointCloudPtr 402 | getCentroids () 403 | { 404 | return voxel_centroids_; 405 | } 406 | 407 | 408 | /** \brief Get a cloud to visualize each voxels normal distribution. 409 | * \param[out] cell_cloud a cloud created by sampling the normal distributions of each voxel 410 | */ 411 | void 412 | getDisplayCloud (pcl::PointCloud& cell_cloud); 413 | 414 | /** \brief Search for the k-nearest occupied voxels for the given query point. 415 | * \note Only voxels containing a sufficient number of points are used. 416 | * \param[in] point the given query point 417 | * \param[in] k the number of neighbors to search for 418 | * \param[out] k_leaves the resultant leaves of the neighboring points 419 | * \param[out] k_sqr_distances the resultant squared distances to the neighboring points 420 | * \return number of neighbors found 421 | */ 422 | int 423 | nearestKSearch (const PointT &point, int k, 424 | std::vector &k_leaves, std::vector &k_sqr_distances) 425 | { 426 | k_leaves.clear (); 427 | 428 | // Check if kdtree has been built 429 | if (!searchable_) 430 | { 431 | PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ()); 432 | return 0; 433 | } 434 | 435 | // Find k-nearest neighbors in the occupied voxel centroid cloud 436 | std::vector k_indices; 437 | k = kdtree_.nearestKSearch (point, k, k_indices, k_sqr_distances); 438 | 439 | // Find leaves corresponding to neighbors 440 | k_leaves.reserve (k); 441 | for (std::vector::iterator iter = k_indices.begin (); iter != k_indices.end (); iter++) 442 | { 443 | k_leaves.push_back (&leaves_[voxel_centroids_leaf_indices_[*iter]]); 444 | } 445 | return k; 446 | } 447 | 448 | /** \brief Search for the k-nearest occupied voxels for the given query point. 449 | * \note Only voxels containing a sufficient number of points are used. 450 | * \param[in] cloud the given query point 451 | * \param[in] index the index 452 | * \param[in] k the number of neighbors to search for 453 | * \param[out] k_leaves the resultant leaves of the neighboring points 454 | * \param[out] k_sqr_distances the resultant squared distances to the neighboring points 455 | * \return number of neighbors found 456 | */ 457 | inline int 458 | nearestKSearch (const PointCloud &cloud, int index, int k, 459 | std::vector &k_leaves, std::vector &k_sqr_distances) 460 | { 461 | if (index >= static_cast (cloud.points.size ()) || index < 0) 462 | return (0); 463 | return (nearestKSearch (cloud.points[index], k, k_leaves, k_sqr_distances)); 464 | } 465 | 466 | 467 | /** \brief Search for all the nearest occupied voxels of the query point in a given radius. 468 | * \note Only voxels containing a sufficient number of points are used. 469 | * \param[in] point the given query point 470 | * \param[in] radius the radius of the sphere bounding all of p_q's neighbors 471 | * \param[out] k_leaves the resultant leaves of the neighboring points 472 | * \param[out] k_sqr_distances the resultant squared distances to the neighboring points 473 | * \param[in] max_nn 474 | * \return number of neighbors found 475 | */ 476 | int 477 | radiusSearch (const PointT &point, double radius, std::vector &k_leaves, 478 | std::vector &k_sqr_distances, unsigned int max_nn = 0) const 479 | { 480 | k_leaves.clear (); 481 | 482 | // Check if kdtree has been built 483 | if (!searchable_) 484 | { 485 | PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ()); 486 | return 0; 487 | } 488 | 489 | // Find neighbors within radius in the occupied voxel centroid cloud 490 | std::vector k_indices; 491 | int k = kdtree_.radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn); 492 | 493 | // Find leaves corresponding to neighbors 494 | k_leaves.reserve (k); 495 | for (std::vector::iterator iter = k_indices.begin (); iter != k_indices.end (); iter++) 496 | { 497 | auto leaf = leaves_.find(voxel_centroids_leaf_indices_[*iter]); 498 | if (leaf == leaves_.end()) { 499 | std::cerr << "error : could not find the leaf corresponding to the voxel" << std::endl; 500 | std::cin.ignore(1); 501 | } 502 | k_leaves.push_back (&(leaf->second)); 503 | } 504 | return k; 505 | } 506 | 507 | /** \brief Search for all the nearest occupied voxels of the query point in a given radius. 508 | * \note Only voxels containing a sufficient number of points are used. 509 | * \param[in] cloud the given query point 510 | * \param[in] index a valid index in cloud representing a valid (i.e., finite) query point 511 | * \param[in] radius the radius of the sphere bounding all of p_q's neighbors 512 | * \param[out] k_leaves the resultant leaves of the neighboring points 513 | * \param[out] k_sqr_distances the resultant squared distances to the neighboring points 514 | * \param[in] max_nn 515 | * \return number of neighbors found 516 | */ 517 | inline int 518 | radiusSearch (const PointCloud &cloud, int index, double radius, 519 | std::vector &k_leaves, std::vector &k_sqr_distances, 520 | unsigned int max_nn = 0) const 521 | { 522 | if (index >= static_cast (cloud.points.size ()) || index < 0) 523 | return (0); 524 | return (radiusSearch (cloud.points[index], radius, k_leaves, k_sqr_distances, max_nn)); 525 | } 526 | 527 | protected: 528 | 529 | /** \brief Filter cloud and initializes voxel structure. 530 | * \param[out] output cloud containing centroids of voxels containing a sufficient number of points 531 | */ 532 | void applyFilter (PointCloud &output); 533 | 534 | /** \brief Flag to determine if voxel structure is searchable. */ 535 | bool searchable_; 536 | 537 | /** \brief Minimum points contained with in a voxel to allow it to be usable. */ 538 | int min_points_per_voxel_; 539 | 540 | /** \brief Minimum allowable ratio between eigenvalues to prevent singular covariance matrices. */ 541 | double min_covar_eigvalue_mult_; 542 | 543 | /** \brief Voxel structure containing all leaf nodes (includes voxels with less than a sufficient number of points). */ 544 | Map leaves_; 545 | 546 | /** \brief Point cloud containing centroids of voxels containing at least minimum number of points. */ 547 | PointCloudPtr voxel_centroids_; 548 | 549 | /** \brief Indices of leaf structures associated with each point in \ref voxel_centroids_ (used for searching). */ 550 | std::vector voxel_centroids_leaf_indices_; 551 | 552 | /** \brief KdTree generated using \ref voxel_centroids_ (used for searching). */ 553 | pcl::KdTreeFLANN kdtree_; 554 | }; 555 | } 556 | 557 | #endif //#ifndef PCL_VOXEL_GRID_COVARIANCE_H_ 558 | -------------------------------------------------------------------------------- /include/pclomp/gicp_omp_impl.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2010, Willow Garage, Inc. 6 | * Copyright (c) 2012-, Open Perception, Inc. 7 | * 8 | * All rights reserved. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions 12 | * are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * * Neither the name of the copyright holder(s) nor the names of its 21 | * contributors may be used to endorse or promote products derived 22 | * from this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | * POSSIBILITY OF SUCH DAMAGE. 36 | * 37 | * $Id$ 38 | * 39 | */ 40 | #ifndef PCL_REGISTRATION_IMPL_GICP_OMP_HPP_ 41 | #define PCL_REGISTRATION_IMPL_GICP_OMP_HPP_ 42 | 43 | #include 44 | #include 45 | #include 46 | 47 | //////////////////////////////////////////////////////////////////////////////////////// 48 | template 49 | template void 50 | pclomp::GeneralizedIterativeClosestPoint::computeCovariances(typename pcl::PointCloud::ConstPtr cloud, 51 | const typename pcl::search::KdTree::ConstPtr kdtree, 52 | MatricesVector& cloud_covariances) 53 | { 54 | if (k_correspondences_ > int (cloud->size ())) 55 | { 56 | PCL_ERROR ("[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number of points in cloud (%lu) is less than k_correspondences_ (%lu)!\n", cloud->size (), k_correspondences_); 57 | return; 58 | } 59 | 60 | // We should never get there but who knows 61 | if(cloud_covariances.size () < cloud->size ()) 62 | cloud_covariances.resize (cloud->size ()); 63 | 64 | std::vector> nn_indices_array(omp_get_max_threads()); 65 | std::vector> nn_dist_sq_array(omp_get_max_threads()); 66 | 67 | #pragma omp parallel for 68 | for(std::size_t i=0; i < cloud->size(); i++) { 69 | auto& nn_indices = nn_indices_array[omp_get_thread_num()]; 70 | auto& nn_dist_sq = nn_dist_sq_array[omp_get_thread_num()]; 71 | 72 | const PointT &query_point = cloud->at(i); 73 | Eigen::Vector3d mean = Eigen::Vector3d::Zero(); 74 | Eigen::Matrix3d &cov = cloud_covariances[i]; 75 | // Zero out the cov and mean 76 | cov.setZero (); 77 | 78 | // Search for the K nearest neighbours 79 | kdtree->nearestKSearch(query_point, k_correspondences_, nn_indices, nn_dist_sq); 80 | 81 | // Find the covariance matrix 82 | for(int j = 0; j < k_correspondences_; j++) { 83 | const PointT &pt = (*cloud)[nn_indices[j]]; 84 | 85 | mean[0] += pt.x; 86 | mean[1] += pt.y; 87 | mean[2] += pt.z; 88 | 89 | cov(0,0) += pt.x*pt.x; 90 | 91 | cov(1,0) += pt.y*pt.x; 92 | cov(1,1) += pt.y*pt.y; 93 | 94 | cov(2,0) += pt.z*pt.x; 95 | cov(2,1) += pt.z*pt.y; 96 | cov(2,2) += pt.z*pt.z; 97 | } 98 | 99 | mean /= static_cast (k_correspondences_); 100 | // Get the actual covariance 101 | for (int k = 0; k < 3; k++) 102 | for (int l = 0; l <= k; l++) 103 | { 104 | cov(k,l) /= static_cast (k_correspondences_); 105 | cov(k,l) -= mean[k]*mean[l]; 106 | cov(l,k) = cov(k,l); 107 | } 108 | 109 | // Compute the SVD (covariance matrix is symmetric so U = V') 110 | Eigen::JacobiSVD svd(cov, Eigen::ComputeFullU); 111 | cov.setZero (); 112 | Eigen::Matrix3d U = svd.matrixU (); 113 | // Reconstitute the covariance matrix with modified singular values using the column // vectors in V. 114 | for(int k = 0; k < 3; k++) { 115 | Eigen::Vector3d col = U.col(k); 116 | double v = 1.; // biggest 2 singular values replaced by 1 117 | if(k == 2) // smallest singular value replaced by gicp_epsilon 118 | v = gicp_epsilon_; 119 | cov+= v * col * col.transpose(); 120 | } 121 | } 122 | } 123 | 124 | //////////////////////////////////////////////////////////////////////////////////////// 125 | template void 126 | pclomp::GeneralizedIterativeClosestPoint::computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d& g) const 127 | { 128 | Eigen::Matrix3d dR_dPhi; 129 | Eigen::Matrix3d dR_dTheta; 130 | Eigen::Matrix3d dR_dPsi; 131 | 132 | double phi = x[3], theta = x[4], psi = x[5]; 133 | 134 | double cphi = std::cos(phi), sphi = sin(phi); 135 | double ctheta = std::cos(theta), stheta = sin(theta); 136 | double cpsi = std::cos(psi), spsi = sin(psi); 137 | 138 | dR_dPhi(0,0) = 0.; 139 | dR_dPhi(1,0) = 0.; 140 | dR_dPhi(2,0) = 0.; 141 | 142 | dR_dPhi(0,1) = sphi*spsi + cphi*cpsi*stheta; 143 | dR_dPhi(1,1) = -cpsi*sphi + cphi*spsi*stheta; 144 | dR_dPhi(2,1) = cphi*ctheta; 145 | 146 | dR_dPhi(0,2) = cphi*spsi - cpsi*sphi*stheta; 147 | dR_dPhi(1,2) = -cphi*cpsi - sphi*spsi*stheta; 148 | dR_dPhi(2,2) = -ctheta*sphi; 149 | 150 | dR_dTheta(0,0) = -cpsi*stheta; 151 | dR_dTheta(1,0) = -spsi*stheta; 152 | dR_dTheta(2,0) = -ctheta; 153 | 154 | dR_dTheta(0,1) = cpsi*ctheta*sphi; 155 | dR_dTheta(1,1) = ctheta*sphi*spsi; 156 | dR_dTheta(2,1) = -sphi*stheta; 157 | 158 | dR_dTheta(0,2) = cphi*cpsi*ctheta; 159 | dR_dTheta(1,2) = cphi*ctheta*spsi; 160 | dR_dTheta(2,2) = -cphi*stheta; 161 | 162 | dR_dPsi(0,0) = -ctheta*spsi; 163 | dR_dPsi(1,0) = cpsi*ctheta; 164 | dR_dPsi(2,0) = 0.; 165 | 166 | dR_dPsi(0,1) = -cphi*cpsi - sphi*spsi*stheta; 167 | dR_dPsi(1,1) = -cphi*spsi + cpsi*sphi*stheta; 168 | dR_dPsi(2,1) = 0.; 169 | 170 | dR_dPsi(0,2) = cpsi*sphi - cphi*spsi*stheta; 171 | dR_dPsi(1,2) = sphi*spsi + cphi*cpsi*stheta; 172 | dR_dPsi(2,2) = 0.; 173 | 174 | g[3] = matricesInnerProd(dR_dPhi, R); 175 | g[4] = matricesInnerProd(dR_dTheta, R); 176 | g[5] = matricesInnerProd(dR_dPsi, R); 177 | } 178 | 179 | //////////////////////////////////////////////////////////////////////////////////////// 180 | template void 181 | pclomp::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS (const PointCloudSource &cloud_src, 182 | const std::vector &indices_src, 183 | const PointCloudTarget &cloud_tgt, 184 | const std::vector &indices_tgt, 185 | Eigen::Matrix4f &transformation_matrix) 186 | { 187 | if (indices_src.size () < 4) // need at least 4 samples 188 | { 189 | PCL_THROW_EXCEPTION (pcl::NotEnoughPointsException, 190 | "[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS] Need at least 4 points to estimate a transform! Source and target have " << indices_src.size () << " points!"); 191 | return; 192 | } 193 | // Set the initial solution 194 | Vector6d x = Vector6d::Zero (); 195 | x[0] = transformation_matrix (0,3); 196 | x[1] = transformation_matrix (1,3); 197 | x[2] = transformation_matrix (2,3); 198 | x[3] = std::atan2 (transformation_matrix (2,1), transformation_matrix (2,2)); 199 | x[4] = asin (-transformation_matrix (2,0)); 200 | x[5] = std::atan2 (transformation_matrix (1,0), transformation_matrix (0,0)); 201 | 202 | // Set temporary pointers 203 | tmp_src_ = &cloud_src; 204 | tmp_tgt_ = &cloud_tgt; 205 | tmp_idx_src_ = &indices_src; 206 | tmp_idx_tgt_ = &indices_tgt; 207 | 208 | // Optimize using forward-difference approximation LM 209 | const double gradient_tol = 1e-2; 210 | OptimizationFunctorWithIndices functor(this); 211 | BFGS bfgs (functor); 212 | bfgs.parameters.sigma = 0.01; 213 | bfgs.parameters.rho = 0.01; 214 | bfgs.parameters.tau1 = 9; 215 | bfgs.parameters.tau2 = 0.05; 216 | bfgs.parameters.tau3 = 0.5; 217 | bfgs.parameters.order = 3; 218 | 219 | int inner_iterations_ = 0; 220 | int result = bfgs.minimizeInit (x); 221 | result = BFGSSpace::Running; 222 | do 223 | { 224 | inner_iterations_++; 225 | result = bfgs.minimizeOneStep (x); 226 | if(result) 227 | { 228 | break; 229 | } 230 | #if PCL_VERSION_COMPARE(<, 1, 11, 0) 231 | result = bfgs.testGradient(gradient_tol); 232 | #else 233 | result = bfgs.testGradient(); 234 | #endif 235 | } while(result == BFGSSpace::Running && inner_iterations_ < max_inner_iterations_); 236 | if(result == BFGSSpace::NoProgress || result == BFGSSpace::Success || inner_iterations_ == max_inner_iterations_) 237 | { 238 | PCL_DEBUG ("[pcl::registration::TransformationEstimationBFGS::estimateRigidTransformation]"); 239 | PCL_DEBUG ("BFGS solver finished with exit code %i \n", result); 240 | transformation_matrix.setIdentity(); 241 | applyState(transformation_matrix, x); 242 | } 243 | else 244 | PCL_THROW_EXCEPTION(pcl::SolverDidntConvergeException, 245 | "[pcl::" << getClassName () << "::TransformationEstimationBFGS::estimateRigidTransformation] BFGS solver didn't converge!"); 246 | } 247 | 248 | //////////////////////////////////////////////////////////////////////////////////////// 249 | template inline double 250 | pclomp::GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::operator() (const Vector6d& x) 251 | { 252 | Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_; 253 | gicp_->applyState(transformation_matrix, x); 254 | double f = 0; 255 | std::vector f_array(omp_get_max_threads(), 0.0); 256 | 257 | int m = static_cast (gicp_->tmp_idx_src_->size ()); 258 | #pragma omp parallel for 259 | for(int i = 0; i < m; ++i) 260 | { 261 | // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp 262 | pcl::Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap(); 263 | // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp 264 | pcl::Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap(); 265 | // Estimate the distance (cost function) 266 | // The last coordinate is still guaranteed to be set to 1.0 267 | // Eigen::AlignedVector3 res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]); 268 | // Eigen::AlignedVector3 temp(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res); 269 | Eigen::Vector4f res = transformation_matrix * p_src - p_tgt; 270 | Eigen::Matrix4f maha = gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]); 271 | // Eigen::Vector4d temp(maha * res); 272 | // increment= res'*temp/num_matches = temp'*M*temp/num_matches (we postpone 1/num_matches after the loop closes) 273 | // double ret = double(res.transpose() * temp); 274 | double ret = res.dot(maha*res); 275 | f_array[omp_get_thread_num()] += ret; 276 | } 277 | f = std::accumulate(f_array.begin(), f_array.end(), 0.0); 278 | return f/m; 279 | } 280 | 281 | //////////////////////////////////////////////////////////////////////////////////////// 282 | template inline void 283 | pclomp::GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::df (const Vector6d& x, Vector6d& g) 284 | { 285 | Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_; 286 | gicp_->applyState(transformation_matrix, x); 287 | //Eigen::Vector3d g_t = g.head<3> (); 288 | std::vector> R_array(omp_get_max_threads()); 289 | std::vector> g_array(omp_get_max_threads()); 290 | 291 | for (std::size_t i = 0; i < R_array.size(); i++) { 292 | R_array[i].setZero(); 293 | g_array[i].setZero(); 294 | } 295 | 296 | int m = static_cast (gicp_->tmp_idx_src_->size ()); 297 | #pragma omp parallel for 298 | for(int i = 0; i < m; ++i) 299 | { 300 | // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp 301 | pcl::Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap(); 302 | // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp 303 | pcl::Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap(); 304 | 305 | Eigen::Vector4f pp(transformation_matrix * p_src); 306 | // The last coordinate is still guaranteed to be set to 1.0 307 | Eigen::Vector4d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2], 0.0); 308 | // temp = M*res 309 | 310 | Eigen::Matrix4d maha = gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]).template cast(); 311 | 312 | Eigen::Vector4d temp(maha * res); 313 | // Increment translation gradient 314 | // g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the loop closes) 315 | // Increment rotation gradient 316 | pp = gicp_->base_transformation_ * p_src; 317 | 318 | Eigen::Vector4d p_src3(pp[0], pp[1], pp[2], 0.0); 319 | g_array[omp_get_thread_num()] += temp; 320 | R_array[omp_get_thread_num()] += p_src3 * temp.transpose(); 321 | } 322 | 323 | g.setZero(); 324 | Eigen::Matrix4d R = Eigen::Matrix4d::Zero(); 325 | for (std::size_t i = 0; i < R_array.size(); i++) { 326 | R += R_array[i]; 327 | g.head<3>() += g_array[i].head<3>(); 328 | } 329 | 330 | g.head<3>() *= 2.0 / m; 331 | R *= 2.0 / m; 332 | 333 | gicp_->computeRDerivative(x, R.block<3, 3>(0, 0), g); 334 | } 335 | 336 | //////////////////////////////////////////////////////////////////////////////////////// 337 | template inline void 338 | pclomp::GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::fdf (const Vector6d& x, double& f, Vector6d& g) 339 | { 340 | Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_; 341 | gicp_->applyState(transformation_matrix, x); 342 | f = 0; 343 | g.setZero (); 344 | Eigen::Matrix3d R = Eigen::Matrix3d::Zero (); 345 | const auto m = static_cast (gicp_->tmp_idx_src_->size ()); 346 | for (int i = 0; i < m; ++i) 347 | { 348 | // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp 349 | pcl::Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap (); 350 | // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp 351 | pcl::Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap (); 352 | Eigen::Vector4f pp (transformation_matrix * p_src); 353 | // The last coordinate is still guaranteed to be set to 1.0 354 | Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]); 355 | // temp = M*res 356 | Eigen::Vector3d temp (gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]).template block<3, 3>(0, 0).template cast() * res); 357 | // Increment total error 358 | f+= double(res.transpose() * temp); 359 | // Increment translation gradient 360 | // g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the loop closes) 361 | g.head<3> ()+= temp; 362 | pp = gicp_->base_transformation_ * p_src; 363 | Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]); 364 | // Increment rotation gradient 365 | R+= p_src3 * temp.transpose(); 366 | } 367 | f/= double(m); 368 | g.head<3> ()*= double(2.0/m); 369 | R*= 2.0/m; 370 | gicp_->computeRDerivative(x, R, g); 371 | } 372 | 373 | //////////////////////////////////////////////////////////////////////////////////////// 374 | template inline void 375 | pclomp::GeneralizedIterativeClosestPoint::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) 376 | { 377 | pcl::IterativeClosestPoint::initComputeReciprocal (); 378 | using namespace std; 379 | // Difference between consecutive transforms 380 | double delta = 0; 381 | // Get the size of the target 382 | const size_t N = indices_->size (); 383 | // Set the mahalanobis matrices to identity 384 | mahalanobis_.resize (N, Eigen::Matrix4f::Identity ()); 385 | // Compute target cloud covariance matrices 386 | if ((!target_covariances_) || (target_covariances_->empty ())) 387 | { 388 | target_covariances_.reset (new MatricesVector); 389 | computeCovariances (target_, tree_, *target_covariances_); 390 | } 391 | // Compute input cloud covariance matrices 392 | if ((!input_covariances_) || (input_covariances_->empty ())) 393 | { 394 | input_covariances_.reset (new MatricesVector); 395 | computeCovariances (input_, tree_reciprocal_, *input_covariances_); 396 | } 397 | 398 | base_transformation_ = Eigen::Matrix4f::Identity(); 399 | nr_iterations_ = 0; 400 | converged_ = false; 401 | double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_; 402 | pcl::transformPointCloud(output, output, guess); 403 | 404 | std::vector> nn_indices_array(omp_get_max_threads()); 405 | std::vector> nn_dists_array(omp_get_max_threads()); 406 | for (auto& nn_indices : nn_indices_array) { nn_indices.resize(1); } 407 | for (auto& nn_dists : nn_dists_array) { nn_dists.resize(1); } 408 | 409 | while(!converged_) 410 | { 411 | std::atomic cnt; 412 | cnt = 0; 413 | std::vector source_indices (indices_->size ()); 414 | std::vector target_indices (indices_->size ()); 415 | 416 | // guess corresponds to base_t and transformation_ to t 417 | Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero (); 418 | for(size_t i = 0; i < 4; i++) 419 | for(size_t j = 0; j < 4; j++) 420 | for(size_t k = 0; k < 4; k++) 421 | transform_R(i,j)+= double(transformation_(i,k)) * double(guess(k,j)); 422 | 423 | const Eigen::Matrix3d R = transform_R.topLeftCorner<3,3> (); 424 | 425 | #pragma omp parallel for 426 | for (std::size_t i = 0; i < N; i++) 427 | { 428 | auto& nn_indices = nn_indices_array[omp_get_thread_num()]; 429 | auto& nn_dists = nn_dists_array[omp_get_thread_num()]; 430 | 431 | PointSource query = output[i]; 432 | query.getVector4fMap () = transformation_ * query.getVector4fMap (); 433 | 434 | if (!searchForNeighbors (query, nn_indices, nn_dists)) 435 | { 436 | 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]); 437 | continue; 438 | } 439 | 440 | // Check if the distance to the nearest neighbor is smaller than the user imposed threshold 441 | if (nn_dists[0] < dist_threshold) 442 | { 443 | const Eigen::Matrix3d &C1 = (*input_covariances_)[i]; 444 | const Eigen::Matrix3d &C2 = (*target_covariances_)[nn_indices[0]]; 445 | Eigen::Matrix4f& M_ = mahalanobis_[i]; 446 | M_.setZero(); 447 | 448 | Eigen::Matrix3d M = M_.block<3, 3>(0, 0).cast(); 449 | // M = R*C1 450 | M = R * C1; 451 | // temp = M*R' + C2 = R*C1*R' + C2 452 | Eigen::Matrix3d temp = M * R.transpose(); 453 | temp+= C2; 454 | // M = temp^-1 455 | M = temp.inverse (); 456 | M_.block<3, 3>(0, 0) = M.cast(); 457 | int c = cnt++; 458 | source_indices[c] = static_cast (i); 459 | target_indices[c] = nn_indices[0]; 460 | } 461 | } 462 | // Resize to the actual number of valid correspondences 463 | source_indices.resize(cnt); target_indices.resize(cnt); 464 | 465 | std::vector> indices(source_indices.size()); 466 | for(std::size_t i = 0; i& lhs, const std::pair& rhs) { return lhs.first < rhs.first; }); 472 | 473 | for(std::size_t i = 0; i < source_indices.size(); i++) { 474 | source_indices[i] = indices[i].first; 475 | target_indices[i] = indices[i].second; 476 | } 477 | 478 | /* optimize transformation using the current assignment and Mahalanobis metrics*/ 479 | previous_transformation_ = transformation_; 480 | //optimization right here 481 | try 482 | { 483 | rigid_transformation_estimation_(output, source_indices, *target_, target_indices, transformation_); 484 | /* compute the delta from this iteration */ 485 | delta = 0.; 486 | for(int k = 0; k < 4; k++) { 487 | for(int l = 0; l < 4; l++) { 488 | double ratio = 1; 489 | if(k < 3 && l < 3) // rotation part of the transform 490 | ratio = 1./rotation_epsilon_; 491 | else 492 | ratio = 1./transformation_epsilon_; 493 | double c_delta = ratio*std::abs(previous_transformation_(k,l) - transformation_(k,l)); 494 | if(c_delta > delta) 495 | delta = c_delta; 496 | } 497 | } 498 | } 499 | catch (pcl::PCLException &e) 500 | { 501 | PCL_DEBUG ("[pcl::%s::computeTransformation] Optimization issue %s\n", getClassName ().c_str (), e.what ()); 502 | break; 503 | } 504 | nr_iterations_++; 505 | // Check for convergence 506 | if (nr_iterations_ >= max_iterations_ || delta < 1) 507 | { 508 | converged_ = true; 509 | previous_transformation_ = transformation_; 510 | PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n", 511 | getClassName ().c_str (), nr_iterations_, max_iterations_, (transformation_ - previous_transformation_).array ().abs ().sum ()); 512 | } 513 | else 514 | PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence failed\n", getClassName ().c_str ()); 515 | } 516 | final_transformation_ = previous_transformation_ * guess; 517 | 518 | // Transform the point cloud 519 | pcl::transformPointCloud (*input_, output, final_transformation_); 520 | } 521 | 522 | template void 523 | pclomp::GeneralizedIterativeClosestPoint::applyState(Eigen::Matrix4f &t, const Vector6d& x) const 524 | { 525 | // !!! CAUTION Stanford GICP uses the Z Y X euler angles convention 526 | Eigen::Matrix3f R; 527 | R = Eigen::AngleAxisf (static_cast (x[5]), Eigen::Vector3f::UnitZ ()) 528 | * Eigen::AngleAxisf (static_cast (x[4]), Eigen::Vector3f::UnitY ()) 529 | * Eigen::AngleAxisf (static_cast (x[3]), Eigen::Vector3f::UnitX ()); 530 | t.topLeftCorner<3,3> ().matrix () = R * t.topLeftCorner<3,3> ().matrix (); 531 | Eigen::Vector4f T (static_cast (x[0]), static_cast (x[1]), static_cast (x[2]), 0.0f); 532 | t.col (3) += T; 533 | } 534 | 535 | #endif //PCL_REGISTRATION_IMPL_GICP_HPP_ 536 | -------------------------------------------------------------------------------- /include/pclomp/ndt_omp.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2010-2012, Willow Garage, Inc. 6 | * Copyright (c) 2012-, Open Perception, Inc. 7 | * 8 | * All rights reserved. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions 12 | * are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * * Neither the name of the copyright holder(s) nor the names of its 21 | * contributors may be used to endorse or promote products derived 22 | * from this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | * POSSIBILITY OF SUCH DAMAGE. 36 | * 37 | * $Id$ 38 | * 39 | */ 40 | 41 | #ifndef PCL_REGISTRATION_NDT_OMP_H_ 42 | #define PCL_REGISTRATION_NDT_OMP_H_ 43 | 44 | #include 45 | #include 46 | #include "voxel_grid_covariance_omp.h" 47 | 48 | #include 49 | 50 | namespace pclomp 51 | { 52 | enum NeighborSearchMethod { 53 | KDTREE, 54 | DIRECT26, 55 | DIRECT7, 56 | DIRECT1 57 | }; 58 | 59 | /** \brief A 3D Normal Distribution Transform registration implementation for point cloud data. 60 | * \note For more information please see 61 | * Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform — 62 | * an Efficient Representation for Registration, Surface Analysis, and Loop Detection. 63 | * PhD thesis, Orebro University. Orebro Studies in Technology 36., 64 | * More, J., and Thuente, D. (1994). Line Search Algorithm with Guaranteed Sufficient Decrease 65 | * In ACM Transactions on Mathematical Software. and 66 | * Sun, W. and Yuan, Y, (2006) Optimization Theory and Methods: Nonlinear Programming. 89-100 67 | * \note Math refactored by Todor Stoyanov. 68 | * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific) 69 | */ 70 | template 71 | class NormalDistributionsTransform : public pcl::Registration 72 | { 73 | protected: 74 | 75 | typedef typename pcl::Registration::PointCloudSource PointCloudSource; 76 | typedef typename PointCloudSource::Ptr PointCloudSourcePtr; 77 | typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr; 78 | 79 | typedef typename pcl::Registration::PointCloudTarget PointCloudTarget; 80 | typedef typename PointCloudTarget::Ptr PointCloudTargetPtr; 81 | typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr; 82 | 83 | typedef pcl::PointIndices::Ptr PointIndicesPtr; 84 | typedef pcl::PointIndices::ConstPtr PointIndicesConstPtr; 85 | 86 | /** \brief Typename of searchable voxel grid containing mean and covariance. */ 87 | typedef pclomp::VoxelGridCovariance TargetGrid; 88 | /** \brief Typename of pointer to searchable voxel grid. */ 89 | typedef TargetGrid* TargetGridPtr; 90 | /** \brief Typename of const pointer to searchable voxel grid. */ 91 | typedef const TargetGrid* TargetGridConstPtr; 92 | /** \brief Typename of const pointer to searchable voxel grid leaf. */ 93 | typedef typename TargetGrid::LeafConstPtr TargetGridLeafConstPtr; 94 | 95 | 96 | public: 97 | 98 | #if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0) 99 | typedef pcl::shared_ptr< NormalDistributionsTransform > Ptr; 100 | typedef pcl::shared_ptr< const NormalDistributionsTransform > ConstPtr; 101 | #else 102 | typedef boost::shared_ptr< NormalDistributionsTransform > Ptr; 103 | typedef boost::shared_ptr< const NormalDistributionsTransform > ConstPtr; 104 | #endif 105 | 106 | 107 | /** \brief Constructor. 108 | * Sets \ref outlier_ratio_ to 0.35, \ref step_size_ to 0.05 and \ref resolution_ to 1.0 109 | */ 110 | NormalDistributionsTransform(); 111 | 112 | /** \brief Empty destructor */ 113 | virtual ~NormalDistributionsTransform() {} 114 | 115 | void setNumThreads(int n) { 116 | num_threads_ = n; 117 | } 118 | 119 | /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to). 120 | * \param[in] cloud the input point cloud target 121 | */ 122 | inline void 123 | setInputTarget(const PointCloudTargetConstPtr &cloud) 124 | { 125 | pcl::Registration::setInputTarget(cloud); 126 | init(); 127 | } 128 | 129 | /** \brief Set/change the voxel grid resolution. 130 | * \param[in] resolution side length of voxels 131 | */ 132 | inline void 133 | setResolution(float resolution) 134 | { 135 | // Prevents unnecessary voxel initiations 136 | if (resolution_ != resolution) 137 | { 138 | resolution_ = resolution; 139 | if (input_) 140 | init(); 141 | } 142 | } 143 | 144 | /** \brief Get voxel grid resolution. 145 | * \return side length of voxels 146 | */ 147 | inline float 148 | getResolution() const 149 | { 150 | return (resolution_); 151 | } 152 | 153 | /** \brief Get the newton line search maximum step length. 154 | * \return maximum step length 155 | */ 156 | inline double 157 | getStepSize() const 158 | { 159 | return (step_size_); 160 | } 161 | 162 | /** \brief Set/change the newton line search maximum step length. 163 | * \param[in] step_size maximum step length 164 | */ 165 | inline void 166 | setStepSize(double step_size) 167 | { 168 | step_size_ = step_size; 169 | } 170 | 171 | /** \brief Get the point cloud outlier ratio. 172 | * \return outlier ratio 173 | */ 174 | inline double 175 | getOutlierRatio() const 176 | { 177 | return (outlier_ratio_); 178 | } 179 | 180 | /** \brief Set/change the point cloud outlier ratio. 181 | * \param[in] outlier_ratio outlier ratio 182 | */ 183 | inline void 184 | setOutlierRatio(double outlier_ratio) 185 | { 186 | outlier_ratio_ = outlier_ratio; 187 | } 188 | 189 | inline void setNeighborhoodSearchMethod(NeighborSearchMethod method) { 190 | search_method = method; 191 | } 192 | 193 | /** \brief Get the registration alignment probability. 194 | * \return transformation probability 195 | */ 196 | inline double 197 | getTransformationProbability() const 198 | { 199 | return (trans_probability_); 200 | } 201 | 202 | /** \brief Get the number of iterations required to calculate alignment. 203 | * \return final number of iterations 204 | */ 205 | inline int 206 | getFinalNumIteration() const 207 | { 208 | return (nr_iterations_); 209 | } 210 | 211 | /** \brief Convert 6 element transformation vector to affine transformation. 212 | * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw] 213 | * \param[out] trans affine transform corresponding to given transformation vector 214 | */ 215 | static void 216 | convertTransform(const Eigen::Matrix &x, Eigen::Affine3f &trans) 217 | { 218 | trans = Eigen::Translation(float(x(0)), float(x(1)), float(x(2))) * 219 | Eigen::AngleAxis(float(x(3)), Eigen::Vector3f::UnitX()) * 220 | Eigen::AngleAxis(float(x(4)), Eigen::Vector3f::UnitY()) * 221 | Eigen::AngleAxis(float(x(5)), Eigen::Vector3f::UnitZ()); 222 | } 223 | 224 | /** \brief Convert 6 element transformation vector to transformation matrix. 225 | * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw] 226 | * \param[out] trans 4x4 transformation matrix corresponding to given transformation vector 227 | */ 228 | static void 229 | convertTransform(const Eigen::Matrix &x, Eigen::Matrix4f &trans) 230 | { 231 | Eigen::Affine3f _affine; 232 | convertTransform(x, _affine); 233 | trans = _affine.matrix(); 234 | } 235 | 236 | // negative log likelihood function 237 | // lower is better 238 | double calculateScore(const PointCloudSource& cloud) const; 239 | 240 | protected: 241 | 242 | using pcl::Registration::reg_name_; 243 | using pcl::Registration::getClassName; 244 | using pcl::Registration::input_; 245 | using pcl::Registration::indices_; 246 | using pcl::Registration::target_; 247 | using pcl::Registration::nr_iterations_; 248 | using pcl::Registration::max_iterations_; 249 | using pcl::Registration::previous_transformation_; 250 | using pcl::Registration::final_transformation_; 251 | using pcl::Registration::transformation_; 252 | using pcl::Registration::transformation_epsilon_; 253 | using pcl::Registration::converged_; 254 | using pcl::Registration::corr_dist_threshold_; 255 | using pcl::Registration::inlier_threshold_; 256 | 257 | using pcl::Registration::update_visualizer_; 258 | 259 | /** \brief Estimate the transformation and returns the transformed source (input) as output. 260 | * \param[out] output the resultant input transformed point cloud dataset 261 | */ 262 | virtual void 263 | computeTransformation(PointCloudSource &output) 264 | { 265 | computeTransformation(output, Eigen::Matrix4f::Identity()); 266 | } 267 | 268 | /** \brief Estimate the transformation and returns the transformed source (input) as output. 269 | * \param[out] output the resultant input transformed point cloud dataset 270 | * \param[in] guess the initial gross estimation of the transformation 271 | */ 272 | virtual void 273 | computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess); 274 | 275 | /** \brief Initiate covariance voxel structure. */ 276 | void inline 277 | init() 278 | { 279 | target_cells_.setLeafSize(resolution_, resolution_, resolution_); 280 | target_cells_.setInputCloud(target_); 281 | // Initiate voxel structure. 282 | target_cells_.filter(true); 283 | } 284 | 285 | /** \brief Compute derivatives of probability function w.r.t. the transformation vector. 286 | * \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009]. 287 | * \param[out] score_gradient the gradient vector of the probability function w.r.t. the transformation vector 288 | * \param[out] hessian the hessian matrix of the probability function w.r.t. the transformation vector 289 | * \param[in] trans_cloud transformed point cloud 290 | * \param[in] p the current transform vector 291 | * \param[in] compute_hessian flag to calculate hessian, unnecessary for step calculation. 292 | */ 293 | double 294 | computeDerivatives(Eigen::Matrix &score_gradient, 295 | Eigen::Matrix &hessian, 296 | PointCloudSource &trans_cloud, 297 | Eigen::Matrix &p, 298 | bool compute_hessian = true); 299 | 300 | /** \brief Compute individual point contributions to derivatives of probability function w.r.t. the transformation vector. 301 | * \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009]. 302 | * \param[in,out] score_gradient the gradient vector of the probability function w.r.t. the transformation vector 303 | * \param[in,out] hessian the hessian matrix of the probability function w.r.t. the transformation vector 304 | * \param[in] x_trans transformed point minus mean of occupied covariance voxel 305 | * \param[in] c_inv covariance of occupied covariance voxel 306 | * \param[in] compute_hessian flag to calculate hessian, unnecessary for step calculation. 307 | */ 308 | double 309 | updateDerivatives(Eigen::Matrix &score_gradient, 310 | Eigen::Matrix &hessian, 311 | const Eigen::Matrix &point_gradient_, 312 | const Eigen::Matrix &point_hessian_, 313 | const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv, 314 | bool compute_hessian = true) const; 315 | 316 | /** \brief Precompute angular components of derivatives. 317 | * \note Equation 6.19 and 6.21 [Magnusson 2009]. 318 | * \param[in] p the current transform vector 319 | * \param[in] compute_hessian flag to calculate hessian, unnecessary for step calculation. 320 | */ 321 | void 322 | computeAngleDerivatives(Eigen::Matrix &p, bool compute_hessian = true); 323 | 324 | /** \brief Compute point derivatives. 325 | * \note Equation 6.18-21 [Magnusson 2009]. 326 | * \param[in] x point from the input cloud 327 | * \param[in] compute_hessian flag to calculate hessian, unnecessary for step calculation. 328 | */ 329 | void 330 | computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix& point_gradient_, Eigen::Matrix& point_hessian_, bool compute_hessian = true) const; 331 | 332 | void 333 | computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix& point_gradient_, Eigen::Matrix& point_hessian_, bool compute_hessian = true) const; 334 | 335 | /** \brief Compute hessian of probability function w.r.t. the transformation vector. 336 | * \note Equation 6.13 [Magnusson 2009]. 337 | * \param[out] hessian the hessian matrix of the probability function w.r.t. the transformation vector 338 | * \param[in] trans_cloud transformed point cloud 339 | * \param[in] p the current transform vector 340 | */ 341 | void 342 | computeHessian(Eigen::Matrix &hessian, 343 | PointCloudSource &trans_cloud, 344 | Eigen::Matrix &p); 345 | 346 | /** \brief Compute individual point contributions to hessian of probability function w.r.t. the transformation vector. 347 | * \note Equation 6.13 [Magnusson 2009]. 348 | * \param[in,out] hessian the hessian matrix of the probability function w.r.t. the transformation vector 349 | * \param[in] x_trans transformed point minus mean of occupied covariance voxel 350 | * \param[in] c_inv covariance of occupied covariance voxel 351 | */ 352 | void 353 | updateHessian(Eigen::Matrix &hessian, 354 | const Eigen::Matrix &point_gradient_, 355 | const Eigen::Matrix &point_hessian_, 356 | const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv) const; 357 | 358 | /** \brief Compute line search step length and update transform and probability derivatives using More-Thuente method. 359 | * \note Search Algorithm [More, Thuente 1994] 360 | * \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] 361 | * \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] 362 | * \param[in] step_init initial step length estimate, \f$ \alpha_0 \f$ in Moore-Thuente (1994) and the normal of \f$ \delta \vec{p} \f$ in Algorithm 2 [Magnusson 2009] 363 | * \param[in] step_max maximum step length, \f$ \alpha_max \f$ in Moore-Thuente (1994) 364 | * \param[in] step_min minimum step length, \f$ \alpha_min \f$ in Moore-Thuente (1994) 365 | * \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] 366 | * \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] 367 | * \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] 368 | * \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] 369 | * \return final step length 370 | */ 371 | double 372 | computeStepLengthMT(const Eigen::Matrix &x, 373 | Eigen::Matrix &step_dir, 374 | double step_init, 375 | double step_max, double step_min, 376 | double &score, 377 | Eigen::Matrix &score_gradient, 378 | Eigen::Matrix &hessian, 379 | PointCloudSource &trans_cloud); 380 | 381 | /** \brief Update interval of possible step lengths for More-Thuente method, \f$ I \f$ in More-Thuente (1994) 382 | * \note Updating Algorithm until some value satisfies \f$ \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$ 383 | * and Modified Updating Algorithm from then on [More, Thuente 1994]. 384 | * \param[in,out] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$ in Moore-Thuente (1994) 385 | * \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 386 | * \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 387 | * \param[in,out] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$ in Moore-Thuente (1994) 388 | * \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 389 | * \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 390 | * \param[in] a_t trial value, \f$ \alpha_t \f$ in Moore-Thuente (1994) 391 | * \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 392 | * \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 393 | * \return if interval converges 394 | */ 395 | bool 396 | updateIntervalMT(double &a_l, double &f_l, double &g_l, 397 | double &a_u, double &f_u, double &g_u, 398 | double a_t, double f_t, double g_t); 399 | 400 | /** \brief Select new trial value for More-Thuente method. 401 | * \note Trial Value Selection [More, Thuente 1994], \f$ \psi(\alpha_k) \f$ is used for \f$ f_k \f$ and \f$ g_k \f$ 402 | * until some value satisfies the test \f$ \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$ 403 | * then \f$ \phi(\alpha_k) \f$ is used from then on. 404 | * \note Interpolation Minimizer equations from Optimization Theory and Methods: Nonlinear Programming By Wenyu Sun, Ya-xiang Yuan (89-100). 405 | * \param[in] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$ in Moore-Thuente (1994) 406 | * \param[in] f_l value at first endpoint, \f$ f_l \f$ in Moore-Thuente (1994) 407 | * \param[in] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente (1994) 408 | * \param[in] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$ in Moore-Thuente (1994) 409 | * \param[in] f_u value at second endpoint, \f$ f_u \f$ in Moore-Thuente (1994) 410 | * \param[in] g_u derivative at second endpoint, \f$ g_u \f$ in Moore-Thuente (1994) 411 | * \param[in] a_t previous trial value, \f$ \alpha_t \f$ in Moore-Thuente (1994) 412 | * \param[in] f_t value at previous trial value, \f$ f_t \f$ in Moore-Thuente (1994) 413 | * \param[in] g_t derivative at previous trial value, \f$ g_t \f$ in Moore-Thuente (1994) 414 | * \return new trial value 415 | */ 416 | double 417 | trialValueSelectionMT(double a_l, double f_l, double g_l, 418 | double a_u, double f_u, double g_u, 419 | double a_t, double f_t, double g_t); 420 | 421 | /** \brief Auxiliary function used to determine endpoints of More-Thuente interval. 422 | * \note \f$ \psi(\alpha) \f$ in Equation 1.6 (Moore, Thuente 1994) 423 | * \param[in] a the step length, \f$ \alpha \f$ in More-Thuente (1994) 424 | * \param[in] f_a function value at step length a, \f$ \phi(\alpha) \f$ in More-Thuente (1994) 425 | * \param[in] f_0 initial function value, \f$ \phi(0) \f$ in Moore-Thuente (1994) 426 | * \param[in] g_0 initial function gradiant, \f$ \phi'(0) \f$ in More-Thuente (1994) 427 | * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More, Thuente 1994] 428 | * \return sufficient decrease value 429 | */ 430 | inline double 431 | auxiliaryFunction_PsiMT(double a, double f_a, double f_0, double g_0, double mu = 1.e-4) 432 | { 433 | return (f_a - f_0 - mu * g_0 * a); 434 | } 435 | 436 | /** \brief Auxiliary function derivative used to determine endpoints of More-Thuente interval. 437 | * \note \f$ \psi'(\alpha) \f$, derivative of Equation 1.6 (Moore, Thuente 1994) 438 | * \param[in] g_a function gradient at step length a, \f$ \phi'(\alpha) \f$ in More-Thuente (1994) 439 | * \param[in] g_0 initial function gradiant, \f$ \phi'(0) \f$ in More-Thuente (1994) 440 | * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More, Thuente 1994] 441 | * \return sufficient decrease derivative 442 | */ 443 | inline double 444 | auxiliaryFunction_dPsiMT(double g_a, double g_0, double mu = 1.e-4) 445 | { 446 | return (g_a - mu * g_0); 447 | } 448 | 449 | /** \brief The voxel grid generated from target cloud containing point means and covariances. */ 450 | TargetGrid target_cells_; 451 | 452 | //double fitness_epsilon_; 453 | 454 | /** \brief The side length of voxels. */ 455 | float resolution_; 456 | 457 | /** \brief The maximum step length. */ 458 | double step_size_; 459 | 460 | /** \brief The ratio of outliers of points w.r.t. a normal distribution, Equation 6.7 [Magnusson 2009]. */ 461 | double outlier_ratio_; 462 | 463 | /** \brief The normalization constants used fit the point distribution to a normal distribution, Equation 6.8 [Magnusson 2009]. */ 464 | double gauss_d1_, gauss_d2_, gauss_d3_; 465 | 466 | /** \brief The probability score of the transform applied to the input cloud, Equation 6.9 and 6.10 [Magnusson 2009]. */ 467 | double trans_probability_; 468 | 469 | /** \brief Precomputed Angular Gradient 470 | * 471 | * The precomputed angular derivatives for the jacobian of a transformation vector, Equation 6.19 [Magnusson 2009]. 472 | */ 473 | 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_; 474 | 475 | Eigen::Matrix j_ang; 476 | 477 | /** \brief Precomputed Angular Hessian 478 | * 479 | * The precomputed angular derivatives for the hessian of a transformation vector, Equation 6.19 [Magnusson 2009]. 480 | */ 481 | Eigen::Vector3d h_ang_a2_, h_ang_a3_, 482 | h_ang_b2_, h_ang_b3_, 483 | h_ang_c2_, h_ang_c3_, 484 | h_ang_d1_, h_ang_d2_, h_ang_d3_, 485 | h_ang_e1_, h_ang_e2_, h_ang_e3_, 486 | h_ang_f1_, h_ang_f2_, h_ang_f3_; 487 | 488 | Eigen::Matrix h_ang; 489 | 490 | /** \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]. */ 491 | // Eigen::Matrix point_gradient_; 492 | 493 | /** \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]. */ 494 | // Eigen::Matrix point_hessian_; 495 | 496 | int num_threads_; 497 | 498 | public: 499 | NeighborSearchMethod search_method; 500 | 501 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 502 | }; 503 | 504 | } 505 | 506 | #endif // PCL_REGISTRATION_NDT_H_ 507 | -------------------------------------------------------------------------------- /include/pclomp/ndt_omp_impl.hpp: -------------------------------------------------------------------------------- 1 | #include "ndt_omp.h" 2 | /* 3 | * Software License Agreement (BSD License) 4 | * 5 | * Point Cloud Library (PCL) - www.pointclouds.org 6 | * Copyright (c) 2010-2011, Willow Garage, Inc. 7 | * Copyright (c) 2012-, Open Perception, Inc. 8 | * 9 | * All rights reserved. 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions 13 | * are met: 14 | * 15 | * * Redistributions of source code must retain the above copyright 16 | * notice, this list of conditions and the following disclaimer. 17 | * * Redistributions in binary form must reproduce the above 18 | * copyright notice, this list of conditions and the following 19 | * disclaimer in the documentation and/or other materials provided 20 | * with the distribution. 21 | * * Neither the name of the copyright holder(s) nor the names of its 22 | * contributors may be used to endorse or promote products derived 23 | * from this software without specific prior written permission. 24 | * 25 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 26 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 27 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 28 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 29 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 30 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 31 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 32 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 33 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 34 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 35 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 36 | * POSSIBILITY OF SUCH DAMAGE. 37 | * 38 | * $Id$ 39 | * 40 | */ 41 | 42 | #ifndef PCL_REGISTRATION_NDT_OMP_IMPL_H_ 43 | #define PCL_REGISTRATION_NDT_OMP_IMPL_H_ 44 | 45 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 46 | template 47 | pclomp::NormalDistributionsTransform::NormalDistributionsTransform () 48 | : target_cells_ () 49 | , resolution_ (1.0f) 50 | , step_size_ (0.1) 51 | , outlier_ratio_ (0.55) 52 | , gauss_d1_ () 53 | , gauss_d2_ () 54 | , gauss_d3_ () 55 | , trans_probability_ () 56 | , j_ang_a_ (), j_ang_b_ (), j_ang_c_ (), j_ang_d_ (), j_ang_e_ (), j_ang_f_ (), j_ang_g_ (), j_ang_h_ () 57 | , h_ang_a2_ (), h_ang_a3_ (), h_ang_b2_ (), h_ang_b3_ (), h_ang_c2_ (), h_ang_c3_ (), h_ang_d1_ (), h_ang_d2_ () 58 | , h_ang_d3_ (), h_ang_e1_ (), h_ang_e2_ (), h_ang_e3_ (), h_ang_f1_ (), h_ang_f2_ (), h_ang_f3_ () 59 | { 60 | reg_name_ = "NormalDistributionsTransform"; 61 | 62 | double gauss_c1, gauss_c2; 63 | 64 | // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009] 65 | gauss_c1 = 10.0 * (1 - outlier_ratio_); 66 | gauss_c2 = outlier_ratio_ / pow (resolution_, 3); 67 | gauss_d3_ = -log (gauss_c2); 68 | gauss_d1_ = -log ( gauss_c1 + gauss_c2 ) - gauss_d3_; 69 | gauss_d2_ = -2 * log ((-log ( gauss_c1 * exp ( -0.5 ) + gauss_c2 ) - gauss_d3_) / gauss_d1_); 70 | 71 | transformation_epsilon_ = 0.1; 72 | max_iterations_ = 35; 73 | 74 | search_method = DIRECT7; 75 | num_threads_ = omp_get_max_threads(); 76 | } 77 | 78 | 79 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 80 | template void 81 | pclomp::NormalDistributionsTransform::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) 82 | { 83 | nr_iterations_ = 0; 84 | converged_ = false; 85 | 86 | double gauss_c1, gauss_c2; 87 | 88 | // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009] 89 | gauss_c1 = 10 * (1 - outlier_ratio_); 90 | gauss_c2 = outlier_ratio_ / pow (resolution_, 3); 91 | gauss_d3_ = -log (gauss_c2); 92 | gauss_d1_ = -log ( gauss_c1 + gauss_c2 ) - gauss_d3_; 93 | gauss_d2_ = -2 * log ((-log ( gauss_c1 * exp ( -0.5 ) + gauss_c2 ) - gauss_d3_) / gauss_d1_); 94 | 95 | if (guess != Eigen::Matrix4f::Identity ()) 96 | { 97 | // Initialise final transformation to the guessed one 98 | final_transformation_ = guess; 99 | // Apply guessed transformation prior to search for neighbours 100 | transformPointCloud (output, output, guess); 101 | } 102 | 103 | Eigen::Transform eig_transformation; 104 | eig_transformation.matrix () = final_transformation_; 105 | 106 | // Convert initial guess matrix to 6 element transformation vector 107 | Eigen::Matrix p, delta_p, score_gradient; 108 | Eigen::Vector3f init_translation = eig_transformation.translation (); 109 | Eigen::Vector3f init_rotation = eig_transformation.rotation ().eulerAngles (0, 1, 2); 110 | p << init_translation (0), init_translation (1), init_translation (2), 111 | init_rotation (0), init_rotation (1), init_rotation (2); 112 | 113 | Eigen::Matrix hessian; 114 | 115 | double score = 0; 116 | double delta_p_norm; 117 | 118 | // Calculate derivatives of initial transform vector, subsequent derivative calculations are done in the step length determination. 119 | score = computeDerivatives (score_gradient, hessian, output, p); 120 | 121 | while (!converged_) 122 | { 123 | // Store previous transformation 124 | previous_transformation_ = transformation_; 125 | 126 | // Solve for decent direction using newton method, line 23 in Algorithm 2 [Magnusson 2009] 127 | Eigen::JacobiSVD > sv (hessian, Eigen::ComputeFullU | Eigen::ComputeFullV); 128 | // Negative for maximization as opposed to minimization 129 | delta_p = sv.solve (-score_gradient); 130 | 131 | //Calculate step length with guaranteed sufficient decrease [More, Thuente 1994] 132 | delta_p_norm = delta_p.norm (); 133 | 134 | if (delta_p_norm == 0 || delta_p_norm != delta_p_norm) 135 | { 136 | trans_probability_ = score / static_cast (input_->points.size ()); 137 | converged_ = delta_p_norm == delta_p_norm; 138 | return; 139 | } 140 | 141 | delta_p.normalize (); 142 | delta_p_norm = computeStepLengthMT (p, delta_p, delta_p_norm, step_size_, transformation_epsilon_ / 2, score, score_gradient, hessian, output); 143 | delta_p *= delta_p_norm; 144 | 145 | 146 | transformation_ = (Eigen::Translation (static_cast (delta_p (0)), static_cast (delta_p (1)), static_cast (delta_p (2))) * 147 | Eigen::AngleAxis (static_cast (delta_p (3)), Eigen::Vector3f::UnitX ()) * 148 | Eigen::AngleAxis (static_cast (delta_p (4)), Eigen::Vector3f::UnitY ()) * 149 | Eigen::AngleAxis (static_cast (delta_p (5)), Eigen::Vector3f::UnitZ ())).matrix (); 150 | 151 | 152 | p = p + delta_p; 153 | 154 | // Update Visualizer (untested) 155 | if (update_visualizer_ != 0) 156 | update_visualizer_ (output, std::vector(), *target_, std::vector() ); 157 | 158 | if (nr_iterations_ > max_iterations_ || 159 | (nr_iterations_ && (std::fabs (delta_p_norm) < transformation_epsilon_))) 160 | { 161 | converged_ = true; 162 | } 163 | 164 | nr_iterations_++; 165 | 166 | } 167 | 168 | // Store transformation probability. The relative differences within each scan registration are accurate 169 | // but the normalization constants need to be modified for it to be globally accurate 170 | trans_probability_ = score / static_cast (input_->points.size ()); 171 | } 172 | 173 | #ifndef _OPENMP 174 | int omp_get_max_threads() { return 1; } 175 | int omp_get_thread_num() { return 0; } 176 | #endif 177 | 178 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 179 | template double 180 | pclomp::NormalDistributionsTransform::computeDerivatives(Eigen::Matrix &score_gradient, 181 | Eigen::Matrix &hessian, 182 | PointCloudSource &trans_cloud, 183 | Eigen::Matrix &p, 184 | bool compute_hessian) 185 | { 186 | score_gradient.setZero(); 187 | hessian.setZero(); 188 | double score = 0; 189 | 190 | std::vector scores(input_->points.size()); 191 | std::vector, Eigen::aligned_allocator>> score_gradients(input_->points.size()); 192 | std::vector, Eigen::aligned_allocator>> hessians(input_->points.size()); 193 | for (std::size_t i = 0; i < input_->points.size(); i++) { 194 | scores[i] = 0; 195 | score_gradients[i].setZero(); 196 | hessians[i].setZero(); 197 | } 198 | 199 | // Precompute Angular Derivatives (eq. 6.19 and 6.21)[Magnusson 2009] 200 | computeAngleDerivatives(p); 201 | 202 | std::vector> neighborhoods(num_threads_); 203 | std::vector> distancess(num_threads_); 204 | 205 | // Update gradient and hessian for each point, line 17 in Algorithm 2 [Magnusson 2009] 206 | #pragma omp parallel for num_threads(num_threads_) schedule(guided, 8) 207 | for (std::size_t idx = 0; idx < input_->points.size(); idx++) 208 | { 209 | int thread_n = omp_get_thread_num(); 210 | 211 | // Original Point and Transformed Point 212 | PointSource x_pt, x_trans_pt; 213 | // Original Point and Transformed Point (for math) 214 | Eigen::Vector3d x, x_trans; 215 | // Occupied Voxel 216 | TargetGridLeafConstPtr cell; 217 | // Inverse Covariance of Occupied Voxel 218 | Eigen::Matrix3d c_inv; 219 | 220 | // Initialize Point Gradient and Hessian 221 | Eigen::Matrix point_gradient_; 222 | Eigen::Matrix point_hessian_; 223 | point_gradient_.setZero(); 224 | point_gradient_.block<3, 3>(0, 0).setIdentity(); 225 | point_hessian_.setZero(); 226 | 227 | x_trans_pt = trans_cloud.points[idx]; 228 | 229 | auto& neighborhood = neighborhoods[thread_n]; 230 | auto& distances = distancess[thread_n]; 231 | 232 | // Find neighbors (Radius search has been experimentally faster than direct neighbor checking. 233 | switch (search_method) { 234 | case KDTREE: 235 | target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances); 236 | break; 237 | case DIRECT26: 238 | target_cells_.getNeighborhoodAtPoint(x_trans_pt, neighborhood); 239 | break; 240 | default: 241 | case DIRECT7: 242 | target_cells_.getNeighborhoodAtPoint7(x_trans_pt, neighborhood); 243 | break; 244 | case DIRECT1: 245 | target_cells_.getNeighborhoodAtPoint1(x_trans_pt, neighborhood); 246 | break; 247 | } 248 | 249 | double score_pt = 0; 250 | Eigen::Matrix score_gradient_pt = Eigen::Matrix::Zero(); 251 | Eigen::Matrix hessian_pt = Eigen::Matrix::Zero(); 252 | 253 | for (typename std::vector::iterator neighborhood_it = neighborhood.begin(); neighborhood_it != neighborhood.end(); neighborhood_it++) 254 | { 255 | cell = *neighborhood_it; 256 | x_pt = input_->points[idx]; 257 | x = Eigen::Vector3d(x_pt.x, x_pt.y, x_pt.z); 258 | 259 | x_trans = Eigen::Vector3d(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z); 260 | 261 | // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009] 262 | x_trans -= cell->getMean(); 263 | // Uses precomputed covariance for speed. 264 | c_inv = cell->getInverseCov(); 265 | 266 | // Compute derivative of transform function w.r.t. transform vector, J_E and H_E in Equations 6.18 and 6.20 [Magnusson 2009] 267 | computePointDerivatives(x, point_gradient_, point_hessian_); 268 | // Update score, gradient and hessian, lines 19-21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13, respectively [Magnusson 2009] 269 | score_pt += updateDerivatives(score_gradient_pt, hessian_pt, point_gradient_, point_hessian_, x_trans, c_inv, compute_hessian); 270 | } 271 | 272 | scores[idx] = score_pt; 273 | score_gradients[idx].noalias() = score_gradient_pt; 274 | hessians[idx].noalias() = hessian_pt; 275 | } 276 | 277 | // Ensure that the result is invariant against the summing up order 278 | for (std::size_t i = 0; i < input_->points.size(); i++) { 279 | score += scores[i]; 280 | score_gradient += score_gradients[i]; 281 | hessian += hessians[i]; 282 | } 283 | 284 | return (score); 285 | } 286 | 287 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 288 | template void 289 | pclomp::NormalDistributionsTransform::computeAngleDerivatives(Eigen::Matrix &p, bool compute_hessian) 290 | { 291 | // Simplified math for near 0 angles 292 | double cx, cy, cz, sx, sy, sz; 293 | if (fabs(p(3)) < 10e-5) 294 | { 295 | //p(3) = 0; 296 | cx = 1.0; 297 | sx = 0.0; 298 | } 299 | else 300 | { 301 | cx = cos(p(3)); 302 | sx = sin(p(3)); 303 | } 304 | if (fabs(p(4)) < 10e-5) 305 | { 306 | //p(4) = 0; 307 | cy = 1.0; 308 | sy = 0.0; 309 | } 310 | else 311 | { 312 | cy = cos(p(4)); 313 | sy = sin(p(4)); 314 | } 315 | 316 | if (fabs(p(5)) < 10e-5) 317 | { 318 | //p(5) = 0; 319 | cz = 1.0; 320 | sz = 0.0; 321 | } 322 | else 323 | { 324 | cz = cos(p(5)); 325 | sz = sin(p(5)); 326 | } 327 | 328 | // Precomputed angular gradiant components. Letters correspond to Equation 6.19 [Magnusson 2009] 329 | j_ang_a_ << (-sx * sz + cx * sy * cz), (-sx * cz - cx * sy * sz), (-cx * cy); 330 | j_ang_b_ << (cx * sz + sx * sy * cz), (cx * cz - sx * sy * sz), (-sx * cy); 331 | j_ang_c_ << (-sy * cz), sy * sz, cy; 332 | j_ang_d_ << sx * cy * cz, (-sx * cy * sz), sx * sy; 333 | j_ang_e_ << (-cx * cy * cz), cx * cy * sz, (-cx * sy); 334 | j_ang_f_ << (-cy * sz), (-cy * cz), 0; 335 | j_ang_g_ << (cx * cz - sx * sy * sz), (-cx * sz - sx * sy * cz), 0; 336 | j_ang_h_ << (sx * cz + cx * sy * sz), (cx * sy * cz - sx * sz), 0; 337 | 338 | j_ang.setZero(); 339 | j_ang.row(0).noalias() = Eigen::Vector4f((-sx * sz + cx * sy * cz), (-sx * cz - cx * sy * sz), (-cx * cy), 0.0f); 340 | j_ang.row(1).noalias() = Eigen::Vector4f((cx * sz + sx * sy * cz), (cx * cz - sx * sy * sz), (-sx * cy), 0.0f); 341 | j_ang.row(2).noalias() = Eigen::Vector4f((-sy * cz), sy * sz, cy, 0.0f); 342 | j_ang.row(3).noalias() = Eigen::Vector4f(sx * cy * cz, (-sx * cy * sz), sx * sy, 0.0f); 343 | j_ang.row(4).noalias() = Eigen::Vector4f((-cx * cy * cz), cx * cy * sz, (-cx * sy), 0.0f); 344 | j_ang.row(5).noalias() = Eigen::Vector4f((-cy * sz), (-cy * cz), 0, 0.0f); 345 | j_ang.row(6).noalias() = Eigen::Vector4f((cx * cz - sx * sy * sz), (-cx * sz - sx * sy * cz), 0, 0.0f); 346 | j_ang.row(7).noalias() = Eigen::Vector4f((sx * cz + cx * sy * sz), (cx * sy * cz - sx * sz), 0, 0.0f); 347 | 348 | if (compute_hessian) 349 | { 350 | // Precomputed angular hessian components. Letters correspond to Equation 6.21 and numbers correspond to row index [Magnusson 2009] 351 | h_ang_a2_ << (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), sx * cy; 352 | h_ang_a3_ << (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), (-cx * cy); 353 | 354 | h_ang_b2_ << (cx * cy * cz), (-cx * cy * sz), (cx * sy); 355 | h_ang_b3_ << (sx * cy * cz), (-sx * cy * sz), (sx * sy); 356 | 357 | // The sign of 'sx * sz' in c2 is incorrect in [Magnusson 2009], and it is fixed here. 358 | h_ang_c2_ << (-sx * cz - cx * sy * sz), (sx * sz - cx * sy * cz), 0; 359 | h_ang_c3_ << (cx * cz - sx * sy * sz), (-sx * sy * cz - cx * sz), 0; 360 | 361 | h_ang_d1_ << (-cy * cz), (cy * sz), (-sy); 362 | h_ang_d2_ << (-sx * sy * cz), (sx * sy * sz), (sx * cy); 363 | h_ang_d3_ << (cx * sy * cz), (-cx * sy * sz), (-cx * cy); 364 | 365 | h_ang_e1_ << (sy * sz), (sy * cz), 0; 366 | h_ang_e2_ << (-sx * cy * sz), (-sx * cy * cz), 0; 367 | h_ang_e3_ << (cx * cy * sz), (cx * cy * cz), 0; 368 | 369 | h_ang_f1_ << (-cy * cz), (cy * sz), 0; 370 | h_ang_f2_ << (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), 0; 371 | h_ang_f3_ << (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), 0; 372 | 373 | h_ang.setZero(); 374 | h_ang.row(0).noalias() = Eigen::Vector4f((-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), sx * cy, 0.0f); // a2 375 | h_ang.row(1).noalias() = Eigen::Vector4f((-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), (-cx * cy), 0.0f); // a3 376 | 377 | h_ang.row(2).noalias() = Eigen::Vector4f((cx * cy * cz), (-cx * cy * sz), (cx * sy), 0.0f); // b2 378 | h_ang.row(3).noalias() = Eigen::Vector4f((sx * cy * cz), (-sx * cy * sz), (sx * sy), 0.0f); // b3 379 | 380 | h_ang.row(4).noalias() = Eigen::Vector4f((-sx * cz - cx * sy * sz), (sx * sz - cx * sy * cz), 0, 0.0f); // c2 381 | h_ang.row(5).noalias() = Eigen::Vector4f((cx * cz - sx * sy * sz), (-sx * sy * cz - cx * sz), 0, 0.0f); // c3 382 | 383 | h_ang.row(6).noalias() = Eigen::Vector4f((-cy * cz), (cy * sz), (sy), 0.0f); // d1 384 | h_ang.row(7).noalias() = Eigen::Vector4f((-sx * sy * cz), (sx * sy * sz), (sx * cy), 0.0f); // d2 385 | h_ang.row(8).noalias() = Eigen::Vector4f((cx * sy * cz), (-cx * sy * sz), (-cx * cy), 0.0f); // d3 386 | 387 | h_ang.row(9).noalias() = Eigen::Vector4f((sy * sz), (sy * cz), 0, 0.0f); // e1 388 | h_ang.row(10).noalias() = Eigen::Vector4f ((-sx * cy * sz), (-sx * cy * cz), 0, 0.0f); // e2 389 | h_ang.row(11).noalias() = Eigen::Vector4f ((cx * cy * sz), (cx * cy * cz), 0, 0.0f); // e3 390 | 391 | h_ang.row(12).noalias() = Eigen::Vector4f ((-cy * cz), (cy * sz), 0, 0.0f); // f1 392 | h_ang.row(13).noalias() = Eigen::Vector4f ((-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), 0, 0.0f); // f2 393 | h_ang.row(14).noalias() = Eigen::Vector4f ((-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), 0, 0.0f); // f3 394 | } 395 | } 396 | 397 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 398 | template void 399 | pclomp::NormalDistributionsTransform::computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix& point_gradient_, Eigen::Matrix& point_hessian_, bool compute_hessian) const 400 | { 401 | Eigen::Vector4f x4(x[0], x[1], x[2], 0.0f); 402 | 403 | // Calculate first derivative of Transformation Equation 6.17 w.r.t. transform vector p. 404 | // Derivative w.r.t. ith element of transform vector corresponds to column i, Equation 6.18 and 6.19 [Magnusson 2009] 405 | Eigen::Matrix x_j_ang = j_ang * x4; 406 | 407 | point_gradient_(1, 3) = x_j_ang[0]; 408 | point_gradient_(2, 3) = x_j_ang[1]; 409 | point_gradient_(0, 4) = x_j_ang[2]; 410 | point_gradient_(1, 4) = x_j_ang[3]; 411 | point_gradient_(2, 4) = x_j_ang[4]; 412 | point_gradient_(0, 5) = x_j_ang[5]; 413 | point_gradient_(1, 5) = x_j_ang[6]; 414 | point_gradient_(2, 5) = x_j_ang[7]; 415 | 416 | if (compute_hessian) 417 | { 418 | Eigen::Matrix x_h_ang = h_ang * x4; 419 | 420 | // Vectors from Equation 6.21 [Magnusson 2009] 421 | Eigen::Vector4f a (0, x_h_ang[0], x_h_ang[1], 0.0f); 422 | Eigen::Vector4f b (0, x_h_ang[2], x_h_ang[3], 0.0f); 423 | Eigen::Vector4f c (0, x_h_ang[4], x_h_ang[5], 0.0f); 424 | Eigen::Vector4f d (x_h_ang[6], x_h_ang[7], x_h_ang[8], 0.0f); 425 | Eigen::Vector4f e (x_h_ang[9], x_h_ang[10], x_h_ang[11], 0.0f); 426 | Eigen::Vector4f f (x_h_ang[12], x_h_ang[13], x_h_ang[14], 0.0f); 427 | 428 | // Calculate second derivative of Transformation Equation 6.17 w.r.t. transform vector p. 429 | // 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] 430 | point_hessian_.block<4, 1>((9/3)*4, 3) = a; 431 | point_hessian_.block<4, 1>((12/3)*4, 3) = b; 432 | point_hessian_.block<4, 1>((15/3)*4, 3) = c; 433 | point_hessian_.block<4, 1>((9/3)*4, 4) = b; 434 | point_hessian_.block<4, 1>((12/3)*4, 4) = d; 435 | point_hessian_.block<4, 1>((15/3)*4, 4) = e; 436 | point_hessian_.block<4, 1>((9/3)*4, 5) = c; 437 | point_hessian_.block<4, 1>((12/3)*4, 5) = e; 438 | point_hessian_.block<4, 1>((15/3)*4, 5) = f; 439 | } 440 | } 441 | 442 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 443 | template void 444 | pclomp::NormalDistributionsTransform::computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix& point_gradient_, Eigen::Matrix& point_hessian_, bool compute_hessian) const 445 | { 446 | // Calculate first derivative of Transformation Equation 6.17 w.r.t. transform vector p. 447 | // Derivative w.r.t. ith element of transform vector corresponds to column i, Equation 6.18 and 6.19 [Magnusson 2009] 448 | point_gradient_(1, 3) = x.dot(j_ang_a_); 449 | point_gradient_(2, 3) = x.dot(j_ang_b_); 450 | point_gradient_(0, 4) = x.dot(j_ang_c_); 451 | point_gradient_(1, 4) = x.dot(j_ang_d_); 452 | point_gradient_(2, 4) = x.dot(j_ang_e_); 453 | point_gradient_(0, 5) = x.dot(j_ang_f_); 454 | point_gradient_(1, 5) = x.dot(j_ang_g_); 455 | point_gradient_(2, 5) = x.dot(j_ang_h_); 456 | 457 | if (compute_hessian) 458 | { 459 | // Vectors from Equation 6.21 [Magnusson 2009] 460 | Eigen::Vector3d a, b, c, d, e, f; 461 | 462 | a << 0, x.dot(h_ang_a2_), x.dot(h_ang_a3_); 463 | b << 0, x.dot(h_ang_b2_), x.dot(h_ang_b3_); 464 | c << 0, x.dot(h_ang_c2_), x.dot(h_ang_c3_); 465 | d << x.dot(h_ang_d1_), x.dot(h_ang_d2_), x.dot(h_ang_d3_); 466 | e << x.dot(h_ang_e1_), x.dot(h_ang_e2_), x.dot(h_ang_e3_); 467 | f << x.dot(h_ang_f1_), x.dot(h_ang_f2_), x.dot(h_ang_f3_); 468 | 469 | // Calculate second derivative of Transformation Equation 6.17 w.r.t. transform vector p. 470 | // 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] 471 | point_hessian_.block<3, 1>(9, 3) = a; 472 | point_hessian_.block<3, 1>(12, 3) = b; 473 | point_hessian_.block<3, 1>(15, 3) = c; 474 | point_hessian_.block<3, 1>(9, 4) = b; 475 | point_hessian_.block<3, 1>(12, 4) = d; 476 | point_hessian_.block<3, 1>(15, 4) = e; 477 | point_hessian_.block<3, 1>(9, 5) = c; 478 | point_hessian_.block<3, 1>(12, 5) = e; 479 | point_hessian_.block<3, 1>(15, 5) = f; 480 | } 481 | } 482 | 483 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 484 | template double 485 | pclomp::NormalDistributionsTransform::updateDerivatives(Eigen::Matrix &score_gradient, 486 | Eigen::Matrix &hessian, 487 | const Eigen::Matrix &point_gradient4, 488 | const Eigen::Matrix &point_hessian_, 489 | const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv, 490 | bool compute_hessian) const 491 | { 492 | Eigen::Matrix x_trans4( x_trans[0], x_trans[1], x_trans[2], 0.0f ); 493 | Eigen::Matrix4f c_inv4 = Eigen::Matrix4f::Zero(); 494 | c_inv4.topLeftCorner(3, 3) = c_inv.cast(); 495 | 496 | float gauss_d2 = gauss_d2_; 497 | 498 | // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009] 499 | float e_x_cov_x = exp(-gauss_d2 * x_trans4.dot(x_trans4 * c_inv4) * 0.5f); 500 | // Calculate probability of transformed points existence, Equation 6.9 [Magnusson 2009] 501 | float score_inc = -gauss_d1_ * e_x_cov_x; 502 | 503 | e_x_cov_x = gauss_d2 * e_x_cov_x; 504 | 505 | // Error checking for invalid values. 506 | if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x) 507 | return (0); 508 | 509 | // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] 510 | e_x_cov_x *= gauss_d1_; 511 | 512 | Eigen::Matrix c_inv4_x_point_gradient4 = c_inv4 * point_gradient4; 513 | Eigen::Matrix x_trans4_dot_c_inv4_x_point_gradient4 = x_trans4 * c_inv4_x_point_gradient4; 514 | 515 | score_gradient.noalias() += (e_x_cov_x * x_trans4_dot_c_inv4_x_point_gradient4).cast(); 516 | 517 | if (compute_hessian) { 518 | Eigen::Matrix x_trans4_x_c_inv4 = x_trans4 * c_inv4; 519 | Eigen::Matrix point_gradient4_colj_dot_c_inv4_x_point_gradient4_col_i = point_gradient4.transpose() * c_inv4_x_point_gradient4; 520 | Eigen::Matrix x_trans4_dot_c_inv4_x_ext_point_hessian_4ij; 521 | 522 | for (int i = 0; i < 6; i++) { 523 | // Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] 524 | // Update gradient, Equation 6.12 [Magnusson 2009] 525 | x_trans4_dot_c_inv4_x_ext_point_hessian_4ij.noalias() = x_trans4_x_c_inv4 * point_hessian_.block<4, 6>(i * 4, 0); 526 | 527 | for (int j = 0; j < hessian.cols(); j++) { 528 | // Update hessian, Equation 6.13 [Magnusson 2009] 529 | hessian(i, j) += e_x_cov_x * (-gauss_d2 * x_trans4_dot_c_inv4_x_point_gradient4(i) * x_trans4_dot_c_inv4_x_point_gradient4(j) + 530 | x_trans4_dot_c_inv4_x_ext_point_hessian_4ij(j) + 531 | point_gradient4_colj_dot_c_inv4_x_point_gradient4_col_i(j, i)); 532 | } 533 | } 534 | } 535 | 536 | return (score_inc); 537 | } 538 | 539 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 540 | template void 541 | pclomp::NormalDistributionsTransform::computeHessian (Eigen::Matrix &hessian, 542 | PointCloudSource &trans_cloud, Eigen::Matrix &) 543 | { 544 | // Original Point and Transformed Point 545 | PointSource x_pt, x_trans_pt; 546 | // Original Point and Transformed Point (for math) 547 | Eigen::Vector3d x, x_trans; 548 | // Occupied Voxel 549 | TargetGridLeafConstPtr cell; 550 | // Inverse Covariance of Occupied Voxel 551 | Eigen::Matrix3d c_inv; 552 | 553 | // Initialize Point Gradient and Hessian 554 | Eigen::Matrix point_gradient_; 555 | Eigen::Matrix point_hessian_; 556 | point_gradient_.setZero(); 557 | point_gradient_.block<3, 3>(0, 0).setIdentity(); 558 | point_hessian_.setZero(); 559 | 560 | hessian.setZero (); 561 | 562 | // Precompute Angular Derivatives unnecessary because only used after regular derivative calculation 563 | 564 | // Update hessian for each point, line 17 in Algorithm 2 [Magnusson 2009] 565 | for (size_t idx = 0; idx < input_->points.size (); idx++) 566 | { 567 | x_trans_pt = trans_cloud.points[idx]; 568 | 569 | // Find neighbors (Radius search has been experimentally faster than direct neighbor checking. 570 | std::vector neighborhood; 571 | std::vector distances; 572 | switch (search_method) { 573 | case KDTREE: 574 | target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances); 575 | break; 576 | case DIRECT26: 577 | target_cells_.getNeighborhoodAtPoint(x_trans_pt, neighborhood); 578 | break; 579 | default: 580 | case DIRECT7: 581 | target_cells_.getNeighborhoodAtPoint7(x_trans_pt, neighborhood); 582 | break; 583 | case DIRECT1: 584 | target_cells_.getNeighborhoodAtPoint1(x_trans_pt, neighborhood); 585 | break; 586 | } 587 | 588 | for (typename std::vector::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); neighborhood_it++) 589 | { 590 | cell = *neighborhood_it; 591 | 592 | { 593 | x_pt = input_->points[idx]; 594 | x = Eigen::Vector3d (x_pt.x, x_pt.y, x_pt.z); 595 | 596 | x_trans = Eigen::Vector3d (x_trans_pt.x, x_trans_pt.y, x_trans_pt.z); 597 | 598 | // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009] 599 | x_trans -= cell->getMean (); 600 | // Uses precomputed covariance for speed. 601 | c_inv = cell->getInverseCov (); 602 | 603 | // Compute derivative of transform function w.r.t. transform vector, J_E and H_E in Equations 6.18 and 6.20 [Magnusson 2009] 604 | computePointDerivatives (x, point_gradient_, point_hessian_); 605 | // Update hessian, lines 21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13, respectively [Magnusson 2009] 606 | updateHessian (hessian, point_gradient_, point_hessian_, x_trans, c_inv); 607 | } 608 | } 609 | } 610 | } 611 | 612 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 613 | template void 614 | pclomp::NormalDistributionsTransform::updateHessian (Eigen::Matrix &hessian, 615 | const Eigen::Matrix &point_gradient_, 616 | const Eigen::Matrix &point_hessian_, 617 | const Eigen::Vector3d &x_trans, 618 | const Eigen::Matrix3d &c_inv) const 619 | { 620 | Eigen::Vector3d cov_dxd_pi; 621 | // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009] 622 | double e_x_cov_x = gauss_d2_ * exp (-gauss_d2_ * x_trans.dot (c_inv * x_trans) / 2); 623 | 624 | // Error checking for invalid values. 625 | if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x) 626 | return; 627 | 628 | // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] 629 | e_x_cov_x *= gauss_d1_; 630 | 631 | for (int i = 0; i < 6; i++) 632 | { 633 | // Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] 634 | cov_dxd_pi = c_inv * point_gradient_.col (i); 635 | 636 | for (int j = 0; j < hessian.cols (); j++) 637 | { 638 | // Update hessian, Equation 6.13 [Magnusson 2009] 639 | hessian (i, j) += e_x_cov_x * (-gauss_d2_ * x_trans.dot (cov_dxd_pi) * x_trans.dot (c_inv * point_gradient_.col (j)) + 640 | x_trans.dot (c_inv * point_hessian_.block<3, 1>(3 * i, j)) + 641 | point_gradient_.col (j).dot (cov_dxd_pi) ); 642 | } 643 | } 644 | 645 | } 646 | 647 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 648 | template bool 649 | pclomp::NormalDistributionsTransform::updateIntervalMT (double &a_l, double &f_l, double &g_l, 650 | double &a_u, double &f_u, double &g_u, 651 | double a_t, double f_t, double g_t) 652 | { 653 | // Case U1 in Update Algorithm and Case a in Modified Update Algorithm [More, Thuente 1994] 654 | if (f_t > f_l) 655 | { 656 | a_u = a_t; 657 | f_u = f_t; 658 | g_u = g_t; 659 | return (false); 660 | } 661 | // Case U2 in Update Algorithm and Case b in Modified Update Algorithm [More, Thuente 1994] 662 | else 663 | if (g_t * (a_l - a_t) > 0) 664 | { 665 | a_l = a_t; 666 | f_l = f_t; 667 | g_l = g_t; 668 | return (false); 669 | } 670 | // Case U3 in Update Algorithm and Case c in Modified Update Algorithm [More, Thuente 1994] 671 | else 672 | if (g_t * (a_l - a_t) < 0) 673 | { 674 | a_u = a_l; 675 | f_u = f_l; 676 | g_u = g_l; 677 | 678 | a_l = a_t; 679 | f_l = f_t; 680 | g_l = g_t; 681 | return (false); 682 | } 683 | // Interval Converged 684 | else 685 | return (true); 686 | } 687 | 688 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 689 | template double 690 | pclomp::NormalDistributionsTransform::trialValueSelectionMT (double a_l, double f_l, double g_l, 691 | double a_u, double f_u, double g_u, 692 | double a_t, double f_t, double g_t) 693 | { 694 | // Case 1 in Trial Value Selection [More, Thuente 1994] 695 | if (f_t > f_l) 696 | { 697 | // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t 698 | // Equation 2.4.52 [Sun, Yuan 2006] 699 | double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l; 700 | double w = std::sqrt (z * z - g_t * g_l); 701 | // Equation 2.4.56 [Sun, Yuan 2006] 702 | double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w); 703 | 704 | // Calculate the minimizer of the quadratic that interpolates f_l, f_t and g_l 705 | // Equation 2.4.2 [Sun, Yuan 2006] 706 | double a_q = a_l - 0.5 * (a_l - a_t) * g_l / (g_l - (f_l - f_t) / (a_l - a_t)); 707 | 708 | if (std::fabs (a_c - a_l) < std::fabs (a_q - a_l)) 709 | return (a_c); 710 | else 711 | return (0.5 * (a_q + a_c)); 712 | } 713 | // Case 2 in Trial Value Selection [More, Thuente 1994] 714 | else 715 | if (g_t * g_l < 0) 716 | { 717 | // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t 718 | // Equation 2.4.52 [Sun, Yuan 2006] 719 | double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l; 720 | double w = std::sqrt (z * z - g_t * g_l); 721 | // Equation 2.4.56 [Sun, Yuan 2006] 722 | double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w); 723 | 724 | // Calculate the minimizer of the quadratic that interpolates f_l, g_l and g_t 725 | // Equation 2.4.5 [Sun, Yuan 2006] 726 | double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l; 727 | 728 | if (std::fabs (a_c - a_t) >= std::fabs (a_s - a_t)) 729 | return (a_c); 730 | else 731 | return (a_s); 732 | } 733 | // Case 3 in Trial Value Selection [More, Thuente 1994] 734 | else 735 | if (std::fabs (g_t) <= std::fabs (g_l)) 736 | { 737 | // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t 738 | // Equation 2.4.52 [Sun, Yuan 2006] 739 | double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l; 740 | double w = std::sqrt (z * z - g_t * g_l); 741 | double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w); 742 | 743 | // Calculate the minimizer of the quadratic that interpolates g_l and g_t 744 | // Equation 2.4.5 [Sun, Yuan 2006] 745 | double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l; 746 | 747 | double a_t_next; 748 | 749 | if (std::fabs (a_c - a_t) < std::fabs (a_s - a_t)) 750 | a_t_next = a_c; 751 | else 752 | a_t_next = a_s; 753 | 754 | if (a_t > a_l) 755 | return (std::min (a_t + 0.66 * (a_u - a_t), a_t_next)); 756 | else 757 | return (std::max (a_t + 0.66 * (a_u - a_t), a_t_next)); 758 | } 759 | // Case 4 in Trial Value Selection [More, Thuente 1994] 760 | else 761 | { 762 | // Calculate the minimizer of the cubic that interpolates f_u, f_t, g_u and g_t 763 | // Equation 2.4.52 [Sun, Yuan 2006] 764 | double z = 3 * (f_t - f_u) / (a_t - a_u) - g_t - g_u; 765 | double w = std::sqrt (z * z - g_t * g_u); 766 | // Equation 2.4.56 [Sun, Yuan 2006] 767 | return (a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w)); 768 | } 769 | } 770 | 771 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 772 | template double 773 | pclomp::NormalDistributionsTransform::computeStepLengthMT (const Eigen::Matrix &x, Eigen::Matrix &step_dir, double step_init, double step_max, 774 | double step_min, double &score, Eigen::Matrix &score_gradient, Eigen::Matrix &hessian, 775 | PointCloudSource &trans_cloud) 776 | { 777 | // Set the value of phi(0), Equation 1.3 [More, Thuente 1994] 778 | double phi_0 = -score; 779 | // Set the value of phi'(0), Equation 1.3 [More, Thuente 1994] 780 | double d_phi_0 = -(score_gradient.dot (step_dir)); 781 | 782 | Eigen::Matrix x_t; 783 | 784 | if (d_phi_0 >= 0) 785 | { 786 | // Not a decent direction 787 | if (d_phi_0 == 0) 788 | return 0; 789 | else 790 | { 791 | // Reverse step direction and calculate optimal step. 792 | d_phi_0 *= -1; 793 | step_dir *= -1; 794 | 795 | } 796 | } 797 | 798 | // The Search Algorithm for T(mu) [More, Thuente 1994] 799 | 800 | int max_step_iterations = 10; 801 | int step_iterations = 0; 802 | 803 | // Sufficient decrease constant, Equation 1.1 [More, Thuete 1994] 804 | double mu = 1.e-4; 805 | // Curvature condition constant, Equation 1.2 [More, Thuete 1994] 806 | double nu = 0.9; 807 | 808 | // Initial endpoints of Interval I, 809 | double a_l = 0, a_u = 0; 810 | 811 | // Auxiliary function psi is used until I is determined ot be a closed interval, Equation 2.1 [More, Thuente 1994] 812 | double f_l = auxiliaryFunction_PsiMT (a_l, phi_0, phi_0, d_phi_0, mu); 813 | double g_l = auxiliaryFunction_dPsiMT (d_phi_0, d_phi_0, mu); 814 | 815 | double f_u = auxiliaryFunction_PsiMT (a_u, phi_0, phi_0, d_phi_0, mu); 816 | double g_u = auxiliaryFunction_dPsiMT (d_phi_0, d_phi_0, mu); 817 | 818 | // Check used to allow More-Thuente step length calculation to be skipped by making step_min == step_max 819 | bool interval_converged = (step_max - step_min) < 0, open_interval = true; 820 | 821 | double a_t = step_init; 822 | a_t = std::min (a_t, step_max); 823 | a_t = std::max (a_t, step_min); 824 | 825 | x_t = x + step_dir * a_t; 826 | 827 | final_transformation_ = (Eigen::Translation(static_cast (x_t (0)), static_cast (x_t (1)), static_cast (x_t (2))) * 828 | Eigen::AngleAxis (static_cast (x_t (3)), Eigen::Vector3f::UnitX ()) * 829 | Eigen::AngleAxis (static_cast (x_t (4)), Eigen::Vector3f::UnitY ()) * 830 | Eigen::AngleAxis (static_cast (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix (); 831 | 832 | // New transformed point cloud 833 | transformPointCloud (*input_, trans_cloud, final_transformation_); 834 | 835 | // Updates score, gradient and hessian. Hessian calculation is unnecessary but testing showed that most step calculations use the 836 | // initial step suggestion and recalculation the reusable portions of the hessian would intail more computation time. 837 | score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, true); 838 | 839 | // Calculate phi(alpha_t) 840 | double phi_t = -score; 841 | // Calculate phi'(alpha_t) 842 | double d_phi_t = -(score_gradient.dot (step_dir)); 843 | 844 | // Calculate psi(alpha_t) 845 | double psi_t = auxiliaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu); 846 | // Calculate psi'(alpha_t) 847 | double d_psi_t = auxiliaryFunction_dPsiMT (d_phi_t, d_phi_0, mu); 848 | 849 | // Iterate until max number of iterations, interval convergence or a value satisfies the sufficient decrease, Equation 1.1, and curvature condition, Equation 1.2 [More, Thuente 1994] 850 | while (!interval_converged && step_iterations < max_step_iterations && !(psi_t <= 0 /*Sufficient Decrease*/ && d_phi_t <= -nu * d_phi_0 /*Curvature Condition*/)) 851 | { 852 | // Use auxiliary function if interval I is not closed 853 | if (open_interval) 854 | { 855 | a_t = trialValueSelectionMT (a_l, f_l, g_l, 856 | a_u, f_u, g_u, 857 | a_t, psi_t, d_psi_t); 858 | } 859 | else 860 | { 861 | a_t = trialValueSelectionMT (a_l, f_l, g_l, 862 | a_u, f_u, g_u, 863 | a_t, phi_t, d_phi_t); 864 | } 865 | 866 | a_t = std::min (a_t, step_max); 867 | a_t = std::max (a_t, step_min); 868 | 869 | x_t = x + step_dir * a_t; 870 | 871 | final_transformation_ = (Eigen::Translation (static_cast (x_t (0)), static_cast (x_t (1)), static_cast (x_t (2))) * 872 | Eigen::AngleAxis (static_cast (x_t (3)), Eigen::Vector3f::UnitX ()) * 873 | Eigen::AngleAxis (static_cast (x_t (4)), Eigen::Vector3f::UnitY ()) * 874 | Eigen::AngleAxis (static_cast (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix (); 875 | 876 | // New transformed point cloud 877 | // Done on final cloud to prevent wasted computation 878 | transformPointCloud (*input_, trans_cloud, final_transformation_); 879 | 880 | // Updates score, gradient. Values stored to prevent wasted computation. 881 | score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, false); 882 | 883 | // Calculate phi(alpha_t+) 884 | phi_t = -score; 885 | // Calculate phi'(alpha_t+) 886 | d_phi_t = -(score_gradient.dot (step_dir)); 887 | 888 | // Calculate psi(alpha_t+) 889 | psi_t = auxiliaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu); 890 | // Calculate psi'(alpha_t+) 891 | d_psi_t = auxiliaryFunction_dPsiMT (d_phi_t, d_phi_0, mu); 892 | 893 | // Check if I is now a closed interval 894 | if (open_interval && (psi_t <= 0 && d_psi_t >= 0)) 895 | { 896 | open_interval = false; 897 | 898 | // Converts f_l and g_l from psi to phi 899 | f_l = f_l + phi_0 - mu * d_phi_0 * a_l; 900 | g_l = g_l + mu * d_phi_0; 901 | 902 | // Converts f_u and g_u from psi to phi 903 | f_u = f_u + phi_0 - mu * d_phi_0 * a_u; 904 | g_u = g_u + mu * d_phi_0; 905 | } 906 | 907 | if (open_interval) 908 | { 909 | // Update interval end points using Updating Algorithm [More, Thuente 1994] 910 | interval_converged = updateIntervalMT (a_l, f_l, g_l, 911 | a_u, f_u, g_u, 912 | a_t, psi_t, d_psi_t); 913 | } 914 | else 915 | { 916 | // Update interval end points using Modified Updating Algorithm [More, Thuente 1994] 917 | interval_converged = updateIntervalMT (a_l, f_l, g_l, 918 | a_u, f_u, g_u, 919 | a_t, phi_t, d_phi_t); 920 | } 921 | 922 | step_iterations++; 923 | } 924 | 925 | // If inner loop was run then hessian needs to be calculated. 926 | // Hessian is unnecessary for step length determination but gradients are required 927 | // so derivative and transform data is stored for the next iteration. 928 | if (step_iterations) 929 | computeHessian (hessian, trans_cloud, x_t); 930 | 931 | return (a_t); 932 | } 933 | 934 | 935 | template 936 | double pclomp::NormalDistributionsTransform::calculateScore(const PointCloudSource & trans_cloud) const 937 | { 938 | double score = 0; 939 | 940 | for (std::size_t idx = 0; idx < trans_cloud.points.size(); idx++) 941 | { 942 | PointSource x_trans_pt = trans_cloud.points[idx]; 943 | 944 | // Find neighbors (Radius search has been experimentally faster than direct neighbor checking. 945 | std::vector neighborhood; 946 | std::vector distances; 947 | switch (search_method) { 948 | case KDTREE: 949 | target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances); 950 | break; 951 | case DIRECT26: 952 | target_cells_.getNeighborhoodAtPoint(x_trans_pt, neighborhood); 953 | break; 954 | default: 955 | case DIRECT7: 956 | target_cells_.getNeighborhoodAtPoint7(x_trans_pt, neighborhood); 957 | break; 958 | case DIRECT1: 959 | target_cells_.getNeighborhoodAtPoint1(x_trans_pt, neighborhood); 960 | break; 961 | } 962 | 963 | for (typename std::vector::iterator neighborhood_it = neighborhood.begin(); neighborhood_it != neighborhood.end(); neighborhood_it++) 964 | { 965 | TargetGridLeafConstPtr cell = *neighborhood_it; 966 | 967 | Eigen::Vector3d x_trans = Eigen::Vector3d(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z); 968 | 969 | // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009] 970 | x_trans -= cell->getMean(); 971 | // Uses precomputed covariance for speed. 972 | Eigen::Matrix3d c_inv = cell->getInverseCov(); 973 | 974 | // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009] 975 | double e_x_cov_x = exp(-gauss_d2_ * x_trans.dot(c_inv * x_trans) / 2); 976 | // Calculate probability of transformed points existence, Equation 6.9 [Magnusson 2009] 977 | double score_inc = -gauss_d1_ * e_x_cov_x - gauss_d3_; 978 | 979 | score += score_inc / neighborhood.size(); 980 | } 981 | } 982 | return (score) / static_cast (trans_cloud.size()); 983 | } 984 | 985 | #endif // PCL_REGISTRATION_NDT_IMPL_H_ 986 | --------------------------------------------------------------------------------