refactor(ndt_omp): prefix with autoware_

Signed-off-by: Esteve Fernandez <esteve.fernandez@tier4.jp>

CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.5)
-project(ndt_omp)
+project(autoware_ndt_omp)
 
 add_definitions(-std=c++17)
 set(CMAKE_CXX_FLAGS "-std=c++17")
@@ -39,31 +39,31 @@ find_package(OpenMP)
 ## Build ##
 ###########
 
-ament_auto_add_library(ndt_omp SHARED
+ament_auto_add_library(autoware_ndt_omp SHARED
   src/pclomp/voxel_grid_covariance_omp.cpp
   src/pclomp/ndt_omp.cpp
   src/pclomp/gicp_omp.cpp
 )
 
-target_link_libraries(ndt_omp ${PCL_LIBRARIES})
+target_link_libraries(autoware_ndt_omp ${PCL_LIBRARIES})
 
 if(OpenMP_CXX_FOUND)
-  target_link_libraries(ndt_omp OpenMP::OpenMP_CXX)
+  target_link_libraries(autoware_ndt_omp OpenMP::OpenMP_CXX)
 else()
   message(WARNING "OpenMP not found")
 endif()
 
 #############################
 ## ROS 2 multigrid ndt_omp ##
 #############################
-ament_auto_add_library(multigrid_ndt_omp SHARED
+ament_auto_add_library(autoware_multigrid_ndt_omp SHARED
   src/multigrid_pclomp/multi_voxel_grid_covariance_omp.cpp
   src/multigrid_pclomp/multigrid_ndt_omp.cpp
   src/estimate_covariance/estimate_covariance.cpp
 )
-target_link_libraries(multigrid_ndt_omp ${PCL_LIBRARIES})
+target_link_libraries(autoware_multigrid_ndt_omp ${PCL_LIBRARIES})
 if(OpenMP_CXX_FOUND)
-  target_link_libraries(multigrid_ndt_omp OpenMP::OpenMP_CXX)
+  target_link_libraries(autoware_multigrid_ndt_omp OpenMP::OpenMP_CXX)
 else()
   message(WARNING "OpenMP not found")
 endif()
@@ -81,6 +81,6 @@ set(EXECUTABLES
 
 foreach(executable IN LISTS EXECUTABLES)
   add_executable(${executable} apps/${executable}.cpp)
-  add_dependencies(${executable} ndt_omp multigrid_ndt_omp)
-  target_link_libraries(${executable} ndt_omp multigrid_ndt_omp ${PCL_LIBRARIES})
+  add_dependencies(${executable} autoware_ndt_omp autoware_multigrid_ndt_omp)
+  target_link_libraries(${executable} autoware_ndt_omp autoware_multigrid_ndt_omp ${PCL_LIBRARIES})
 endforeach()

README.md

@@ -18,38 +18,38 @@ single : 282.222[msec]
 10times: 2921.92[msec]
 fitness: 0.213937
 
---- pclomp::NDT (KDTREE, 1 threads) ---
+--- autoware::ndt_omp::pclomp::NDT (KDTREE, 1 threads) ---
 single : 207.697[msec]
 10times: 2059.19[msec]
 fitness: 0.213937
 
---- pclomp::NDT (DIRECT7, 1 threads) ---
+--- autoware::ndt_omp::pclomp::NDT (DIRECT7, 1 threads) ---
 single : 139.433[msec]
 10times: 1356.79[msec]
 fitness: 0.214205
 
---- pclomp::NDT (DIRECT1, 1 threads) ---
+--- autoware::ndt_omp::pclomp::NDT (DIRECT1, 1 threads) ---
 single : 34.6418[msec]
 10times: 317.03[msec]
 fitness: 0.208511
 
---- pclomp::NDT (KDTREE, 8 threads) ---
+--- autoware::ndt_omp::pclomp::NDT (KDTREE, 8 threads) ---
 single : 54.9903[msec]
 10times: 500.51[msec]
 fitness: 0.213937
 
---- pclomp::NDT (DIRECT7, 8 threads) ---
+--- autoware::ndt_omp::pclomp::NDT (DIRECT7, 8 threads) ---
 single : 63.1442[msec]
 10times: 343.336[msec]
 fitness: 0.214205
 
---- pclomp::NDT (DIRECT1, 8 threads) ---
+--- autoware::ndt_omp::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.
+Several methods for neighbor voxel search are implemented. If you select autoware::ndt_omp::pclomp::KDTREE, results will be completely same as the original pcl::NDT. We recommend to use autoware::ndt_omp::pclomp::DIRECT7 which is faster and stable. If you need extremely fast registration, choose autoware::ndt_omp::pclomp::DIRECT1, but it might be a bit unstable.
 
 <img src="data/screenshot.png" height="400pix" /><br>
 Red: target, Green: source, Blue: aligned

apps/align.cpp

@@ -1,13 +1,13 @@
-#include <multigrid_pclomp/multigrid_ndt_omp.h>
+#include <autoware/ndt_omp/multigrid_pclomp/multigrid_ndt_omp.h>
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl/registration/gicp.h>
 #include <pcl/registration/ndt.h>
 #include <pcl/visualization/pcl_visualizer.h>
-#include <pclomp/gicp_omp.h>
-#include <pclomp/ndt_omp.h>
+#include <autoware/ndt_omp/pclomp/gicp_omp.h>
+#include <autoware/ndt_omp/pclomp/ndt_omp.h>
 
 #include <chrono>
 #include <iostream>
@@ -82,9 +82,9 @@ int main(int argc, char ** argv)
   // cppcheck-suppress redundantAssignment
   aligned = align(gicp, target_cloud, source_cloud);
 
-  std::cout << "--- pclomp::GICP ---" << std::endl;
-  pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>::Ptr gicp_omp(
-    new pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>());
+  std::cout << "--- autoware::ndt_omp::pclomp::GICP ---" << std::endl;
+  autoware::ndt_omp::pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>::Ptr gicp_omp(
+    new autoware::ndt_omp::pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>());
   // cppcheck-suppress redundantAssignment
   aligned = align(gicp_omp, target_cloud, source_cloud);
 
@@ -96,20 +96,20 @@ int main(int argc, char ** argv)
   aligned = align(ndt, target_cloud, source_cloud);
 
   std::vector<int> num_threads = {1, omp_get_max_threads()};
-  std::vector<std::pair<std::string, pclomp::NeighborSearchMethod>> search_methods = {
-    {"KDTREE", pclomp::KDTREE}, {"DIRECT7", pclomp::DIRECT7}, {"DIRECT1", pclomp::DIRECT1}};
+  std::vector<std::pair<std::string, autoware::ndt_omp::pclomp::NeighborSearchMethod>> search_methods = {
+    {"KDTREE", autoware::ndt_omp::pclomp::KDTREE}, {"DIRECT7", autoware::ndt_omp::pclomp::DIRECT7}, {"DIRECT1", autoware::ndt_omp::pclomp::DIRECT1}};
 
-  pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndt_omp(
-    new pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
+  autoware::ndt_omp::pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndt_omp(
+    new autoware::ndt_omp::pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
   ndt_omp->setResolution(1.0);
 
-  pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr mg_ndt_omp(
-    new pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
+  autoware::ndt_omp::pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr mg_ndt_omp(
+    new autoware::ndt_omp::pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
   mg_ndt_omp->setResolution(1.0);
 
   for (int n : num_threads) {
     for (const auto & search_method : search_methods) {
-      std::cout << "--- pclomp::NDT (" << search_method.first << ", " << n << " threads) ---"
+      std::cout << "--- autoware::ndt_omp::pclomp::NDT (" << search_method.first << ", " << n << " threads) ---"
                 << std::endl;
       ndt_omp->setNumThreads(n);
       ndt_omp->setNeighborhoodSearchMethod(search_method.second);

apps/check_covariance.cpp

@@ -1,7 +1,7 @@
-#include "estimate_covariance/estimate_covariance.hpp"
+#include "autoware/ndt_omp/estimate_covariance/estimate_covariance.hpp"
 
 #include <glob.h>
-#include <multigrid_pclomp/multigrid_ndt_omp.h>
+#include <autoware/ndt_omp/multigrid_pclomp/multigrid_ndt_omp.h>
 #include <omp.h>
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/io/pcd_io.h>
@@ -10,7 +10,7 @@
 #include <pcl/registration/gicp.h>
 #include <pcl/registration/ndt.h>
 #include <pcl/visualization/pcl_visualizer.h>
-#include <pclomp/gicp_omp.h>
+#include <autoware/ndt_omp/pclomp/gicp_omp.h>
 
 // this must included after pcd_io.h
 // clang-format off
@@ -50,8 +50,8 @@ int main(int argc, char ** argv)
   const auto n_data = static_cast<int64_t>(initial_pose_list.size());
 
   // prepare ndt
-  std::shared_ptr<pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>>
-    mg_ndt_omp(new pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
+  std::shared_ptr<autoware::ndt_omp::pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>>
+    mg_ndt_omp(new autoware::ndt_omp::pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
   mg_ndt_omp->setInputTarget(target_cloud);
   mg_ndt_omp->setResolution(2.0);
   mg_ndt_omp->setNumThreads(4);
@@ -103,29 +103,29 @@ int main(int argc, char ** argv)
     mg_ndt_omp->setInputSource(source_cloud);
     pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>());
     mg_ndt_omp->align(*aligned, initial_pose);
-    const pclomp::NdtResult ndt_result = mg_ndt_omp->getResult();
+    const autoware::ndt_omp::pclomp::NdtResult ndt_result = mg_ndt_omp->getResult();
     const double score = ndt_result.nearest_voxel_transformation_likelihood;
     std::cout << source_pcd << ", num=" << std::setw(4) << source_cloud->size()
               << " points, score=" << score << std::endl;
 
     const std::vector<Eigen::Matrix4f> poses_to_search =
-      pclomp::propose_poses_to_search(ndt_result, offset_x, offset_y);
+      autoware::ndt_omp::pclomp::propose_poses_to_search(ndt_result, offset_x, offset_y);
 
     // estimate covariance
     // (1) Laplace approximation
     t1 = std::chrono::system_clock::now();
     const Eigen::Matrix2d cov_by_la =
-      pclomp::estimate_xy_covariance_by_laplace_approximation(ndt_result.hessian);
+      autoware::ndt_omp::pclomp::estimate_xy_covariance_by_laplace_approximation(ndt_result.hessian);
     t2 = std::chrono::system_clock::now();
     const auto elapsed_la =
       static_cast<double>(std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count()) /
       1000.0;
 
     // (2) Multi NDT
     t1 = std::chrono::system_clock::now();
-    const pclomp::ResultOfMultiNdtCovarianceEstimation result_of_mndt =
-      pclomp::estimate_xy_covariance_by_multi_ndt(ndt_result, mg_ndt_omp, poses_to_search);
-    const Eigen::Matrix2d cov_by_mndt = pclomp::adjust_diagonal_covariance(
+    const autoware::ndt_omp::pclomp::ResultOfMultiNdtCovarianceEstimation result_of_mndt =
+      autoware::ndt_omp::pclomp::estimate_xy_covariance_by_multi_ndt(ndt_result, mg_ndt_omp, poses_to_search);
+    const Eigen::Matrix2d cov_by_mndt = autoware::ndt_omp::pclomp::adjust_diagonal_covariance(
       result_of_mndt.covariance, ndt_result.pose, 0.0225, 0.0225);
     t2 = std::chrono::system_clock::now();
     const auto elapsed_mndt =
@@ -135,10 +135,10 @@ int main(int argc, char ** argv)
     // (3) Multi NDT with score
     const double temperature = 0.1;
     t1 = std::chrono::system_clock::now();
-    const pclomp::ResultOfMultiNdtCovarianceEstimation result_of_mndt_score =
-      pclomp::estimate_xy_covariance_by_multi_ndt_score(
+    const autoware::ndt_omp::pclomp::ResultOfMultiNdtCovarianceEstimation result_of_mndt_score =
+      autoware::ndt_omp::pclomp::estimate_xy_covariance_by_multi_ndt_score(
         ndt_result, mg_ndt_omp, poses_to_search, temperature);
-    const Eigen::Matrix2d cov_by_mndt_score = pclomp::adjust_diagonal_covariance(
+    const Eigen::Matrix2d cov_by_mndt_score = autoware::ndt_omp::pclomp::adjust_diagonal_covariance(
       result_of_mndt_score.covariance, ndt_result.pose, 0.0225, 0.0225);
     t2 = std::chrono::system_clock::now();
     const auto elapsed_mndt_score =
@@ -151,11 +151,11 @@ int main(int argc, char ** argv)
     const Eigen::Vector3f euler_initial = initial_pose.block<3, 3>(0, 0).eulerAngles(0, 1, 2);
     const Eigen::Vector3f euler_result = ndt_result.pose.block<3, 3>(0, 0).eulerAngles(0, 1, 2);
     const Eigen::Matrix2d cov_by_la_rotated =
-      pclomp::rotate_covariance_to_base_link(cov_by_la, ndt_result.pose);
+      autoware::ndt_omp::pclomp::rotate_covariance_to_base_link(cov_by_la, ndt_result.pose);
     const Eigen::Matrix2d cov_by_mndt_rotated =
-      pclomp::rotate_covariance_to_base_link(cov_by_mndt, ndt_result.pose);
+      autoware::ndt_omp::pclomp::rotate_covariance_to_base_link(cov_by_mndt, ndt_result.pose);
     const Eigen::Matrix2d cov_by_mndt_score_rotated =
-      pclomp::rotate_covariance_to_base_link(cov_by_mndt_score, ndt_result.pose);
+     autoware::ndt_omp::pclomp::rotate_covariance_to_base_link(cov_by_mndt_score, ndt_result.pose);
     ofs << i << "," << score << ",";
     ofs << initial_pose(0, 3) << "," << initial_pose(1, 3) << "," << euler_initial(2) << ",";
     ofs << result_x << "," << result_y << "," << euler_result(2) << ",";
@@ -181,7 +181,7 @@ int main(int argc, char ** argv)
     ofs_mndt << "index,score,initial_x,initial_y,result_x,result_y" << std::endl;
     ofs_mndt << std::fixed;
     for (int j = 0; j < n_mndt; j++) {
-      const pclomp::NdtResult & multi_ndt_result = result_of_mndt.ndt_results[j];
+      const autoware::ndt_omp::pclomp::NdtResult & multi_ndt_result = result_of_mndt.ndt_results[j];
       const auto nvtl = multi_ndt_result.nearest_voxel_transformation_likelihood;
       const auto initial_x = result_of_mndt.ndt_initial_poses[j](0, 3);
       const auto initial_y = result_of_mndt.ndt_initial_poses[j](1, 3);
@@ -197,7 +197,7 @@ int main(int argc, char ** argv)
     ofs_mndt_score << "index,score,initial_x,initial_y,result_x,result_y" << std::endl;
     ofs_mndt_score << std::fixed;
     for (int j = 0; j < n_mndt_score; j++) {
-      const pclomp::NdtResult & multi_ndt_score_result = result_of_mndt_score.ndt_results[j];
+      const autoware::ndt_omp::pclomp::NdtResult & multi_ndt_score_result = result_of_mndt_score.ndt_results[j];
       const auto nvtl = multi_ndt_score_result.nearest_voxel_transformation_likelihood;
       const auto initial_x = result_of_mndt_score.ndt_initial_poses[j](0, 3);
       const auto initial_y = result_of_mndt_score.ndt_initial_poses[j](1, 3);

apps/regression_test.cpp

@@ -15,7 +15,7 @@
 #include "timer.hpp"
 
 #include <glob.h>
-#include <multigrid_pclomp/multigrid_ndt_omp.h>
+#include <autoware/ndt_omp/multigrid_pclomp/multigrid_ndt_omp.h>
 #include <omp.h>
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/io/pcd_io.h>
@@ -24,7 +24,7 @@
 #include <pcl/registration/gicp.h>
 #include <pcl/registration/ndt.h>
 #include <pcl/visualization/pcl_visualizer.h>
-#include <pclomp/gicp_omp.h>
+#include <autoware/ndt_omp/pclomp/gicp_omp.h>
 
 // must include after including pcl header
 // clang-format off
@@ -65,8 +65,8 @@ int main(int argc, char ** argv)
   const auto n_data = static_cast<int64_t>(initial_pose_list.size());
 
   // prepare ndt
-  pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr mg_ndt_omp(
-    new pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
+  autoware::ndt_omp::pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr mg_ndt_omp(
+    new autoware::ndt_omp::pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
   mg_ndt_omp->setResolution(2.0);
   mg_ndt_omp->setNumThreads(4);
   mg_ndt_omp->setMaximumIterations(30);
@@ -112,7 +112,7 @@ int main(int argc, char ** argv)
     pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>());
     timer.start();
     mg_ndt_omp->align(*aligned, initial_pose);
-    const pclomp::NdtResult ndt_result = mg_ndt_omp->getResult();
+    const autoware::ndt_omp::pclomp::NdtResult ndt_result = mg_ndt_omp->getResult();
     const double elapsed = timer.elapsed_milliseconds();
 
     const float gain_tp =

include/estimate_covariance/estimate_covariance.hpp → include/autoware/ndt_omp/estimate_covariance/estimate_covariance.hpp

@@ -1,15 +1,14 @@
 #ifndef NDT_OMP__ESTIMATE_COVARIANCE_HPP_
 #define NDT_OMP__ESTIMATE_COVARIANCE_HPP_
 
-#include "multigrid_pclomp/multigrid_ndt_omp.h"
-
 #include <Eigen/Core>
-
 #include <memory>
 #include <utility>
 #include <vector>
 
-namespace pclomp
+#include "autoware/ndt_omp/multigrid_pclomp/multigrid_ndt_omp.h"
+
+namespace autoware::ndt_omp::pclomp
 {
 
 struct ResultOfMultiNdtCovarianceEstimation
@@ -25,13 +24,13 @@ Eigen::Matrix2d estimate_xy_covariance_by_laplace_approximation(
   const Eigen::Matrix<double, 6, 6> & hessian);
 ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt(
   const NdtResult & ndt_result,
-  const std::shared_ptr<
-    pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>> & ndt_ptr,
+  const std::shared_ptr<autoware::ndt_omp::pclomp::MultiGridNormalDistributionsTransform<
+    pcl::PointXYZ, pcl::PointXYZ>> & ndt_ptr,
   const std::vector<Eigen::Matrix4f> & poses_to_search);
 ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt_score(
   const NdtResult & ndt_result,
-  const std::shared_ptr<
-    pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>> & ndt_ptr,
+  const std::shared_ptr<autoware::ndt_omp::pclomp::MultiGridNormalDistributionsTransform<
+    pcl::PointXYZ, pcl::PointXYZ>> & ndt_ptr,
   const std::vector<Eigen::Matrix4f> & poses_to_search, const double temperature);
 
 /** \brief Find rotation matrix aligning covariance to principal axes
@@ -71,6 +70,6 @@ Eigen::Matrix2d adjust_diagonal_covariance(
   const Eigen::Matrix2d & covariance, const Eigen::Matrix4f & pose, const double fixed_cov00,
   const double fixed_cov11);
 
-}  // namespace pclomp
+}  // namespace autoware::ndt_omp::pclomp
 
 #endif  // NDT_OMP__ESTIMATE_COVARIANCE_HPP_