Merge branch 'tier4/main' into refactor/multigrid_ndt_omp

.github/workflows/build.yml

@@ -16,21 +16,25 @@ jobs:
     runs-on: ubuntu-latest
     strategy:
       matrix:
-        ROS_DISTRO: [melodic, melodic_llvm, noetic, noetic_llvm, foxy, foxy_llvm, galactic, galactic_llvm]
+        ROS_DISTRO: [foxy, galactic, humble]
 
     steps:
       - uses: actions/checkout@v2
 
-      - name: Docker login
-        continue-on-error: true
-        uses: docker/login-action@v1
+      - name: Docker build gcc
+        uses: docker/build-push-action@v2
         with:
-          username: ${{ secrets.DOCKER_USERNAME }}
-          password: ${{ secrets.DOCKER_TOKEN }}
+          file: ${{github.workspace}}/docker/Dockerfile_gcc
+          context: .
+          push: false
+          build-args: |
+            ROS_DISTRO=${{ matrix.ROS_DISTRO }}
 
-      - name: Docker build
+      - name: Docker build llvm
         uses: docker/build-push-action@v2
         with:
-          file: ${{github.workspace}}/docker/${{ matrix.ROS_DISTRO }}/Dockerfile
+          file: ${{github.workspace}}/docker/Dockerfile_llvm
           context: .
           push: false
+          build-args: |
+            ROS_DISTRO=${{ matrix.ROS_DISTRO }}

CMakeLists.txt

@@ -1,8 +1,8 @@
 cmake_minimum_required(VERSION 3.5)
 project(ndt_omp)
 
-add_definitions(-std=c++14)
-set(CMAKE_CXX_FLAGS "-std=c++14")
+add_definitions(-std=c++17)
+set(CMAKE_CXX_FLAGS "-std=c++17")
 
 # Compile flags for SIMD instructions
 if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "x86_64")
@@ -20,78 +20,6 @@ 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
-  src/pclomp/gicp_omp.cpp
-)
-
-add_executable(align
-  apps/align.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()
@@ -155,5 +83,3 @@ target_link_libraries(regression_test
   ${PCL_LIBRARIES}
   ndt_omp multigrid_ndt_omp
 )
-
-endif()

docker/foxy/Dockerfile → docker/Dockerfile_gcc

@@ -1,9 +1,11 @@
-FROM ros:foxy
+ARG ROS_DISTRO
+
+FROM ros:${ROS_DISTRO}
 
 RUN apt-get update && apt-get install -y --no-install-recommends \
   wget nano build-essential libomp-dev clang lld git\
-  ros-foxy-geodesy ros-foxy-pcl-ros ros-foxy-nmea-msgs \
-  ros-foxy-rviz2 ros-foxy-libg2o \
+  ros-${ROS_DISTRO}-geodesy ros-${ROS_DISTRO}-pcl-ros ros-${ROS_DISTRO}-nmea-msgs \
+  ros-${ROS_DISTRO}-rviz2 ros-${ROS_DISTRO}-libg2o \
   && apt-get clean \
   && rm -rf /var/lib/apt/lists/*
 
@@ -16,7 +18,7 @@ WORKDIR /root/colcon_ws/src
 COPY . /root/colcon_ws/src/ndt_omp/
 
 WORKDIR /root/colcon_ws
-RUN /bin/bash -c '. /opt/ros/foxy/setup.bash; colcon build'
+RUN /bin/bash -c '. /opt/ros/${ROS_DISTRO}/setup.bash; colcon build'
 RUN sed -i "6i source \"/root/colcon_ws/devel/setup.bash\"" /ros_entrypoint.sh
 
 WORKDIR /

docker/foxy_llvm/Dockerfile → docker/Dockerfile_llvm

@@ -1,9 +1,11 @@
-FROM ros:foxy
+ARG ROS_DISTRO
+
+FROM ros:${ROS_DISTRO}
 
 RUN apt-get update && apt-get install -y --no-install-recommends \
   wget nano build-essential libomp-dev clang lld git\
-  ros-foxy-geodesy ros-foxy-pcl-ros ros-foxy-nmea-msgs \
-  ros-foxy-rviz2 ros-foxy-libg2o \
+  ros-${ROS_DISTRO}-geodesy ros-${ROS_DISTRO}-pcl-ros ros-${ROS_DISTRO}-nmea-msgs \
+  ros-${ROS_DISTRO}-rviz2 ros-${ROS_DISTRO}-libg2o \
   && apt-get clean \
   && rm -rf /var/lib/apt/lists/*
 
@@ -18,7 +20,7 @@ COPY . /root/colcon_ws/src/ndt_omp/
 RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld 50
 
 WORKDIR /root/colcon_ws
-RUN /bin/bash -c '. /opt/ros/foxy/setup.bash; CC=clang CXX=clang++ colcon build'
+RUN /bin/bash -c '. /opt/ros/${ROS_DISTRO}/setup.bash; CC=clang CXX=clang++ colcon build'
 RUN sed -i "6i source \"/root/colcon_ws/devel/setup.bash\"" /ros_entrypoint.sh
 
 WORKDIR /

include/multigrid_pclomp/multigrid_ndt_omp.h

@@ -60,40 +60,11 @@
 #include <pcl/search/impl/search.hpp>
 #include <pcl/registration/registration.h>
 #include "multigrid_pclomp/multi_voxel_grid_covariance_omp.h"
+#include "../pclomp/ndt_struct.hpp"
 
 #include <unsupported/Eigen/NonLinearOptimization>
 
 namespace pclomp {
-enum NeighborSearchMethod { KDTREE, DIRECT26, DIRECT7, DIRECT1 };
-
-struct NdtResult {
-  Eigen::Matrix4f pose;
-  float transform_probability;
-  float nearest_voxel_transformation_likelihood;
-  int iteration_num;
-  Eigen::Matrix<double, 6, 6> hessian;
-  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> transformation_array;
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-  friend std::ostream &operator<<(std::ostream &os, const NdtResult &val) {
-    os << "Pose: " << std::endl << val.pose << std::endl;
-    os << "TP: " << val.transform_probability << std::endl;
-    os << "NVTP: " << val.nearest_voxel_transformation_likelihood << std::endl;
-    os << "Iteration num: " << val.iteration_num << std::endl;
-    os << "Hessian: " << std::endl << val.hessian << std::endl;
-
-    return os;
-  }
-};
-
-struct NdtParams {
-  double trans_epsilon;
-  double step_size;
-  double resolution;
-  int max_iterations;
-  int num_threads;
-  float regularization_scale_factor;
-};
 
 /** \brief A 3D Normal Distribution Transform registration implementation for point cloud data.
  * \note For more information please see