From 52837289757fa589f04d2482693c0d4c49f55610 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Tue, 17 Sep 2024 13:12:21 +0200 Subject: [PATCH] refactor(ndt_omp): prefix with autoware_ Signed-off-by: Esteve Fernandez --- CMakeLists.txt | 18 +-- README.md | 14 +- apps/align.cpp | 36 +++-- apps/check_covariance.cpp | 43 +++--- apps/regression_test.cpp | 11 +- .../estimate_covariance.hpp | 14 +- .../multi_voxel_grid_covariance_omp.h | 28 ++-- .../multi_voxel_grid_covariance_omp_impl.hpp | 24 +-- .../multigrid_pclomp/multigrid_ndt_omp.h | 30 ++-- .../multigrid_ndt_omp_impl.hpp | 58 ++++--- .../{ => autoware/ndt_omp}/pclomp/gicp_omp.h | 41 ++--- .../ndt_omp}/pclomp/gicp_omp_impl.hpp | 59 ++++--- .../{ => autoware/ndt_omp}/pclomp/ndt_omp.h | 33 ++-- .../ndt_omp}/pclomp/ndt_omp_impl.hpp | 144 ++++++++++-------- .../ndt_omp}/pclomp/ndt_struct.hpp | 4 +- .../pclomp/voxel_grid_covariance_omp.h | 51 ++++--- .../pclomp/voxel_grid_covariance_omp_impl.hpp | 68 ++++++--- package.xml | 2 +- .../estimate_covariance.cpp | 14 +- .../multi_voxel_grid_covariance_omp.cpp | 8 +- src/multigrid_pclomp/multigrid_ndt_omp.cpp | 10 +- src/pclomp/gicp_omp.cpp | 10 +- src/pclomp/ndt_omp.cpp | 10 +- src/pclomp/voxel_grid_covariance_omp.cpp | 8 +- 24 files changed, 419 insertions(+), 319 deletions(-) rename include/{ => autoware/ndt_omp}/estimate_covariance/estimate_covariance.hpp (85%) rename include/{ => autoware/ndt_omp}/multigrid_pclomp/multi_voxel_grid_covariance_omp.h (95%) rename include/{ => autoware/ndt_omp}/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp (95%) rename include/{ => autoware/ndt_omp}/multigrid_pclomp/multigrid_ndt_omp.h (97%) rename include/{ => autoware/ndt_omp}/multigrid_pclomp/multigrid_ndt_omp_impl.hpp (98%) rename include/{ => autoware/ndt_omp}/pclomp/gicp_omp.h (95%) rename include/{ => autoware/ndt_omp}/pclomp/gicp_omp_impl.hpp (92%) rename include/{ => autoware/ndt_omp}/pclomp/ndt_omp.h (96%) rename include/{ => autoware/ndt_omp}/pclomp/ndt_omp_impl.hpp (92%) rename include/{ => autoware/ndt_omp}/pclomp/ndt_struct.hpp (97%) rename include/{ => autoware/ndt_omp}/pclomp/voxel_grid_covariance_omp.h (94%) rename include/{ => autoware/ndt_omp}/pclomp/voxel_grid_covariance_omp_impl.hpp (92%) diff --git a/CMakeLists.txt b/CMakeLists.txt index bae8560c..e87894e5 100644 --- a/CMakeLists.txt +++ b/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,16 +39,16 @@ 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() @@ -56,14 +56,14 @@ 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() diff --git a/README.md b/README.md index c62f8cc8..61f4644d 100644 --- a/README.md +++ b/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.
Red: target, Green: source, Blue: aligned diff --git a/apps/align.cpp b/apps/align.cpp index 0e18f4bf..c5171942 100644 --- a/apps/align.cpp +++ b/apps/align.cpp @@ -1,4 +1,6 @@ -#include +#include +#include +#include #include #include #include @@ -6,8 +8,6 @@ #include #include #include -#include -#include #include #include @@ -82,9 +82,10 @@ int main(int argc, char ** argv) // cppcheck-suppress redundantAssignment aligned = align(gicp, target_cloud, source_cloud); - std::cout << "--- pclomp::GICP ---" << std::endl; - pclomp::GeneralizedIterativeClosestPoint::Ptr gicp_omp( - new pclomp::GeneralizedIterativeClosestPoint()); + std::cout << "--- autoware::ndt_omp::pclomp::GICP ---" << std::endl; + autoware::ndt_omp::pclomp::GeneralizedIterativeClosestPoint::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,21 +97,26 @@ int main(int argc, char ** argv) aligned = align(ndt, target_cloud, source_cloud); std::vector num_threads = {1, omp_get_max_threads()}; - std::vector> search_methods = { - {"KDTREE", pclomp::KDTREE}, {"DIRECT7", pclomp::DIRECT7}, {"DIRECT1", pclomp::DIRECT1}}; - - pclomp::NormalDistributionsTransform::Ptr ndt_omp( - new pclomp::NormalDistributionsTransform()); + std::vector> + search_methods = { + {"KDTREE", autoware::ndt_omp::pclomp::KDTREE}, + {"DIRECT7", autoware::ndt_omp::pclomp::DIRECT7}, + {"DIRECT1", autoware::ndt_omp::pclomp::DIRECT1}}; + + autoware::ndt_omp::pclomp::NormalDistributionsTransform::Ptr + ndt_omp( + new autoware::ndt_omp::pclomp::NormalDistributionsTransform()); ndt_omp->setResolution(1.0); - pclomp::MultiGridNormalDistributionsTransform::Ptr mg_ndt_omp( - new pclomp::MultiGridNormalDistributionsTransform()); + autoware::ndt_omp::pclomp::MultiGridNormalDistributionsTransform:: + 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::endl; + std::cout << "--- autoware::ndt_omp::pclomp::NDT (" << search_method.first << ", " << n + << " threads) ---" << std::endl; ndt_omp->setNumThreads(n); ndt_omp->setNeighborhoodSearchMethod(search_method.second); // cppcheck-suppress redundantAssignment diff --git a/apps/check_covariance.cpp b/apps/check_covariance.cpp index aae78a08..455ff8dc 100644 --- a/apps/check_covariance.cpp +++ b/apps/check_covariance.cpp @@ -1,7 +1,8 @@ -#include "estimate_covariance/estimate_covariance.hpp" +#include "autoware/ndt_omp/estimate_covariance/estimate_covariance.hpp" +#include +#include #include -#include #include #include #include @@ -10,7 +11,6 @@ #include #include #include -#include // this must included after pcd_io.h // clang-format off @@ -50,8 +50,10 @@ int main(int argc, char ** argv) const auto n_data = static_cast(initial_pose_list.size()); // prepare ndt - std::shared_ptr> - mg_ndt_omp(new pclomp::MultiGridNormalDistributionsTransform()); + std::shared_ptr< + autoware::ndt_omp::pclomp::MultiGridNormalDistributionsTransform> + 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,19 +105,20 @@ int main(int argc, char ** argv) mg_ndt_omp->setInputSource(source_cloud); pcl::PointCloud::Ptr aligned(new pcl::PointCloud()); 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 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(std::chrono::duration_cast(t2 - t1).count()) / @@ -123,9 +126,10 @@ int main(int argc, char ** argv) // (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 +139,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 +155,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 +185,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 +201,8 @@ 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); diff --git a/apps/regression_test.cpp b/apps/regression_test.cpp index d9dc48d3..de3612af 100644 --- a/apps/regression_test.cpp +++ b/apps/regression_test.cpp @@ -14,8 +14,9 @@ #include "timer.hpp" +#include +#include #include -#include #include #include #include @@ -24,7 +25,6 @@ #include #include #include -#include // must include after including pcl header // clang-format off @@ -65,8 +65,9 @@ int main(int argc, char ** argv) const auto n_data = static_cast(initial_pose_list.size()); // prepare ndt - pclomp::MultiGridNormalDistributionsTransform::Ptr mg_ndt_omp( - new pclomp::MultiGridNormalDistributionsTransform()); + autoware::ndt_omp::pclomp::MultiGridNormalDistributionsTransform:: + 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 +113,7 @@ int main(int argc, char ** argv) pcl::PointCloud::Ptr aligned(new pcl::PointCloud()); 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 = diff --git a/include/estimate_covariance/estimate_covariance.hpp b/include/autoware/ndt_omp/estimate_covariance/estimate_covariance.hpp similarity index 85% rename from include/estimate_covariance/estimate_covariance.hpp rename to include/autoware/ndt_omp/estimate_covariance/estimate_covariance.hpp index 35da2a71..512bf8c2 100644 --- a/include/estimate_covariance/estimate_covariance.hpp +++ b/include/autoware/ndt_omp/estimate_covariance/estimate_covariance.hpp @@ -1,7 +1,7 @@ #ifndef NDT_OMP__ESTIMATE_COVARIANCE_HPP_ #define NDT_OMP__ESTIMATE_COVARIANCE_HPP_ -#include "multigrid_pclomp/multigrid_ndt_omp.h" +#include "autoware/ndt_omp/multigrid_pclomp/multigrid_ndt_omp.h" #include @@ -9,7 +9,7 @@ #include #include -namespace pclomp +namespace autoware::ndt_omp::pclomp { struct ResultOfMultiNdtCovarianceEstimation @@ -25,13 +25,13 @@ Eigen::Matrix2d estimate_xy_covariance_by_laplace_approximation( const Eigen::Matrix & hessian); ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt( const NdtResult & ndt_result, - const std::shared_ptr< - pclomp::MultiGridNormalDistributionsTransform> & ndt_ptr, + const std::shared_ptr> & ndt_ptr, const std::vector & poses_to_search); ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt_score( const NdtResult & ndt_result, - const std::shared_ptr< - pclomp::MultiGridNormalDistributionsTransform> & ndt_ptr, + const std::shared_ptr> & ndt_ptr, const std::vector & poses_to_search, const double temperature); /** \brief Find rotation matrix aligning covariance to principal axes @@ -71,6 +71,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_ diff --git a/include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h b/include/autoware/ndt_omp/multigrid_pclomp/multi_voxel_grid_covariance_omp.h similarity index 95% rename from include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h rename to include/autoware/ndt_omp/multigrid_pclomp/multi_voxel_grid_covariance_omp.h index bfd60ddb..84c96f0a 100644 --- a/include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h +++ b/include/autoware/ndt_omp/multigrid_pclomp/multi_voxel_grid_covariance_omp.h @@ -69,7 +69,7 @@ #include #include -namespace pclomp +namespace autoware::ndt_omp::pclomp { /** \brief A searchable voxel structure containing the mean and covariance of the data. * \note For more information please see @@ -182,43 +182,43 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid /** \brief Get the voxel covariance. * \return covariance matrix */ - const Eigen::Matrix3d & getCov() const { return (cov_); } - Eigen::Matrix3d & getCov() { return (cov_); } + const Eigen::Matrix3d & getCov() const { return cov_; } + Eigen::Matrix3d & getCov() { return cov_; } /** \brief Get the inverse of the voxel covariance. * \return inverse covariance matrix */ - const Eigen::Matrix3d & getInverseCov() const { return (icov_); } + const Eigen::Matrix3d & getInverseCov() const { return icov_; } - Eigen::Matrix3d & getInverseCov() { return (icov_); } + Eigen::Matrix3d & getInverseCov() { return icov_; } /** \brief Get the voxel centroid. * \return centroid */ - const Eigen::Vector3d & getMean() const { return (mean_); } + const Eigen::Vector3d & getMean() const { return mean_; } - Eigen::Vector3d & getMean() { return (mean_); } + Eigen::Vector3d & getMean() { return mean_; } /** \brief Get the eigen vectors of the voxel covariance. * \note Order corresponds with \ref getEvals * \return matrix whose columns contain eigen vectors */ - const Eigen::Matrix3d & getEvecs() const { return (evecs_); } + const Eigen::Matrix3d & getEvecs() const { return evecs_; } - Eigen::Matrix3d & getEvecs() { return (evecs_); } + Eigen::Matrix3d & getEvecs() { return evecs_; } /** \brief Get the eigen values of the voxel covariance. * \note Order corresponds with \ref getEvecs * \return vector of eigen values */ - const Eigen::Vector3d & getEvals() const { return (evals_); } + const Eigen::Vector3d & getEvals() const { return evals_; } - Eigen::Vector3d & getEvals() { return (evals_); } + Eigen::Vector3d & getEvals() { return evals_; } /** \brief Get the number of points contained by this voxel. * \return number of points */ - int getPointCount() const { return (nr_points_); } + int getPointCount() const { return nr_points_; } /** \brief Number of points contained by voxel */ int nr_points_; @@ -428,7 +428,7 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid pcl::KdTreeFLANN kdtree_; // To access leaf by the search results by kdtree std::vector leaf_ptrs_; -}; -} // namespace pclomp +} +} // namespace autoware::ndt_omp::pclomp #endif // #ifndef PCL_MULTI_VOXEL_GRID_COVARIANCE_H_ diff --git a/include/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp b/include/autoware/ndt_omp/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp similarity index 95% rename from include/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp rename to include/autoware/ndt_omp/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp index d6638c3d..0ebf124c 100644 --- a/include/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp +++ b/include/autoware/ndt_omp/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp @@ -52,7 +52,7 @@ #ifndef PCL_MULTI_VOXEL_GRID_COVARIANCE_IMPL_OMP_H_ #define PCL_MULTI_VOXEL_GRID_COVARIANCE_IMPL_OMP_H_ -#include "multigrid_pclomp/multi_voxel_grid_covariance_omp.h" +#include "autoware/ndt_omp/multigrid_pclomp/multi_voxel_grid_covariance_omp.h" #include #include @@ -63,7 +63,7 @@ #include #include -namespace pclomp +namespace autoware::ndt_omp::pclomp { template @@ -106,7 +106,8 @@ MultiVoxelGridCovariance::MultiVoxelGridCovariance( } template -MultiVoxelGridCovariance & pclomp::MultiVoxelGridCovariance::operator=( +MultiVoxelGridCovariance & +autoware::ndt_omp::pclomp::MultiVoxelGridCovariance::operator=( const MultiVoxelGridCovariance & other) { pcl::VoxelGrid::operator=(other); @@ -132,7 +133,8 @@ MultiVoxelGridCovariance & pclomp::MultiVoxelGridCovariance::ope } template -MultiVoxelGridCovariance & pclomp::MultiVoxelGridCovariance::operator=( +MultiVoxelGridCovariance & +autoware::ndt_omp::pclomp::MultiVoxelGridCovariance::operator=( MultiVoxelGridCovariance && other) noexcept { voxel_centroids_ptr_ = std::move(other.voxel_centroids_ptr_); @@ -274,7 +276,7 @@ int MultiVoxelGridCovariance::radiusSearch( return 0; } - return (radiusSearch(cloud[index], radius, k_leaves, max_nn)); + return radiusSearch(cloud[index], radius, k_leaves, max_nn); } template @@ -352,7 +354,9 @@ void MultiVoxelGridCovariance::applyFilter( for (auto & p : *input) { if (!input->is_dense) { // Check if the point is invalid - if (!std::isfinite(p.x) || !std::isfinite(p.y) || !std::isfinite(p.z)) continue; + if (!std::isfinite(p.x) || !std::isfinite(p.y) || !std::isfinite(p.z)) { + continue; + } } int64_t lid = getLeafID(p, bbox); Leaf & leaf = map_leaves[lid]; @@ -393,7 +397,7 @@ void MultiVoxelGridCovariance::applyFilter( } template -int64_t pclomp::MultiVoxelGridCovariance::getLeafID( +int64_t autoware::ndt_omp::pclomp::MultiVoxelGridCovariance::getLeafID( const PointT & point, const BoundingBox & bbox) const { int ijk0 = @@ -407,7 +411,7 @@ int64_t pclomp::MultiVoxelGridCovariance::getLeafID( } template -void pclomp::MultiVoxelGridCovariance::updateLeaf( +void autoware::ndt_omp::pclomp::MultiVoxelGridCovariance::updateLeaf( const PointT & point, const int & centroid_size, Leaf & leaf) const { if (leaf.nr_points_ == 0) { @@ -427,7 +431,7 @@ void pclomp::MultiVoxelGridCovariance::updateLeaf( } template -void pclomp::MultiVoxelGridCovariance::computeLeafParams( +void autoware::ndt_omp::pclomp::MultiVoxelGridCovariance::computeLeafParams( const Eigen::Vector3d & pt_sum, Eigen::SelfAdjointEigenSolver & eigensolver, Leaf & leaf) const { @@ -467,7 +471,7 @@ void pclomp::MultiVoxelGridCovariance::computeLeafParams( } } -} // namespace pclomp +} // namespace autoware::ndt_omp::pclomp #define PCL_INSTANTIATE_VoxelGridCovariance(T) \ template class PCL_EXPORTS pcl::VoxelGridCovariance; diff --git a/include/multigrid_pclomp/multigrid_ndt_omp.h b/include/autoware/ndt_omp/multigrid_pclomp/multigrid_ndt_omp.h similarity index 97% rename from include/multigrid_pclomp/multigrid_ndt_omp.h rename to include/autoware/ndt_omp/multigrid_pclomp/multigrid_ndt_omp.h index fdf6e81f..e96db928 100644 --- a/include/multigrid_pclomp/multigrid_ndt_omp.h +++ b/include/autoware/ndt_omp/multigrid_pclomp/multigrid_ndt_omp.h @@ -56,7 +56,7 @@ #define PCL_REGISTRATION_NDT_OMP_MULTI_VOXEL_H_ #include "../pclomp/ndt_struct.hpp" -#include "multigrid_pclomp/multi_voxel_grid_covariance_omp.h" +#include "autoware/ndt_omp/multigrid_pclomp/multi_voxel_grid_covariance_omp.h" #include #include @@ -65,7 +65,7 @@ #include -namespace pclomp +namespace autoware::ndt_omp::pclomp { /** \brief A 3D Normal Distribution Transform registration implementation for point cloud data. @@ -93,7 +93,7 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration TargetGrid; + typedef autoware::ndt_omp::pclomp::MultiVoxelGridCovariance TargetGrid; /** \brief Typename of const pointer to searchable voxel grid leaf. */ typedef typename TargetGrid::LeafConstPtr TargetGridLeafConstPtr; @@ -181,19 +181,21 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration getHessian() const { return hessian_; } @@ -303,7 +305,7 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration #include #include -namespace pclomp +namespace autoware::ndt_omp::pclomp { template @@ -301,8 +301,9 @@ void MultiGridNormalDistributionsTransform::computeTra p = p + delta_p; // Update Visualizer (untested) - if (update_visualizer_ != 0) + if (update_visualizer_ != 0) { update_visualizer_(output, std::vector(), *target_, std::vector()); + } nr_iterations_++; @@ -490,7 +491,7 @@ double MultiGridNormalDistributionsTransform::computeD const float transform_probability = (input_->points.empty() ? 0.0f : score / static_cast(input_->points.size())); transform_probability_array_.push_back(transform_probability); - return (score); + return score; } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -640,7 +641,9 @@ double MultiGridNormalDistributionsTransform::updateDe e_x_cov_x = gauss_d2_ * e_x_cov_x; // Error checking for invalid values. - if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x) return (0); + if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x) { + return 0; + } // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] e_x_cov_x *= gauss_d1_; @@ -674,7 +677,7 @@ double MultiGridNormalDistributionsTransform::updateDe } } - return (score_inc); + return score_inc; } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -757,7 +760,9 @@ void MultiGridNormalDistributionsTransform::updateHess double e_x_cov_x = gauss_d2_ * exp(-gauss_d2_ * x_trans.dot(c_inv * x_trans) / 2); // Error checking for invalid values. - if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x) return; + if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x) { + return; + } // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] e_x_cov_x *= gauss_d1_; @@ -789,14 +794,14 @@ bool MultiGridNormalDistributionsTransform::updateInte a_u = a_t; f_u = f_t; g_u = g_t; - return (false); + return false; } // Case U2 in Update Algorithm and Case b in Modified Update Algorithm [More, Thuente 1994] if (g_t * (a_l - a_t) > 0) { a_l = a_t; f_l = f_t; g_l = g_t; - return (false); + return false; } // Case U3 in Update Algorithm and Case c in Modified Update Algorithm [More, Thuente 1994] if (g_t * (a_l - a_t) < 0) { @@ -807,10 +812,10 @@ bool MultiGridNormalDistributionsTransform::updateInte a_l = a_t; f_l = f_t; g_l = g_t; - return (false); + return false; } // Interval Converged - return (true); + return true; } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -833,9 +838,9 @@ double MultiGridNormalDistributionsTransform::trialVal double a_q = a_l - 0.5 * (a_l - a_t) * g_l / (g_l - (f_l - f_t) / (a_l - a_t)); if (std::fabs(a_c - a_l) < std::fabs(a_q - a_l)) { - return (a_c); + return a_c; } - return (0.5 * (a_q + a_c)); + return 0.5 * (a_q + a_c); } // Case 2 in Trial Value Selection [More, Thuente 1994] if (g_t * g_l < 0) { @@ -851,9 +856,9 @@ double MultiGridNormalDistributionsTransform::trialVal double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l; if (std::fabs(a_c - a_t) >= std::fabs(a_s - a_t)) { - return (a_c); + return a_c; } - return (a_s); + return a_s; } // Case 3 in Trial Value Selection [More, Thuente 1994] if (std::fabs(g_t) <= std::fabs(g_l)) { @@ -869,15 +874,16 @@ double MultiGridNormalDistributionsTransform::trialVal double a_t_next = 0.0; - if (std::fabs(a_c - a_t) < std::fabs(a_s - a_t)) + if (std::fabs(a_c - a_t) < std::fabs(a_s - a_t)) { a_t_next = a_c; - else + } else { a_t_next = a_s; + } if (a_t > a_l) { - return (std::min(a_t + 0.66 * (a_u - a_t), a_t_next)); + return std::min(a_t + 0.66 * (a_u - a_t), a_t_next); } - return (std::max(a_t + 0.66 * (a_u - a_t), a_t_next)); + return std::max(a_t + 0.66 * (a_u - a_t), a_t_next); } // Case 4 in Trial Value Selection [More, Thuente 1994] // Calculate the minimizer of the cubic that interpolates f_u, f_t, g_u and g_t @@ -885,7 +891,7 @@ double MultiGridNormalDistributionsTransform::trialVal double z = 3 * (f_t - f_u) / (a_t - a_u) - g_t - g_u; double w = std::sqrt(z * z - g_t * g_u); // Equation 2.4.56 [Sun, Yuan 2006] - return (a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w)); + return a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -904,7 +910,9 @@ double MultiGridNormalDistributionsTransform::computeS if (d_phi_0 >= 0) { // Not a decent direction - if (d_phi_0 == 0) return 0; + if (d_phi_0 == 0) { + return 0; + } // Reverse step direction and calculate optimal step. d_phi_0 *= -1; @@ -1051,9 +1059,11 @@ double MultiGridNormalDistributionsTransform::computeS // If inner loop was run then hessian needs to be calculated. // Hessian is unnecessary for step length determination but gradients are required // so derivative and transform data is stored for the next iteration. - if (step_iterations) computeHessian(hessian, trans_cloud, x_t); + if (step_iterations) { + computeHessian(hessian, trans_cloud, x_t); + } - return (a_t); + return a_t; } template @@ -1235,6 +1245,6 @@ pcl::PointCloud MultiGridNormalDistributionsTransform #include -namespace pclomp +namespace autoware::ndt_omp::pclomp { /** \brief GeneralizedIterativeClosestPoint is an ICP variant that implements the * generalized iterative closest point algorithm as described by Alex Segal et al. in @@ -93,22 +93,22 @@ class GeneralizedIterativeClosestPoint : public pcl::IterativeClosestPoint::KdTree; using InputKdTreePtr = typename pcl::Registration::KdTreePtr; - using MatricesVector = std::vector >; + using MatricesVector = std::vector>; #if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0) using MatricesVectorPtr = pcl::shared_ptr; using MatricesVectorConstPtr = pcl::shared_ptr; - using Ptr = pcl::shared_ptr >; + using Ptr = pcl::shared_ptr>; using ConstPtr = - pcl::shared_ptr >; + pcl::shared_ptr>; #else using MatricesVectorPtr = boost::shared_ptr; using MatricesVectorConstPtr = boost::shared_ptr; - using Ptr = boost::shared_ptr >; + using Ptr = boost::shared_ptr>; using ConstPtr = - boost::shared_ptr >; + boost::shared_ptr>; #endif using Vector6d = Eigen::Matrix; @@ -149,7 +149,9 @@ class GeneralizedIterativeClosestPoint : public pcl::IterativeClosestPoint::setInputSource(cloud); input_covariances_.reset(); @@ -222,7 +224,7 @@ class GeneralizedIterativeClosestPoint : public pcl::IterativeClosestPoint & index, std::vector & distance) const { int k = tree_->nearestKSearch(query, 1, index, distance); - if (k == 0) return (false); - return (true); + if (k == 0) { + return false; + } + return true; } /// \brief compute transformation matrix from transformation matrix @@ -349,14 +356,14 @@ class GeneralizedIterativeClosestPoint : public pcl::IterativeClosestPoint & cloud_src, const std::vector & src_indices, const pcl::PointCloud & cloud_tgt, const std::vector & tgt_indices, Eigen::Matrix4f & transformation_matrix)> rigid_transformation_estimation_; -}; -} // namespace pclomp +} +} // namespace autoware::ndt_omp::pclomp #endif // #ifndef PCL_GICP_H_ diff --git a/include/pclomp/gicp_omp_impl.hpp b/include/autoware/ndt_omp/pclomp/gicp_omp_impl.hpp similarity index 92% rename from include/pclomp/gicp_omp_impl.hpp rename to include/autoware/ndt_omp/pclomp/gicp_omp_impl.hpp index 8a1409f7..4a86ff99 100644 --- a/include/pclomp/gicp_omp_impl.hpp +++ b/include/autoware/ndt_omp/pclomp/gicp_omp_impl.hpp @@ -51,9 +51,10 @@ //////////////////////////////////////////////////////////////////////////////////////// template template -void pclomp::GeneralizedIterativeClosestPoint::computeCovariances( - typename pcl::PointCloud::ConstPtr cloud, - const typename pcl::search::KdTree::ConstPtr kdtree, MatricesVector & cloud_covariances) +void autoware::ndt_omp::pclomp::GeneralizedIterativeClosestPoint:: + computeCovariances( + typename pcl::PointCloud::ConstPtr cloud, + const typename pcl::search::KdTree::ConstPtr kdtree, MatricesVector & cloud_covariances) { if (k_correspondences_ > static_cast(cloud->size())) { PCL_ERROR( @@ -64,7 +65,9 @@ void pclomp::GeneralizedIterativeClosestPoint::compute } // We should never get there but who knows - if (cloud_covariances.size() < cloud->size()) cloud_covariances.resize(cloud->size()); + if (cloud_covariances.size() < cloud->size()) { + cloud_covariances.resize(cloud->size()); + } std::vector> nn_indices_array(omp_get_max_threads()); std::vector> nn_dist_sq_array(omp_get_max_threads()); @@ -103,12 +106,13 @@ void pclomp::GeneralizedIterativeClosestPoint::compute mean /= static_cast(k_correspondences_); // Get the actual covariance - for (int k = 0; k < 3; k++) + for (int k = 0; k < 3; k++) { for (int l = 0; l <= k; l++) { cov(k, l) /= static_cast(k_correspondences_); cov(k, l) -= mean[k] * mean[l]; cov(l, k) = cov(k, l); } + } // Compute the SVD (covariance matrix is symmetric so U = V') Eigen::JacobiSVD svd(cov, Eigen::ComputeFullU); @@ -119,8 +123,9 @@ void pclomp::GeneralizedIterativeClosestPoint::compute for (int k = 0; k < 3; k++) { Eigen::Vector3d col = U.col(k); double v = 1.; // biggest 2 singular values replaced by 1 - if (k == 2) // smallest singular value replaced by gicp_epsilon + if (k == 2) { // smallest singular value replaced by gicp_epsilon v = gicp_epsilon_; + } cov += v * col * col.transpose(); } } @@ -128,8 +133,8 @@ void pclomp::GeneralizedIterativeClosestPoint::compute //////////////////////////////////////////////////////////////////////////////////////// template -void pclomp::GeneralizedIterativeClosestPoint::computeRDerivative( - const Vector6d & x, const Eigen::Matrix3d & R, Vector6d & g) const +void autoware::ndt_omp::pclomp::GeneralizedIterativeClosestPoint:: + computeRDerivative(const Vector6d & x, const Eigen::Matrix3d & R, Vector6d & g) const { Eigen::Matrix3d dR_dPhi; Eigen::Matrix3d dR_dTheta; @@ -184,14 +189,13 @@ void pclomp::GeneralizedIterativeClosestPoint::compute //////////////////////////////////////////////////////////////////////////////////////// template -void pclomp::GeneralizedIterativeClosestPoint:: +void autoware::ndt_omp::pclomp::GeneralizedIterativeClosestPoint:: estimateRigidTransformationBFGS( const PointCloudSource & cloud_src, const std::vector & indices_src, const PointCloudTarget & cloud_tgt, const std::vector & indices_tgt, Eigen::Matrix4f & transformation_matrix) { - if (indices_src.size() < 4) // need at least 4 samples - { + if (indices_src.size() < 4) { // need at least 4 samples PCL_THROW_EXCEPTION( pcl::NotEnoughPointsException, "[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS] Need at least 4 " @@ -253,7 +257,7 @@ void pclomp::GeneralizedIterativeClosestPoint:: //////////////////////////////////////////////////////////////////////////////////////// template -inline double pclomp::GeneralizedIterativeClosestPoint< +inline double autoware::ndt_omp::pclomp::GeneralizedIterativeClosestPoint< PointSource, PointTarget>::OptimizationFunctorWithIndices::operator()(const Vector6d & x) { Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_; @@ -288,7 +292,7 @@ inline double pclomp::GeneralizedIterativeClosestPoint< //////////////////////////////////////////////////////////////////////////////////////// template -inline void pclomp::GeneralizedIterativeClosestPoint< +inline void autoware::ndt_omp::pclomp::GeneralizedIterativeClosestPoint< PointSource, PointTarget>::OptimizationFunctorWithIndices::df(const Vector6d & x, Vector6d & g) { Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_; @@ -347,7 +351,7 @@ inline void pclomp::GeneralizedIterativeClosestPoint< //////////////////////////////////////////////////////////////////////////////////////// template -inline void pclomp::GeneralizedIterativeClosestPoint:: +inline void autoware::ndt_omp::pclomp::GeneralizedIterativeClosestPoint:: OptimizationFunctorWithIndices::fdf(const Vector6d & x, double & f, Vector6d & g) { Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_; @@ -390,9 +394,8 @@ inline void pclomp::GeneralizedIterativeClosestPoint:: //////////////////////////////////////////////////////////////////////////////////////// template -inline void -pclomp::GeneralizedIterativeClosestPoint::computeTransformation( - PointCloudSource & output, const Eigen::Matrix4f & guess) +inline void autoware::ndt_omp::pclomp::GeneralizedIterativeClosestPoint:: + computeTransformation(PointCloudSource & output, const Eigen::Matrix4f & guess) { pcl::IterativeClosestPoint::initComputeReciprocal(); // Difference between consecutive transforms @@ -435,11 +438,14 @@ pclomp::GeneralizedIterativeClosestPoint::computeTrans // guess corresponds to base_t and transformation_ to t Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero(); - for (size_t i = 0; i < 4; i++) - for (size_t j = 0; j < 4; j++) - for (size_t k = 0; k < 4; k++) + for (size_t i = 0; i < 4; i++) { + for (size_t j = 0; j < 4; j++) { + for (size_t k = 0; k < 4; k++) { transform_R(i, j) += static_cast(transformation_(i, k)) * static_cast(guess(k, j)); + } + } + } const Eigen::Matrix3d R = transform_R.topLeftCorner<3, 3>(); @@ -512,12 +518,15 @@ pclomp::GeneralizedIterativeClosestPoint::computeTrans for (int k = 0; k < 4; k++) { for (int l = 0; l < 4; l++) { double ratio = 1; - if (k < 3 && l < 3) // rotation part of the transform + if (k < 3 && l < 3) { // rotation part of the transform ratio = 1. / rotation_epsilon_; - else + } else { ratio = 1. / transformation_epsilon_; + } double c_delta = ratio * std::abs(previous_transformation_(k, l) - transformation_(k, l)); - if (c_delta > delta) delta = c_delta; + if (c_delta > delta) { + delta = c_delta; + } } } } catch (pcl::PCLException & e) { @@ -547,8 +556,8 @@ pclomp::GeneralizedIterativeClosestPoint::computeTrans } template -void pclomp::GeneralizedIterativeClosestPoint::applyState( - Eigen::Matrix4f & t, const Vector6d & x) const +void autoware::ndt_omp::pclomp::GeneralizedIterativeClosestPoint< + PointSource, PointTarget>::applyState(Eigen::Matrix4f & t, const Vector6d & x) const { // !!! CAUTION Stanford GICP uses the Z Y X euler angles convention Eigen::Matrix3f R; diff --git a/include/pclomp/ndt_omp.h b/include/autoware/ndt_omp/pclomp/ndt_omp.h similarity index 96% rename from include/pclomp/ndt_omp.h rename to include/autoware/ndt_omp/pclomp/ndt_omp.h index 87062151..b33c609f 100644 --- a/include/pclomp/ndt_omp.h +++ b/include/autoware/ndt_omp/pclomp/ndt_omp.h @@ -51,16 +51,15 @@ #include -namespace pclomp +namespace autoware::ndt_omp::pclomp { struct EigenCmp { bool operator()(const Eigen::Vector3i & a, const Eigen::Vector3i & b) const { - return ( - a(0) < b(0) || (a(0) == b(0) && a(1) < b(1)) || - (a(0) == b(0) && a(1) == b(1) && a(2) < b(2))); + return a(0) < b(0) || (a(0) == b(0) && a(1) < b(1)) || + (a(0) == b(0) && a(1) == b(1) && a(2) < b(2)); } }; @@ -91,7 +90,7 @@ class NormalDistributionsTransform : public pcl::Registration TargetGrid; + typedef autoware::ndt_omp::pclomp::VoxelGridCovariance TargetGrid; /** \brief Typename of pointer to searchable voxel grid. */ typedef TargetGrid * TargetGridPtr; /** \brief Typename of const pointer to searchable voxel grid. */ @@ -137,19 +136,21 @@ class NormalDistributionsTransform : public pcl::Registration getHessian() const { return hessian_; } @@ -281,7 +282,7 @@ class NormalDistributionsTransform : public pcl::Registration -pclomp::NormalDistributionsTransform::NormalDistributionsTransform() +autoware::ndt_omp::pclomp::NormalDistributionsTransform< + PointSource, PointTarget>::NormalDistributionsTransform() : target_cells_(), outlier_ratio_(0.55), gauss_d1_(), @@ -82,8 +83,8 @@ pclomp::NormalDistributionsTransform::NormalDistributi ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template -void pclomp::NormalDistributionsTransform::computeTransformation( - PointCloudSource & output, const Eigen::Matrix4f & guess) +void autoware::ndt_omp::pclomp::NormalDistributionsTransform:: + computeTransformation(PointCloudSource & output, const Eigen::Matrix4f & guess) { nr_iterations_ = 0; converged_ = false; @@ -178,8 +179,9 @@ void pclomp::NormalDistributionsTransform::computeTran p = p + delta_p; // Update Visualizer (untested) - if (update_visualizer_ != 0) + if (update_visualizer_ != 0) { update_visualizer_(output, std::vector(), *target_, std::vector()); + } if ( nr_iterations_ > params_.max_iterations || @@ -214,9 +216,10 @@ int omp_get_thread_num() ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template -double pclomp::NormalDistributionsTransform::computeDerivatives( - Eigen::Matrix & score_gradient, Eigen::Matrix & hessian, - PointCloudSource & trans_cloud, Eigen::Matrix & p, bool compute_hessian) +double autoware::ndt_omp::pclomp::NormalDistributionsTransform:: + computeDerivatives( + Eigen::Matrix & score_gradient, Eigen::Matrix & hessian, + PointCloudSource & trans_cloud, Eigen::Matrix & p, bool compute_hessian) { score_gradient.setZero(); hessian.setZero(); @@ -388,13 +391,13 @@ double pclomp::NormalDistributionsTransform::computeDe nearest_voxel_transformation_likelihood_ = 0.0; } - return (score); + return score; } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template -void pclomp::NormalDistributionsTransform::computeAngleDerivatives( - Eigen::Matrix & p, bool compute_hessian) +void autoware::ndt_omp::pclomp::NormalDistributionsTransform:: + computeAngleDerivatives(Eigen::Matrix & p, bool compute_hessian) { // Simplified math for near 0 angles double cx = NAN; @@ -539,9 +542,10 @@ void pclomp::NormalDistributionsTransform::computeAngl ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template -void pclomp::NormalDistributionsTransform::computePointDerivatives( - Eigen::Vector3d & x, Eigen::Matrix & point_gradient_, - Eigen::Matrix & point_hessian_, bool compute_hessian) const +void autoware::ndt_omp::pclomp::NormalDistributionsTransform:: + computePointDerivatives( + Eigen::Vector3d & x, Eigen::Matrix & point_gradient_, + Eigen::Matrix & point_hessian_, bool compute_hessian) const { Eigen::Vector4f x4( static_cast(x[0]), static_cast(x[1]), static_cast(x[2]), 0.0f); @@ -588,9 +592,10 @@ void pclomp::NormalDistributionsTransform::computePoin ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template -void pclomp::NormalDistributionsTransform::computePointDerivatives( - Eigen::Vector3d & x, Eigen::Matrix & point_gradient_, - Eigen::Matrix & point_hessian_, bool compute_hessian) const +void autoware::ndt_omp::pclomp::NormalDistributionsTransform:: + computePointDerivatives( + Eigen::Vector3d & x, Eigen::Matrix & point_gradient_, + Eigen::Matrix & point_hessian_, bool compute_hessian) const { // Calculate first derivative of Transformation Equation 6.17 w.r.t. transform vector p. // Derivative w.r.t. ith element of transform vector corresponds to column i, Equation 6.18 @@ -637,11 +642,12 @@ void pclomp::NormalDistributionsTransform::computePoin ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template -double pclomp::NormalDistributionsTransform::updateDerivatives( - Eigen::Matrix & score_gradient, Eigen::Matrix & hessian, - const Eigen::Matrix & point_gradient4, - const Eigen::Matrix & point_hessian_, const Eigen::Vector3d & x_trans, - const Eigen::Matrix3d & c_inv, bool compute_hessian) const +double autoware::ndt_omp::pclomp::NormalDistributionsTransform:: + updateDerivatives( + Eigen::Matrix & score_gradient, Eigen::Matrix & hessian, + const Eigen::Matrix & point_gradient4, + const Eigen::Matrix & point_hessian_, const Eigen::Vector3d & x_trans, + const Eigen::Matrix3d & c_inv, bool compute_hessian) const { Eigen::Matrix x_trans4( static_cast(x_trans[0]), static_cast(x_trans[1]), static_cast(x_trans[2]), @@ -659,7 +665,9 @@ double pclomp::NormalDistributionsTransform::updateDer e_x_cov_x = gauss_d2 * e_x_cov_x; // Error checking for invalid values. - if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x) return (0); + if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x) { + return 0; + } // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] e_x_cov_x *= gauss_d1_; @@ -693,14 +701,15 @@ double pclomp::NormalDistributionsTransform::updateDer } } - return (score_inc); + return score_inc; } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template -void pclomp::NormalDistributionsTransform::computeHessian( - Eigen::Matrix & hessian, PointCloudSource & trans_cloud, - Eigen::Matrix &) +void autoware::ndt_omp::pclomp::NormalDistributionsTransform:: + computeHessian( + Eigen::Matrix & hessian, PointCloudSource & trans_cloud, + Eigen::Matrix &) { // Original Point and Transformed Point PointSource x_pt; @@ -771,17 +780,20 @@ void pclomp::NormalDistributionsTransform::computeHess ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template -void pclomp::NormalDistributionsTransform::updateHessian( - Eigen::Matrix & hessian, const Eigen::Matrix & point_gradient_, - const Eigen::Matrix & point_hessian_, const Eigen::Vector3d & x_trans, - const Eigen::Matrix3d & c_inv) const +void autoware::ndt_omp::pclomp::NormalDistributionsTransform:: + updateHessian( + Eigen::Matrix & hessian, const Eigen::Matrix & point_gradient_, + const Eigen::Matrix & point_hessian_, const Eigen::Vector3d & x_trans, + const Eigen::Matrix3d & c_inv) const { Eigen::Vector3d cov_dxd_pi; // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009] double e_x_cov_x = gauss_d2_ * exp(-gauss_d2_ * x_trans.dot(c_inv * x_trans) / 2); // Error checking for invalid values. - if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x) return; + if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x) { + return; + } // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] e_x_cov_x *= gauss_d1_; @@ -802,23 +814,24 @@ void pclomp::NormalDistributionsTransform::updateHessi ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template -bool pclomp::NormalDistributionsTransform::updateIntervalMT( - double & a_l, double & f_l, double & g_l, double & a_u, double & f_u, double & g_u, double a_t, - double f_t, double g_t) +bool autoware::ndt_omp::pclomp::NormalDistributionsTransform:: + updateIntervalMT( + double & a_l, double & f_l, double & g_l, double & a_u, double & f_u, double & g_u, double a_t, + double f_t, double g_t) { // Case U1 in Update Algorithm and Case a in Modified Update Algorithm [More, Thuente 1994] if (f_t > f_l) { a_u = a_t; f_u = f_t; g_u = g_t; - return (false); + return false; } // Case U2 in Update Algorithm and Case b in Modified Update Algorithm [More, Thuente 1994] if (g_t * (a_l - a_t) > 0) { a_l = a_t; f_l = f_t; g_l = g_t; - return (false); + return false; } // Case U3 in Update Algorithm and Case c in Modified Update Algorithm [More, Thuente 1994] if (g_t * (a_l - a_t) < 0) { @@ -829,17 +842,18 @@ bool pclomp::NormalDistributionsTransform::updateInter a_l = a_t; f_l = f_t; g_l = g_t; - return (false); + return false; } // Interval Converged - return (true); + return true; } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template -double pclomp::NormalDistributionsTransform::trialValueSelectionMT( - double a_l, double f_l, double g_l, double a_u, double f_u, double g_u, double a_t, double f_t, - double g_t) +double autoware::ndt_omp::pclomp::NormalDistributionsTransform:: + trialValueSelectionMT( + double a_l, double f_l, double g_l, double a_u, double f_u, double g_u, double a_t, double f_t, + double g_t) { // Case 1 in Trial Value Selection [More, Thuente 1994] if (f_t > f_l) { @@ -855,9 +869,9 @@ double pclomp::NormalDistributionsTransform::trialValu double a_q = a_l - 0.5 * (a_l - a_t) * g_l / (g_l - (f_l - f_t) / (a_l - a_t)); if (std::fabs(a_c - a_l) < std::fabs(a_q - a_l)) { - return (a_c); + return a_c; } - return (0.5 * (a_q + a_c)); + return 0.5 * (a_q + a_c); } // Case 2 in Trial Value Selection [More, Thuente 1994] if (g_t * g_l < 0) { @@ -873,9 +887,9 @@ double pclomp::NormalDistributionsTransform::trialValu double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l; if (std::fabs(a_c - a_t) >= std::fabs(a_s - a_t)) { - return (a_c); + return a_c; } - return (a_s); + return a_s; } // Case 3 in Trial Value Selection [More, Thuente 1994] if (std::fabs(g_t) <= std::fabs(g_l)) { @@ -891,15 +905,16 @@ double pclomp::NormalDistributionsTransform::trialValu double a_t_next = NAN; - if (std::fabs(a_c - a_t) < std::fabs(a_s - a_t)) + if (std::fabs(a_c - a_t) < std::fabs(a_s - a_t)) { a_t_next = a_c; - else + } else { a_t_next = a_s; + } if (a_t > a_l) { - return (std::min(a_t + 0.66 * (a_u - a_t), a_t_next)); + return std::min(a_t + 0.66 * (a_u - a_t), a_t_next); } - return (std::max(a_t + 0.66 * (a_u - a_t), a_t_next)); + return std::max(a_t + 0.66 * (a_u - a_t), a_t_next); } // Case 4 in Trial Value Selection [More, Thuente 1994] // Calculate the minimizer of the cubic that interpolates f_u, f_t, g_u and g_t @@ -907,15 +922,16 @@ double pclomp::NormalDistributionsTransform::trialValu double z = 3 * (f_t - f_u) / (a_t - a_u) - g_t - g_u; double w = std::sqrt(z * z - g_t * g_u); // Equation 2.4.56 [Sun, Yuan 2006] - return (a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w)); + return a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template -double pclomp::NormalDistributionsTransform::computeStepLengthMT( - const Eigen::Matrix & x, Eigen::Matrix & step_dir, double step_init, - double step_max, double step_min, double & score, Eigen::Matrix & score_gradient, - Eigen::Matrix & hessian, PointCloudSource & trans_cloud) +double autoware::ndt_omp::pclomp::NormalDistributionsTransform:: + computeStepLengthMT( + const Eigen::Matrix & x, Eigen::Matrix & step_dir, double step_init, + double step_max, double step_min, double & score, Eigen::Matrix & score_gradient, + Eigen::Matrix & hessian, PointCloudSource & trans_cloud) { // Set the value of phi(0), Equation 1.3 [More, Thuente 1994] double phi_0 = -score; @@ -926,7 +942,9 @@ double pclomp::NormalDistributionsTransform::computeSt if (d_phi_0 >= 0) { // Not a decent direction - if (d_phi_0 == 0) return 0; + if (d_phi_0 == 0) { + return 0; + } // Reverse step direction and calculate optimal step. d_phi_0 *= -1; @@ -1073,14 +1091,17 @@ double pclomp::NormalDistributionsTransform::computeSt // If inner loop was run then hessian needs to be calculated. // Hessian is unnecessary for step length determination but gradients are required // so derivative and transform data is stored for the next iteration. - if (step_iterations) computeHessian(hessian, trans_cloud, x_t); + if (step_iterations) { + computeHessian(hessian, trans_cloud, x_t); + } - return (a_t); + return a_t; } // change at 20220721 konishi template -double pclomp::NormalDistributionsTransform::calculateScore( +double +autoware::ndt_omp::pclomp::NormalDistributionsTransform::calculateScore( const PointCloudSource & trans_cloud) { double score = 0; @@ -1167,9 +1188,8 @@ double pclomp::NormalDistributionsTransform::calculate } template -double -pclomp::NormalDistributionsTransform::calculateTransformationProbability( - const PointCloudSource & trans_cloud) const +double autoware::ndt_omp::pclomp::NormalDistributionsTransform:: + calculateTransformationProbability(const PointCloudSource & trans_cloud) const { double score = 0; @@ -1220,7 +1240,7 @@ pclomp::NormalDistributionsTransform::calculateTransfo } template -double pclomp::NormalDistributionsTransform:: +double autoware::ndt_omp::pclomp::NormalDistributionsTransform:: calculateNearestVoxelTransformationLikelihood(const PointCloudSource & trans_cloud) const { double nearest_voxel_score = 0; diff --git a/include/pclomp/ndt_struct.hpp b/include/autoware/ndt_omp/pclomp/ndt_struct.hpp similarity index 97% rename from include/pclomp/ndt_struct.hpp rename to include/autoware/ndt_omp/pclomp/ndt_struct.hpp index b613a8e5..314f61a5 100644 --- a/include/pclomp/ndt_struct.hpp +++ b/include/autoware/ndt_omp/pclomp/ndt_struct.hpp @@ -59,7 +59,7 @@ #include -namespace pclomp +namespace autoware::ndt_omp::pclomp { enum NeighborSearchMethod { KDTREE, DIRECT26, DIRECT7, DIRECT1 }; @@ -102,6 +102,6 @@ struct NdtParams bool use_line_search = false; }; -} // namespace pclomp +} // namespace autoware::ndt_omp::pclomp #endif // PCLOMP_NDT_STRUCT_HPP_ diff --git a/include/pclomp/voxel_grid_covariance_omp.h b/include/autoware/ndt_omp/pclomp/voxel_grid_covariance_omp.h similarity index 94% rename from include/pclomp/voxel_grid_covariance_omp.h rename to include/autoware/ndt_omp/pclomp/voxel_grid_covariance_omp.h index 373a52a2..94919bd1 100644 --- a/include/pclomp/voxel_grid_covariance_omp.h +++ b/include/autoware/ndt_omp/pclomp/voxel_grid_covariance_omp.h @@ -50,7 +50,7 @@ #include #include -namespace pclomp +namespace autoware::ndt_omp::pclomp { /** \brief A searchable voxel structure containing the mean and covariance of the data. * \note For more information please see @@ -89,11 +89,11 @@ class VoxelGridCovariance : public pcl::VoxelGrid public: #if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0) - typedef pcl::shared_ptr > Ptr; - typedef pcl::shared_ptr > ConstPtr; + typedef pcl::shared_ptr> Ptr; + typedef pcl::shared_ptr> ConstPtr; #else - typedef boost::shared_ptr > Ptr; - typedef boost::shared_ptr > ConstPtr; + typedef boost::shared_ptr> Ptr; + typedef boost::shared_ptr> ConstPtr; #endif /** \brief Simple structure to hold a centroid, covariance and the number of points in a leaf. @@ -120,36 +120,36 @@ class VoxelGridCovariance : public pcl::VoxelGrid /** \brief Get the voxel covariance. * \return covariance matrix */ - Eigen::Matrix3d getCov() const { return (cov_); } + Eigen::Matrix3d getCov() const { return cov_; } /** \brief Get the inverse of the voxel covariance. * \return inverse covariance matrix */ - Eigen::Matrix3d getInverseCov() const { return (icov_); } + Eigen::Matrix3d getInverseCov() const { return icov_; } /** \brief Get the voxel centroid. * \return centroid */ - Eigen::Vector3d getMean() const { return (mean_); } + Eigen::Vector3d getMean() const { return mean_; } // add at 20220721 by konishi - Eigen::Vector3d getLeafCenter() const { return (voxelXYZ_); } + Eigen::Vector3d getLeafCenter() const { return voxelXYZ_; } /** \brief Get the eigen vectors of the voxel covariance. * \note Order corresponds with \ref getEvals * \return matrix whose columns contain eigen vectors */ - Eigen::Matrix3d getEvecs() const { return (evecs_); } + Eigen::Matrix3d getEvecs() const { return evecs_; } /** \brief Get the eigen values of the voxel covariance. * \note Order corresponds with \ref getEvecs * \return vector of eigen values */ - Eigen::Vector3d getEvals() const { return (evals_); } + Eigen::Vector3d getEvals() const { return evals_; } /** \brief Get the number of points contained by this voxel. * \return number of points */ - int getPointCount() const { return (nr_points_); } + int getPointCount() const { return nr_points_; } /** \brief Number of points contained by voxel */ int nr_points_; @@ -284,8 +284,9 @@ class VoxelGridCovariance : public pcl::VoxelGrid if (leaf_iter != leaves_.end()) { LeafConstPtr ret(&(leaf_iter->second)); return ret; - } else + } else { return NULL; + } } /** \brief Get the voxel containing point p. @@ -308,8 +309,9 @@ class VoxelGridCovariance : public pcl::VoxelGrid // If such a leaf exists return the pointer to the leaf structure LeafConstPtr ret(&(leaf_iter->second)); return ret; - } else + } else { return NULL; + } } /** \brief Get the voxel containing point p. @@ -332,8 +334,9 @@ class VoxelGridCovariance : public pcl::VoxelGrid // If such a leaf exists return the pointer to the leaf structure LeafConstPtr ret(&(leaf_iter->second)); return ret; - } else + } else { return NULL; + } } // add at 20220218 by konishi @@ -345,7 +348,7 @@ class VoxelGridCovariance : public pcl::VoxelGrid int ijk2 = static_cast(floor(p[2] * inverse_leaf_size_[2]) - min_b_[2]); // Compute the centroid leaf index - return (ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]); + return ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; } // add at 20220721 by konishi @@ -442,8 +445,10 @@ class VoxelGridCovariance : public pcl::VoxelGrid const PointCloud & cloud, int index, int k, std::vector & k_leaves, std::vector & k_sqr_distances) { - if (index >= static_cast(cloud.points.size()) || index < 0) return (0); - return (nearestKSearch(cloud.points[index], k, k_leaves, k_sqr_distances)); + if (index >= static_cast(cloud.points.size()) || index < 0) { + return 0; + } + return nearestKSearch(cloud.points[index], k, k_leaves, k_sqr_distances); } /** \brief Search for all the nearest occupied voxels of the query point in a given radius. @@ -498,8 +503,10 @@ class VoxelGridCovariance : public pcl::VoxelGrid const PointCloud & cloud, int index, double radius, std::vector & k_leaves, std::vector & k_sqr_distances, unsigned int max_nn = 0) const { - if (index >= static_cast(cloud.points.size()) || index < 0) return (0); - return (radiusSearch(cloud.points[index], radius, k_leaves, k_sqr_distances, max_nn)); + if (index >= static_cast(cloud.points.size()) || index < 0) { + return 0; + } + return radiusSearch(cloud.points[index], radius, k_leaves, k_sqr_distances, max_nn); } protected: @@ -532,7 +539,7 @@ class VoxelGridCovariance : public pcl::VoxelGrid /** \brief KdTree generated using \ref voxel_centroids_ (used for searching). */ pcl::KdTreeFLANN kdtree_; -}; -} // namespace pclomp +} +} // namespace autoware::ndt_omp::pclomp #endif // #ifndef PCL_VOXEL_GRID_COVARIANCE_H_ diff --git a/include/pclomp/voxel_grid_covariance_omp_impl.hpp b/include/autoware/ndt_omp/pclomp/voxel_grid_covariance_omp_impl.hpp similarity index 92% rename from include/pclomp/voxel_grid_covariance_omp_impl.hpp rename to include/autoware/ndt_omp/pclomp/voxel_grid_covariance_omp_impl.hpp index 5d0a8430..5480c9f6 100644 --- a/include/pclomp/voxel_grid_covariance_omp_impl.hpp +++ b/include/autoware/ndt_omp/pclomp/voxel_grid_covariance_omp_impl.hpp @@ -52,7 +52,7 @@ ////////////////////////////////////////////////////////////////////////////////////////// template -void pclomp::VoxelGridCovariance::applyFilter(PointCloud & output) +void autoware::ndt_omp::pclomp::VoxelGridCovariance::applyFilter(PointCloud & output) { voxel_centroids_leaf_indices_.clear(); @@ -72,12 +72,13 @@ void pclomp::VoxelGridCovariance::applyFilter(PointCloud & output) Eigen::Vector4f min_p; Eigen::Vector4f max_p; // Get the minimum and maximum dimensions - if (!filter_field_name_.empty()) // If we don't want to process the entire cloud... + if (!filter_field_name_.empty()) { // If we don't want to process the entire cloud... pcl::getMinMax3D( input_, filter_field_name_, static_cast(filter_limit_min_), static_cast(filter_limit_max_), min_p, max_p, filter_limit_negative_); - else + } else { pcl::getMinMax3D(*input_, min_p, max_p); + } // Check that the leaf size is not too small, given the size of the data int64_t dx = static_cast((max_p[0] - min_p[0]) * inverse_leaf_size_[0]) + 1; @@ -114,13 +115,17 @@ void pclomp::VoxelGridCovariance::applyFilter(PointCloud & output) int centroid_size = 4; - if (downsample_all_data_) centroid_size = boost::mpl::size::value; + if (downsample_all_data_) { + centroid_size = boost::mpl::size::value; + } // ---[ RGB special case std::vector fields; int rgba_index = -1; rgba_index = pcl::getFieldIndex("rgb", fields); - if (rgba_index == -1) rgba_index = pcl::getFieldIndex("rgba", fields); + if (rgba_index == -1) { + rgba_index = pcl::getFieldIndex("rgba", fields); + } if (rgba_index >= 0) { rgba_index = static_cast(fields[rgba_index].offset); centroid_size += 3; @@ -132,19 +137,22 @@ void pclomp::VoxelGridCovariance::applyFilter(PointCloud & output) // Get the distance field index std::vector distance_fields; int distance_idx = pcl::getFieldIndex(filter_field_name_, distance_fields); - if (distance_idx == -1) + if (distance_idx == -1) { PCL_WARN( "[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName().c_str(), distance_idx); + } // First pass: go over all points and insert them into the right leaf for (size_t cp = 0; cp < input_->points.size(); ++cp) { - if (!input_->is_dense) + if (!input_->is_dense) { // Check if the point is invalid if ( !std::isfinite(input_->points[cp].x) || !std::isfinite(input_->points[cp].y) || - !std::isfinite(input_->points[cp].z)) + !std::isfinite(input_->points[cp].z)) { continue; + } + } // Get the distance value const auto * pt_data = reinterpret_cast(&input_->points[cp]); @@ -153,10 +161,14 @@ void pclomp::VoxelGridCovariance::applyFilter(PointCloud & output) if (filter_limit_negative_) { // Use a threshold for cutting out points which inside the interval - if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_)) continue; + if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_)) { + continue; + } } else { // Use a threshold for cutting out points which are too close/far away - if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_)) continue; + if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_)) { + continue; + } } int ijk0 = static_cast( @@ -209,12 +221,14 @@ void pclomp::VoxelGridCovariance::applyFilter(PointCloud & output) } else { // First pass: go over all points and insert them into the right leaf for (size_t cp = 0; cp < input_->points.size(); ++cp) { - if (!input_->is_dense) + if (!input_->is_dense) { // Check if the point is invalid if ( !std::isfinite(input_->points[cp].x) || !std::isfinite(input_->points[cp].y) || - !std::isfinite(input_->points[cp].z)) + !std::isfinite(input_->points[cp].z)) { continue; + } + } int ijk0 = static_cast( floor(input_->points[cp].x * inverse_leaf_size_[0]) - static_cast(min_b_[0])); @@ -265,9 +279,13 @@ void pclomp::VoxelGridCovariance::applyFilter(PointCloud & output) // Second pass: go over all leaves and compute centroids and covariance matrices output.points.reserve(leaves_.size()); - if (searchable_) voxel_centroids_leaf_indices_.reserve(leaves_.size()); + if (searchable_) { + voxel_centroids_leaf_indices_.reserve(leaves_.size()); + } int cp = 0; - if (save_leaf_layout_) leaf_layout_.resize(div_b_[0] * div_b_[1] * div_b_[2], -1); + if (save_leaf_layout_) { + leaf_layout_.resize(div_b_[0] * div_b_[1] * div_b_[2], -1); + } // Eigen values and vectors calculated to prevent near singular matrices Eigen::SelfAdjointEigenSolver eigensolver; @@ -293,7 +311,9 @@ void pclomp::VoxelGridCovariance::applyFilter(PointCloud & output) // voxel centroids and output clouds. Points with less than the minimum points will have a can // not be accurately approximated using a normal distribution. if (leaf.nr_points_ >= min_points_per_voxel_) { - if (save_leaf_layout_) leaf_layout_[it->first] = cp++; + if (save_leaf_layout_) { + leaf_layout_[it->first] = cp++; + } output.push_back(PointT()); @@ -320,7 +340,9 @@ void pclomp::VoxelGridCovariance::applyFilter(PointCloud & output) } // Stores the voxel indices for fast access searching - if (searchable_) voxel_centroids_leaf_indices_.push_back(static_cast(it->first)); + if (searchable_) { + voxel_centroids_leaf_indices_.push_back(static_cast(it->first)); + } // Single pass covariance calculation leaf.cov_ = (leaf.cov_ - 2 * (pt_sum * leaf.mean_.transpose())) / leaf.nr_points_ + @@ -365,7 +387,7 @@ void pclomp::VoxelGridCovariance::applyFilter(PointCloud & output) ////////////////////////////////////////////////////////////////////////////////////////// template -int pclomp::VoxelGridCovariance::getNeighborhoodAtPoint( +int autoware::ndt_omp::pclomp::VoxelGridCovariance::getNeighborhoodAtPoint( const Eigen::MatrixXi & relative_coordinates, const PointT & reference_point, std::vector & neighbors) const { @@ -395,12 +417,12 @@ int pclomp::VoxelGridCovariance::getNeighborhoodAtPoint( } } - return (static_cast(neighbors.size())); + return static_cast(neighbors.size()); } ////////////////////////////////////////////////////////////////////////////////////////// template -int pclomp::VoxelGridCovariance::getNeighborhoodAtPoint( +int autoware::ndt_omp::pclomp::VoxelGridCovariance::getNeighborhoodAtPoint( const PointT & reference_point, std::vector & neighbors) const { neighbors.clear(); @@ -412,7 +434,7 @@ int pclomp::VoxelGridCovariance::getNeighborhoodAtPoint( ////////////////////////////////////////////////////////////////////////////////////////// template -int pclomp::VoxelGridCovariance::getNeighborhoodAtPoint7( +int autoware::ndt_omp::pclomp::VoxelGridCovariance::getNeighborhoodAtPoint7( const PointT & reference_point, std::vector & neighbors) const { neighbors.clear(); @@ -431,7 +453,7 @@ int pclomp::VoxelGridCovariance::getNeighborhoodAtPoint7( ////////////////////////////////////////////////////////////////////////////////////////// template -int pclomp::VoxelGridCovariance::getNeighborhoodAtPoint1( +int autoware::ndt_omp::pclomp::VoxelGridCovariance::getNeighborhoodAtPoint1( const PointT & reference_point, std::vector & neighbors) const { neighbors.clear(); @@ -440,7 +462,7 @@ int pclomp::VoxelGridCovariance::getNeighborhoodAtPoint1( ////////////////////////////////////////////////////////////////////////////////////////// template -void pclomp::VoxelGridCovariance::getDisplayCloud( +void autoware::ndt_omp::pclomp::VoxelGridCovariance::getDisplayCloud( pcl::PointCloud & cell_cloud) { cell_cloud.clear(); @@ -448,7 +470,7 @@ void pclomp::VoxelGridCovariance::getDisplayCloud( int pnt_per_cell = 1000; boost::mt19937 rng; boost::normal_distribution<> nd(0.0, leaf_size_.head(3).norm()); - boost::variate_generator > var_nor(rng, nd); + boost::variate_generator> var_nor(rng, nd); Eigen::LLT llt_of_cov; Eigen::Matrix3d cholesky_decomp; diff --git a/package.xml b/package.xml index f336e52a..2087eca9 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ - ndt_omp + autoware_ndt_omp 0.0.0 OpenMP boosted NDT and GICP algorithms. With some modifications for Autoware diff --git a/src/estimate_covariance/estimate_covariance.cpp b/src/estimate_covariance/estimate_covariance.cpp index 120d66f5..0a5445f0 100644 --- a/src/estimate_covariance/estimate_covariance.cpp +++ b/src/estimate_covariance/estimate_covariance.cpp @@ -1,9 +1,9 @@ -#include "estimate_covariance/estimate_covariance.hpp" +#include "autoware/ndt_omp/estimate_covariance/estimate_covariance.hpp" #include #include -namespace pclomp +namespace autoware::ndt_omp::pclomp { Eigen::Matrix2d estimate_xy_covariance_by_laplace_approximation( @@ -16,8 +16,8 @@ Eigen::Matrix2d estimate_xy_covariance_by_laplace_approximation( ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt( const NdtResult & ndt_result, - const std::shared_ptr< - pclomp::MultiGridNormalDistributionsTransform> & ndt_ptr, + const std::shared_ptr> & ndt_ptr, const std::vector & poses_to_search) { // initialize by the main result @@ -52,8 +52,8 @@ ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt( ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt_score( const NdtResult & ndt_result, - const std::shared_ptr< - pclomp::MultiGridNormalDistributionsTransform> & ndt_ptr, + const std::shared_ptr> & ndt_ptr, const std::vector & poses_to_search, const double temperature) { // initialize by the main result @@ -186,4 +186,4 @@ Eigen::Matrix2d adjust_diagonal_covariance( return cov_map; } -} // namespace pclomp +} // namespace autoware::ndt_omp::pclomp diff --git a/src/multigrid_pclomp/multi_voxel_grid_covariance_omp.cpp b/src/multigrid_pclomp/multi_voxel_grid_covariance_omp.cpp index 64e5126b..2d449615 100644 --- a/src/multigrid_pclomp/multi_voxel_grid_covariance_omp.cpp +++ b/src/multigrid_pclomp/multi_voxel_grid_covariance_omp.cpp @@ -1,6 +1,6 @@ -#include +#include -#include +#include -template class pclomp::MultiVoxelGridCovariance; -template class pclomp::MultiVoxelGridCovariance; +template class autoware::ndt_omp::pclomp::MultiVoxelGridCovariance; +template class autoware::ndt_omp::pclomp::MultiVoxelGridCovariance; diff --git a/src/multigrid_pclomp/multigrid_ndt_omp.cpp b/src/multigrid_pclomp/multigrid_ndt_omp.cpp index 23da6f28..46630fdb 100644 --- a/src/multigrid_pclomp/multigrid_ndt_omp.cpp +++ b/src/multigrid_pclomp/multigrid_ndt_omp.cpp @@ -1,6 +1,8 @@ -#include +#include -#include +#include -template class pclomp::MultiGridNormalDistributionsTransform; -template class pclomp::MultiGridNormalDistributionsTransform; +template class autoware::ndt_omp::pclomp::MultiGridNormalDistributionsTransform< + pcl::PointXYZ, pcl::PointXYZ>; +template class autoware::ndt_omp::pclomp::MultiGridNormalDistributionsTransform< + pcl::PointXYZI, pcl::PointXYZI>; diff --git a/src/pclomp/gicp_omp.cpp b/src/pclomp/gicp_omp.cpp index f2caaf24..82c647bd 100644 --- a/src/pclomp/gicp_omp.cpp +++ b/src/pclomp/gicp_omp.cpp @@ -1,8 +1,10 @@ // this cannot be swapped // clang-format off -#include -#include +#include +#include // clang-format on -template class pclomp::GeneralizedIterativeClosestPoint; -template class pclomp::GeneralizedIterativeClosestPoint; +template class autoware::ndt_omp::pclomp::GeneralizedIterativeClosestPoint< + pcl::PointXYZ, pcl::PointXYZ>; +template class autoware::ndt_omp::pclomp::GeneralizedIterativeClosestPoint< + pcl::PointXYZI, pcl::PointXYZI>; diff --git a/src/pclomp/ndt_omp.cpp b/src/pclomp/ndt_omp.cpp index 12eeea53..e7531a32 100644 --- a/src/pclomp/ndt_omp.cpp +++ b/src/pclomp/ndt_omp.cpp @@ -1,6 +1,8 @@ -#include +#include -#include +#include -template class pclomp::NormalDistributionsTransform; -template class pclomp::NormalDistributionsTransform; +template class autoware::ndt_omp::pclomp::NormalDistributionsTransform< + pcl::PointXYZ, pcl::PointXYZ>; +template class autoware::ndt_omp::pclomp::NormalDistributionsTransform< + pcl::PointXYZI, pcl::PointXYZI>; diff --git a/src/pclomp/voxel_grid_covariance_omp.cpp b/src/pclomp/voxel_grid_covariance_omp.cpp index db430866..238b7c03 100644 --- a/src/pclomp/voxel_grid_covariance_omp.cpp +++ b/src/pclomp/voxel_grid_covariance_omp.cpp @@ -1,6 +1,6 @@ -#include +#include -#include +#include -template class pclomp::VoxelGridCovariance; -template class pclomp::VoxelGridCovariance; +template class autoware::ndt_omp::pclomp::VoxelGridCovariance; +template class autoware::ndt_omp::pclomp::VoxelGridCovariance;