feat(ndt_omp): migrate a package into universe

build_depends.humble.repos

@@ -33,10 +33,6 @@ repositories:
     type: git
     url: https://github.com/tier4/muSSP.git
     version: tier4/main
-  universe/external/ndt_omp:
-    type: git
-    url: https://github.com/tier4/ndt_omp.git
-    version: tier4/main
   universe/external/morai_msgs:
     type: git
     url: https://github.com/MORAI-Autonomous/MORAI-ROS2_morai_msgs.git

build_depends.repos

@@ -33,10 +33,6 @@ repositories:
     type: git
     url: https://github.com/tier4/muSSP.git
     version: tier4/main
-  universe/external/ndt_omp:
-    type: git
-    url: https://github.com/tier4/ndt_omp.git
-    version: tier4/main
   universe/external/morai_msgs:
     type: git
     url: https://github.com/MORAI-Autonomous/MORAI-ROS2_morai_msgs.git

localization/ndt_omp/CMakeLists.txt

@@ -0,0 +1,124 @@
+cmake_minimum_required(VERSION 3.5)
+project(ndt_omp)
+
+add_definitions(-std=c++14)
+set(CMAKE_CXX_FLAGS "-std=c++14")
+
+# Compile flags for SIMD instructions
+if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "x86_64")
+  # For x86_64 architecture, SIMD instruction set is fixed below versions,
+  add_compile_options(-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2)
+else()
+  # For other architecture, like arm64, compile flags are generally prepared by compiler
+  # march=native is disabled as default for specific depending pcl libraries
+  # or pre-building packages for other computers.
+  if(BUILD_WITH_MARCH_NATIVE)
+    add_compile_options(-march=native)
+  endif()
+endif()
+
+# pcl 1.7 causes a segfault when it is built with debug mode
+set(CMAKE_BUILD_TYPE "RELEASE")
+
+if($ENV{ROS_VERSION} EQUAL 1)
+# ROS1
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  pcl_ros
+)
+
+find_package(PCL 1.7 REQUIRED)
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+message(STATUS "PCL_INCLUDE_DIRS:" ${PCL_INCLUDE_DIRS})
+message(STATUS "PCL_LIBRARY_DIRS:" ${PCL_LIBRARY_DIRS})
+message(STATUS "PCL_DEFINITIONS:" ${PCL_DEFINITIONS})
+
+find_package(OpenMP)
+if (OPENMP_FOUND)
+    set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+    set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+endif()
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ndt_omp
+)
+
+###########
+## Build ##
+###########
+include_directories(include)
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_library(ndt_omp
+  src/pclomp/voxel_grid_covariance_omp.cpp
+  src/pclomp/ndt_omp.cpp
+)
+
+add_dependencies(align
+  ndt_omp
+)
+target_link_libraries(align
+  ${catkin_LIBRARIES}
+  ${PCL_LIBRARIES}
+  ndt_omp
+)
+
+#############
+## INSTALL ##
+#############
+
+install(
+  TARGETS
+    ndt_omp
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+
+# install headers
+install(DIRECTORY include/pclomp
+        DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
+else()
+# ROS2
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+find_package(PCL 1.7 REQUIRED)
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+message(STATUS "PCL_INCLUDE_DIRS:" ${PCL_INCLUDE_DIRS})
+message(STATUS "PCL_LIBRARY_DIRS:" ${PCL_LIBRARY_DIRS})
+message(STATUS "PCL_DEFINITIONS:" ${PCL_DEFINITIONS})
+
+find_package(OpenMP)
+
+###########
+## Build ##
+###########
+
+ament_auto_add_library(ndt_omp SHARED
+  src/pclomp/voxel_grid_covariance_omp.cpp
+  src/pclomp/ndt_omp.cpp
+)
+
+target_link_libraries(ndt_omp ${PCL_LIBRARIES})
+
+if(OpenMP_CXX_FOUND)
+  target_link_libraries(ndt_omp OpenMP::OpenMP_CXX)
+else()
+  message(WARNING "OpenMP not found")
+endif()
+
+ament_auto_package()
+endif()

localization/ndt_omp/LICENSE

@@ -0,0 +1,25 @@
+BSD 2-Clause License
+
+Copyright (c) 2019, k.koide
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

localization/ndt_omp/README.md

@@ -0,0 +1,94 @@
+# ndt_omp
+
+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.
+
+NOTE: This package's implementation is a customized version of [ndt_omp from koide3](https://github.com/koide3/ndt_omp) for Autoware.
+
+[![Build](https://github.com/koide3/ndt_omp/actions/workflows/build.yml/badge.svg)](https://github.com/koide3/ndt_omp/actions/workflows/build.yml)
+
+## Benchmark (on Core i7-6700K)
+
+```bash
+$ roscd ndt_omp/data
+$ rosrun ndt_omp align 251370668.pcd 251371071.pcd
+--- pcl::NDT ---
+single : 282.222[msec]
+10times: 2921.92[msec]
+fitness: 0.213937
+
+--- pclomp::NDT (KDTREE, 1 threads) ---
+single : 207.697[msec]
+10times: 2059.19[msec]
+fitness: 0.213937
+
+--- pclomp::NDT (DIRECT7, 1 threads) ---
+single : 139.433[msec]
+10times: 1356.79[msec]
+fitness: 0.214205
+
+--- pclomp::NDT (DIRECT1, 1 threads) ---
+single : 34.6418[msec]
+10times: 317.03[msec]
+fitness: 0.208511
+
+--- pclomp::NDT (KDTREE, 8 threads) ---
+single : 54.9903[msec]
+10times: 500.51[msec]
+fitness: 0.213937
+
+--- pclomp::NDT (DIRECT7, 8 threads) ---
+single : 63.1442[msec]
+10times: 343.336[msec]
+fitness: 0.214205
+
+--- pclomp::NDT (DIRECT1, 8 threads) ---
+single : 17.2353[msec]
+10times: 100.025[msec]
+fitness: 0.208511
+```
+
+Several methods for neighbor voxel search are implemented. If you select pclomp::KDTREE, results will be completely same as the original pcl::NDT. We recommend to use pclomp::DIRECT7 which is faster and stable. If you need extremely fast registration, choose pclomp::DIRECT1, but it might be a bit unstable.
+
+<img src="data/screenshot.png" height="400pix" /><br>
+Red: target, Green: source, Blue: aligned
+
+## Related packages
+
+- [ndt_omp](https://github.com/koide3/ndt_omp)
+- [fast_gicp](https://github.com/SMRT-AIST/fast_gicp)
+
+## License
+
+A license from [koide3/ndt_omp](https://github.com/koide3/ndt_omp):
+
+```markdown
+BSD 2-Clause License
+
+Copyright (c) 2019, k.koide
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+```
+
+## Special thanks
+
+- [koide3/ndt_omp](https://github.com/koide3/ndt_omp)