From 9f105ec3549d1b95f9905fa4fc27fbb8f6aee3fc Mon Sep 17 00:00:00 2001 From: Nikolai Morin Date: Tue, 20 Oct 2020 07:53:54 +0200 Subject: [PATCH] Port ndt_omp to ROS2 (#20) * Remove COLCON_IGNORE * CMakeLists.txt & package.xml * Compiles * Remove rclcpp * Cleanup * Fix package.xml * Better CMakeLists.txt * Update CMakeLists.txt again * Update CMakeLists.txt * Update CMakeLists.txt yet again * Simplify CMakeLists.txt * Last (?) CMakeLists.txt change --- localization/ndt_omp/CMakeLists.txt | 135 ++++++++++++++++++---------- localization/ndt_omp/COLCON_IGNORE | 0 localization/ndt_omp/apps/align.cpp | 17 ++-- localization/ndt_omp/package.xml | 11 ++- 4 files changed, 100 insertions(+), 63 deletions(-) delete mode 100644 localization/ndt_omp/COLCON_IGNORE diff --git a/localization/ndt_omp/CMakeLists.txt b/localization/ndt_omp/CMakeLists.txt index 0679712b91c48..aaea42f2917e8 100644 --- a/localization/ndt_omp/CMakeLists.txt +++ b/localization/ndt_omp/CMakeLists.txt @@ -1,70 +1,111 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.9) project(ndt_omp) -# -mavx causes a lot of errors!! -add_definitions(-std=c++14 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2) -set(CMAKE_CXX_FLAGS "-std=c++14 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2") - -# warn performance issue of ndt_omp which is NOT built in release mode -string(TOUPPER ${CMAKE_BUILD_TYPE} uppercase_CMAKE_BUILD_TYPE) -if (NOT (${uppercase_CMAKE_BUILD_TYPE} STREQUAL "RELEASE")) - message(WARNING - "CMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} may cause a serious performance degradation. Please configure with -DCMAKE_BUILD_TYPE=Release.") +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wno-unused-parameter) endif() -find_package(catkin REQUIRED COMPONENTS - roscpp - pcl_ros -) +set(CMAKE_BUILD_TYPE "Release") +find_package(ament_cmake REQUIRED) +find_package(PCL REQUIRED COMPONENTS common filters kdtree registration io) 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_package( - INCLUDE_DIRS include - LIBRARIES ndt_omp - CATKIN_DEPENDS -) ########### ## Build ## ########### -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - add_library(ndt_omp src/ndt_omp/voxel_grid_covariance_omp.cpp src/ndt_omp/ndt_omp.cpp ) -add_executable(align - apps/align.cpp -) -add_dependencies(align - ndt_omp +target_include_directories(ndt_omp + PUBLIC + $ + $ ) -# pcl 1.7 causes a segfault without -O2 (or -O3) flag -target_compile_options(ndt_omp PUBLIC - $<$:-O2 -g> + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + target_compile_options(ndt_omp + PRIVATE + # -mavx causes a lot of errors!! + -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2 + ) +endif() + +set(NDT_OMP_DEPENDENCIES + PCL + OpenMP ) -target_link_libraries(align - ${catkin_LIBRARIES} - ndt_omp + + +# Naively, I'd want to do something like: +# ament_target_dependencies(ndt_omp PUBLIC ${NDT_OMP_DEPENDENCIES}) +# But there are several problems with that. For one, you'll have a hundred +# errors like "undefined reference to `pcl::something::Something`". That's +# because it only links `libpcl_common.so` for whatever reason, but we also +# want `libpcl_io.so` etc. So we do this PCL thing manually: +target_compile_definitions(ndt_omp PUBLIC ${PCL_DEFINITIONS}) +target_include_directories(ndt_omp PUBLIC ${PCL_INCLUDE_DIRS}) +target_link_libraries(ndt_omp PUBLIC ${PCL_LIBRARIES}) +target_link_directories(ndt_omp PUBLIC ${PCL_LIBRARY_DIRS}) + +# As for OpenMP, the recommended format is the following, which is not +# supported by `ament_target_dependencies()` either. +if(OpenMP_CXX_FOUND) + target_link_libraries(ndt_omp PUBLIC OpenMP::OpenMP_CXX) +else() + message(WARNING "OpenMP not found") +endif() +# See https://cliutils.gitlab.io/modern-cmake/chapters/packages/OpenMP.html +# but it doesn't matter anyway because at the time of writing, omp.h is in +# /usr/lib/gcc/x86_64-linux-gnu/9/include/ which is one of the default +# include directories, and libgomp.so is in /usr/lib/x86_64-linux-gnu/, +# which is one of the default linker search paths. + +ament_export_targets(export_ndt_omp HAS_LIBRARY_TARGET) +ament_export_dependencies(${NDT_OMP_DEPENDENCIES}) + + +# Same story for the executable +add_executable(align + apps/align.cpp ) -install(TARGETS ndt_omp - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +target_compile_definitions(align PRIVATE ${PCL_DEFINITIONS}) +target_include_directories(align PRIVATE ${PCL_INCLUDE_DIRS}) +target_link_libraries(align PRIVATE ${PCL_LIBRARIES}) +target_link_directories(align PRIVATE ${PCL_LIBRARY_DIRS}) +if(OpenMP_CXX_FOUND) + target_link_libraries(align PRIVATE OpenMP::OpenMP_CXX) +else() + message(WARNING "OpenMP not found") +endif() + +# Again, no chance for `ament_target_dependencies()` since you can't depend on +# a local library target: "ament_target_dependencies() the passed package name +# 'ndt_omp' was not found" – so we need to add ndt_omp with +# `target_link_libraries(align PUBLIC ndt_omp)` instead. +target_link_libraries(align PRIVATE ndt_omp) + +install( + DIRECTORY include/ + DESTINATION include ) -## Install project namespaced headers -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install( + TARGETS ndt_omp + EXPORT export_ndt_omp + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include ) + +ament_package() + diff --git a/localization/ndt_omp/COLCON_IGNORE b/localization/ndt_omp/COLCON_IGNORE deleted file mode 100644 index e69de29bb2d1d..0000000000000 diff --git a/localization/ndt_omp/apps/align.cpp b/localization/ndt_omp/apps/align.cpp index 05b2c3ee6b690..0cc56f2da7f98 100644 --- a/localization/ndt_omp/apps/align.cpp +++ b/localization/ndt_omp/apps/align.cpp @@ -26,13 +26,13 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * */ +#include +#include #include #include #include #include #include -#include -#include #include @@ -45,17 +45,16 @@ pcl::PointCloud::Ptr align( registration->setInputTarget(target_cloud); registration->setInputSource(source_cloud); pcl::PointCloud::Ptr aligned(new pcl::PointCloud()); - - auto t1 = ros::WallTime::now(); + auto t1 = std::chrono::steady_clock::now(); registration->align(*aligned); - auto t2 = ros::WallTime::now(); - std::cout << "single : " << (t2 - t1).toSec() * 1000 << "[msec]" << std::endl; + auto t2 = std::chrono::steady_clock::now(); + std::cout << "single : " << std::chrono::duration_cast(t2 - t1).count() << "[msec]" << std::endl; for (int i = 0; i < 10; i++) { registration->align(*aligned); } - auto t3 = ros::WallTime::now(); - std::cout << "10times: " << (t3 - t2).toSec() * 1000 << "[msec]" << std::endl; + auto t3 = std::chrono::steady_clock::now(); + std::cout << "10times: " << std::chrono::duration_cast(t3 - t2).count() << "[msec]" << std::endl; std::cout << "fitness: " << registration->getFitnessScore() << std::endl << std::endl; return aligned; @@ -97,8 +96,6 @@ int main(int argc, char ** argv) voxelgrid.filter(*downsampled); source_cloud = downsampled; - ros::Time::init(); - // benchmark std::cout << "--- pcl::NDT ---" << std::endl; pcl::NormalDistributionsTransform::Ptr ndt( diff --git a/localization/ndt_omp/package.xml b/localization/ndt_omp/package.xml index fd86562670612..45ce9b6185dfc 100644 --- a/localization/ndt_omp/package.xml +++ b/localization/ndt_omp/package.xml @@ -1,5 +1,6 @@ - + + ndt_omp 0.1.0 OpenMP boosted NDT and GICP algorithms @@ -11,12 +12,10 @@ BSD - catkin - pcl_ros - roscpp - pcl_ros - roscpp + ament_cmake + libpcl-all-dev + ament_cmake