From beb904edf5eaf81130fb119c3b881bf7fafc1f0c Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Thu, 27 Oct 2022 16:58:15 +0900 Subject: [PATCH] feat(ndt): remove ndt package (#2053) * first commit Signed-off-by: kminoda * CMakeLists.txt does not work........ Signed-off-by: kminoda * build works Signed-off-by: kminoda * debugged Signed-off-by: kminoda * remove unnecessary parameter Signed-off-by: kminoda * ci(pre-commit): autofix * removed 'omp'-related words completely Signed-off-by: kminoda * ci(pre-commit): autofix * fixed param description of converged_param Signed-off-by: kminoda * remove OMPParams Signed-off-by: kminoda * removed unnecessary includes Signed-off-by: kminoda * removed default parameter from search_method * small fix Signed-off-by: kminoda Signed-off-by: kminoda Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Signed-off-by: Kotaro Yoshimoto --- .../config/ndt_scan_matcher.param.yaml | 12 +- localization/ndt/CMakeLists.txt | 69 ---- localization/ndt/README.md | 12 - localization/ndt/include/ndt/base.hpp | 71 ---- localization/ndt/include/ndt/impl/base.hpp | 25 -- localization/ndt/include/ndt/impl/omp.hpp | 229 ----------- .../ndt/include/ndt/impl/pcl_generic.hpp | 190 --------- .../ndt/include/ndt/impl/pcl_modified.hpp | 190 --------- localization/ndt/include/ndt/omp.hpp | 83 ---- localization/ndt/include/ndt/pcl_generic.hpp | 76 ---- localization/ndt/include/ndt/pcl_modified.hpp | 77 ---- localization/ndt/package.xml | 25 -- localization/ndt/src/base.cpp | 20 - localization/ndt/src/omp.cpp | 20 - localization/ndt/src/pcl_generic.cpp | 20 - localization/ndt/src/pcl_modified.cpp | 20 - localization/ndt_pcl_modified/CMakeLists.txt | 60 --- localization/ndt_pcl_modified/README.md | 11 - .../include/ndt_pcl_modified/impl/ndt.hpp | 386 ------------------ .../include/ndt_pcl_modified/ndt.hpp | 124 ------ localization/ndt_pcl_modified/package.xml | 23 -- localization/ndt_pcl_modified/src/ndt.cpp | 50 --- localization/ndt_scan_matcher/CMakeLists.txt | 6 + localization/ndt_scan_matcher/README.md | 9 +- .../config/ndt_scan_matcher.param.yaml | 11 +- .../ndt_scan_matcher_core.hpp | 53 +-- localization/ndt_scan_matcher/package.xml | 3 +- .../src/ndt_scan_matcher_core.cpp | 110 ++--- 28 files changed, 65 insertions(+), 1920 deletions(-) delete mode 100644 localization/ndt/CMakeLists.txt delete mode 100644 localization/ndt/README.md delete mode 100644 localization/ndt/include/ndt/base.hpp delete mode 100644 localization/ndt/include/ndt/impl/base.hpp delete mode 100644 localization/ndt/include/ndt/impl/omp.hpp delete mode 100644 localization/ndt/include/ndt/impl/pcl_generic.hpp delete mode 100644 localization/ndt/include/ndt/impl/pcl_modified.hpp delete mode 100644 localization/ndt/include/ndt/omp.hpp delete mode 100644 localization/ndt/include/ndt/pcl_generic.hpp delete mode 100644 localization/ndt/include/ndt/pcl_modified.hpp delete mode 100644 localization/ndt/package.xml delete mode 100644 localization/ndt/src/base.cpp delete mode 100644 localization/ndt/src/omp.cpp delete mode 100644 localization/ndt/src/pcl_generic.cpp delete mode 100644 localization/ndt/src/pcl_modified.cpp delete mode 100644 localization/ndt_pcl_modified/CMakeLists.txt delete mode 100644 localization/ndt_pcl_modified/README.md delete mode 100644 localization/ndt_pcl_modified/include/ndt_pcl_modified/impl/ndt.hpp delete mode 100644 localization/ndt_pcl_modified/include/ndt_pcl_modified/ndt.hpp delete mode 100644 localization/ndt_pcl_modified/package.xml delete mode 100644 localization/ndt_pcl_modified/src/ndt.cpp diff --git a/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml b/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml index 05b6d4e666a06..3b7f113c43a94 100644 --- a/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml +++ b/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml @@ -1,15 +1,12 @@ /**: ros__parameters: + # Vehicle reference frame base_frame: "base_link" # Subscriber queue size input_sensor_points_queue_size: 1 - # NDT implementation type - # 0=PCL_GENERIC, 1=PCL_MODIFIED, 2=OMP - ndt_implement_type: 2 - # The maximum difference between two consecutive # transformations in order to consider convergence trans_epsilon: 0.01 @@ -25,7 +22,6 @@ # Converged param type # 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD - # NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD is only available when NDTImplementType::OMP is selected converged_param_type: 1 # If converged_param_type is 0 @@ -45,12 +41,12 @@ # Tolerance of distance difference between two initial poses used for linear interpolation. [m] initial_pose_distance_tolerance_m: 10.0 - # neighborhood search method in OMP + # neighborhood search method # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1 - omp_neighborhood_search_method: 0 + neighborhood_search_method: 0 # Number of threads used for parallel computing - omp_num_threads: 4 + num_threads: 4 # The covariance of output pose # Do note that this covariance matrix is empirically derived diff --git a/localization/ndt/CMakeLists.txt b/localization/ndt/CMakeLists.txt deleted file mode 100644 index 72e0346da770b..0000000000000 --- a/localization/ndt/CMakeLists.txt +++ /dev/null @@ -1,69 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(ndt) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -# Compile flags for SIMD instructions -# Be careful to change these options, especially when `ndt_omp` implementation is used. -# All packages linked to `ndt_omp` should use the same SIMD instruction set. -# In case mismatched instruction set are used, program causes a crash at its initialization -# because of a misaligned access to the `Eigen` libraries' data structure. -if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "x86_64") - # For x86_64 architecture, SIMD instruction set is fixed below versions, - # because the `ndt_omp` is optimized to these versions. - add_compile_options(-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2) -else() - # For other architecture, like arm64, compile flags are generally prepared by compiler - # march=native is disabled as default for specific depending pcl libraries - # or pre-building packages for other computers. - if(BUILD_WITH_MARCH_NATIVE) - add_compile_options(-march=native) - endif() -endif() - -find_package(PCL REQUIRED COMPONENTS common io registration) - -add_library(ndt - src/base.cpp - src/pcl_generic.cpp - src/pcl_modified.cpp - src/omp.cpp -) - -target_include_directories(ndt - PUBLIC - $ - $ -) - -target_include_directories(ndt - SYSTEM PUBLIC - ${PCL_INCLUDE_DIRS} -) - -ament_target_dependencies(ndt PUBLIC ndt_omp ndt_pcl_modified) -# Can't use ament_target_dependencies() here because it doesn't link PCL -# properly, see ndt_omp for more information. -target_compile_definitions(ndt PUBLIC ${PCL_DEFINITIONS}) -target_include_directories(ndt PUBLIC ${PCL_INCLUDE_DIRS}) -target_link_libraries(ndt PUBLIC ${PCL_LIBRARIES}) -target_link_directories(ndt PUBLIC ${PCL_LIBRARY_DIRS}) - -ament_export_targets(export_ndt HAS_LIBRARY_TARGET) -ament_export_dependencies(ndt_omp ndt_pcl_modified PCL) - -install( - DIRECTORY include/ - DESTINATION include -) - -install( - TARGETS ndt - EXPORT export_ndt - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin -) - -ament_package() diff --git a/localization/ndt/README.md b/localization/ndt/README.md deleted file mode 100644 index a0f1b358185f6..0000000000000 --- a/localization/ndt/README.md +++ /dev/null @@ -1,12 +0,0 @@ -# ndt - -This package aims to absorb the differences of several NDT implementations and provide a common interface. - -```plantuml -@startuml -abstract class NormalDistributionsTransformBase -NormalDistributionsTransformBase <|-- NormalDistributionsTransformOMP -NormalDistributionsTransformBase <|-- NormalDistributionsTransformPCLGeneric -NormalDistributionsTransformBase <|-- NormalDistributionsTransformPCLModified -@enduml -``` diff --git a/localization/ndt/include/ndt/base.hpp b/localization/ndt/include/ndt/base.hpp deleted file mode 100644 index 6de011e354b1e..0000000000000 --- a/localization/ndt/include/ndt/base.hpp +++ /dev/null @@ -1,71 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NDT__BASE_HPP_ -#define NDT__BASE_HPP_ - -#include -#include -#include -#include - -#include - -template -class NormalDistributionsTransformBase -{ -public: - NormalDistributionsTransformBase(); - virtual ~NormalDistributionsTransformBase() = default; - - virtual void align(pcl::PointCloud & output, const Eigen::Matrix4f & guess) = 0; - virtual void setInputTarget(const pcl::shared_ptr> & map_ptr) = 0; - virtual void setInputSource(const pcl::shared_ptr> & scan_ptr) = 0; - - virtual void setMaximumIterations(int max_iter) = 0; - virtual void setResolution(float res) = 0; - virtual void setStepSize(double step_size) = 0; - virtual void setTransformationEpsilon(double trans_eps) = 0; - - virtual int getMaximumIterations() = 0; - virtual int getFinalNumIteration() const = 0; - virtual float getResolution() const = 0; - virtual double getStepSize() const = 0; - virtual double getTransformationEpsilon() = 0; - virtual double getTransformationProbability() const = 0; - virtual double getNearestVoxelTransformationLikelihood() const = 0; - virtual double getFitnessScore() = 0; - virtual pcl::shared_ptr> getInputTarget() const = 0; - virtual pcl::shared_ptr> getInputSource() const = 0; - virtual Eigen::Matrix4f getFinalTransformation() const = 0; - virtual std::vector> - getFinalTransformationArray() const = 0; - - virtual void setRegularizationPose(const Eigen::Matrix4f &) = 0; - virtual void unsetRegularizationPose() = 0; - virtual void setRegularizationScaleFactor(const float) = 0; - - virtual Eigen::Matrix getHessian() const = 0; - - virtual pcl::shared_ptr> getSearchMethodTarget() const = 0; - - virtual double calculateTransformationProbability( - const pcl::PointCloud & trans_cloud) const = 0; - virtual double calculateNearestVoxelTransformationLikelihood( - const pcl::PointCloud & trans_cloud) const = 0; -}; - -#include "ndt/impl/base.hpp" - -#endif // NDT__BASE_HPP_ diff --git a/localization/ndt/include/ndt/impl/base.hpp b/localization/ndt/include/ndt/impl/base.hpp deleted file mode 100644 index 1c2381d8842a6..0000000000000 --- a/localization/ndt/include/ndt/impl/base.hpp +++ /dev/null @@ -1,25 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NDT__IMPL__BASE_HPP_ -#define NDT__IMPL__BASE_HPP_ - -#include "ndt/base.hpp" - -template -NormalDistributionsTransformBase::NormalDistributionsTransformBase() -{ -} - -#endif // NDT__IMPL__BASE_HPP_ diff --git a/localization/ndt/include/ndt/impl/omp.hpp b/localization/ndt/include/ndt/impl/omp.hpp deleted file mode 100644 index ffa4a864eb351..0000000000000 --- a/localization/ndt/include/ndt/impl/omp.hpp +++ /dev/null @@ -1,229 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NDT__IMPL__OMP_HPP_ -#define NDT__IMPL__OMP_HPP_ - -#include "ndt/omp.hpp" - -#include - -template -NormalDistributionsTransformOMP::NormalDistributionsTransformOMP() -: ndt_ptr_(new pclomp::NormalDistributionsTransform) -{ -} - -template -void NormalDistributionsTransformOMP::align( - pcl::PointCloud & output, const Eigen::Matrix4f & guess) -{ - ndt_ptr_->align(output, guess); -} - -template -void NormalDistributionsTransformOMP::setInputTarget( - const pcl::shared_ptr> & map_ptr) -{ - ndt_ptr_->setInputTarget(map_ptr); -} - -template -void NormalDistributionsTransformOMP::setInputSource( - const pcl::shared_ptr> & scan_ptr) -{ - ndt_ptr_->setInputSource(scan_ptr); -} - -template -void NormalDistributionsTransformOMP::setMaximumIterations(int max_iter) -{ - ndt_ptr_->setMaximumIterations(max_iter); -} - -template -void NormalDistributionsTransformOMP::setResolution(float res) -{ - ndt_ptr_->setResolution(res); -} - -template -void NormalDistributionsTransformOMP::setStepSize(double step_size) -{ - ndt_ptr_->setStepSize(step_size); -} - -template -void NormalDistributionsTransformOMP::setTransformationEpsilon( - double trans_eps) -{ - ndt_ptr_->setTransformationEpsilon(trans_eps); -} - -template -int NormalDistributionsTransformOMP::getMaximumIterations() -{ - return ndt_ptr_->getMaximumIterations(); -} - -template -int NormalDistributionsTransformOMP::getFinalNumIteration() const -{ - return ndt_ptr_->getFinalNumIteration(); -} - -template -float NormalDistributionsTransformOMP::getResolution() const -{ - return ndt_ptr_->getResolution(); -} - -template -double NormalDistributionsTransformOMP::getStepSize() const -{ - return ndt_ptr_->getStepSize(); -} - -template -double NormalDistributionsTransformOMP::getTransformationEpsilon() -{ - return ndt_ptr_->getTransformationEpsilon(); -} - -template -double NormalDistributionsTransformOMP::getTransformationProbability() - const -{ - return ndt_ptr_->getTransformationProbability(); -} - -template -double NormalDistributionsTransformOMP< - PointSource, PointTarget>::getNearestVoxelTransformationLikelihood() const -{ - return ndt_ptr_->getNearestVoxelTransformationLikelihood(); -} - -template -double NormalDistributionsTransformOMP::getFitnessScore() -{ - return ndt_ptr_->getFitnessScore(); -} - -template -pcl::shared_ptr> -NormalDistributionsTransformOMP::getInputTarget() const -{ - return ndt_ptr_->getInputTarget(); -} - -template -pcl::shared_ptr> -NormalDistributionsTransformOMP::getInputSource() const -{ - return ndt_ptr_->getInputSource(); -} - -template -Eigen::Matrix4f NormalDistributionsTransformOMP::getFinalTransformation() - const -{ - return ndt_ptr_->getFinalTransformation(); -} - -template -std::vector> -NormalDistributionsTransformOMP::getFinalTransformationArray() const -{ - return ndt_ptr_->getFinalTransformationArray(); -} - -template -Eigen::Matrix NormalDistributionsTransformOMP::getHessian() - const -{ - // return ndt_ptr_->getHessian(); - return Eigen::Matrix(); -} - -template -void NormalDistributionsTransformOMP::setRegularizationScaleFactor( - const float regularization_scale_factor) -{ - ndt_ptr_->setRegularizationScaleFactor(regularization_scale_factor); -} - -template -void NormalDistributionsTransformOMP::setRegularizationPose( - const Eigen::Matrix4f & regularization_pose) -{ - ndt_ptr_->setRegularizationPose(regularization_pose); -} - -template -void NormalDistributionsTransformOMP::unsetRegularizationPose() -{ - ndt_ptr_->unsetRegularizationPose(); -} - -template -pcl::shared_ptr> -NormalDistributionsTransformOMP::getSearchMethodTarget() const -{ - return ndt_ptr_->getSearchMethodTarget(); -} - -template -double -NormalDistributionsTransformOMP::calculateTransformationProbability( - const pcl::PointCloud & trans_cloud) const -{ - return ndt_ptr_->calculateTransformationProbability(trans_cloud); -} - -template -double NormalDistributionsTransformOMP:: - calculateNearestVoxelTransformationLikelihood( - const pcl::PointCloud & trans_cloud) const -{ - return ndt_ptr_->calculateNearestVoxelTransformationLikelihood(trans_cloud); -} - -template -void NormalDistributionsTransformOMP::setNumThreads(int n) -{ - ndt_ptr_->setNumThreads(n); -} - -template -void NormalDistributionsTransformOMP::setNeighborhoodSearchMethod( - pclomp::NeighborSearchMethod method) -{ - ndt_ptr_->setNeighborhoodSearchMethod(method); -} - -template -int NormalDistributionsTransformOMP::getNumThreads() const -{ - return ndt_ptr_->getNumThreads(); -} - -template -pclomp::NeighborSearchMethod -NormalDistributionsTransformOMP::getNeighborhoodSearchMethod() const -{ - return ndt_ptr_->getNeighborhoodSearchMethod(); -} - -#endif // NDT__IMPL__OMP_HPP_ diff --git a/localization/ndt/include/ndt/impl/pcl_generic.hpp b/localization/ndt/include/ndt/impl/pcl_generic.hpp deleted file mode 100644 index 329093cd28589..0000000000000 --- a/localization/ndt/include/ndt/impl/pcl_generic.hpp +++ /dev/null @@ -1,190 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NDT__IMPL__PCL_GENERIC_HPP_ -#define NDT__IMPL__PCL_GENERIC_HPP_ - -#include "ndt/pcl_generic.hpp" - -#include - -template -NormalDistributionsTransformPCLGeneric< - PointSource, PointTarget>::NormalDistributionsTransformPCLGeneric() -: ndt_ptr_(new pcl::NormalDistributionsTransform) -{ -} - -template -void NormalDistributionsTransformPCLGeneric::align( - pcl::PointCloud & output, const Eigen::Matrix4f & guess) -{ - ndt_ptr_->align(output, guess); -} - -template -void NormalDistributionsTransformPCLGeneric::setInputTarget( - const pcl::shared_ptr> & map_ptr) -{ - ndt_ptr_->setInputTarget(map_ptr); -} - -template -void NormalDistributionsTransformPCLGeneric::setInputSource( - const pcl::shared_ptr> & scan_ptr) -{ - ndt_ptr_->setInputSource(scan_ptr); -} - -template -void NormalDistributionsTransformPCLGeneric::setMaximumIterations( - int max_iter) -{ - ndt_ptr_->setMaximumIterations(max_iter); -} - -template -void NormalDistributionsTransformPCLGeneric::setResolution(float res) -{ - ndt_ptr_->setResolution(res); -} - -template -void NormalDistributionsTransformPCLGeneric::setStepSize(double step_size) -{ - ndt_ptr_->setStepSize(step_size); -} - -template -void NormalDistributionsTransformPCLGeneric::setTransformationEpsilon( - double trans_eps) -{ - ndt_ptr_->setTransformationEpsilon(trans_eps); -} - -template -int NormalDistributionsTransformPCLGeneric::getMaximumIterations() -{ - return ndt_ptr_->getMaximumIterations(); -} - -template -int NormalDistributionsTransformPCLGeneric::getFinalNumIteration() const -{ - return ndt_ptr_->getFinalNumIteration(); -} - -template -float NormalDistributionsTransformPCLGeneric::getResolution() const -{ - return ndt_ptr_->getResolution(); -} - -template -double NormalDistributionsTransformPCLGeneric::getStepSize() const -{ - return ndt_ptr_->getStepSize(); -} - -template -double NormalDistributionsTransformPCLGeneric::getTransformationEpsilon() -{ - return ndt_ptr_->getTransformationEpsilon(); -} - -template -double NormalDistributionsTransformPCLGeneric< - PointSource, PointTarget>::getTransformationProbability() const -{ - return ndt_ptr_->getTransformationProbability(); -} - -template -double NormalDistributionsTransformPCLGeneric< - PointSource, PointTarget>::getNearestVoxelTransformationLikelihood() const -{ - // return ndt_ptr_->getNearestVoxelTransformationLikelihood(); - return 0.0; -} - -template -double NormalDistributionsTransformPCLGeneric::getFitnessScore() -{ - return ndt_ptr_->getFitnessScore(); -} - -template -pcl::shared_ptr> -NormalDistributionsTransformPCLGeneric::getInputTarget() const -{ - return ndt_ptr_->getInputTarget(); -} - -template -pcl::shared_ptr> -NormalDistributionsTransformPCLGeneric::getInputSource() const -{ - return ndt_ptr_->getInputSource(); -} - -template -Eigen::Matrix4f -NormalDistributionsTransformPCLGeneric::getFinalTransformation() const -{ - return ndt_ptr_->getFinalTransformation(); -} - -template -std::vector> -NormalDistributionsTransformPCLGeneric::getFinalTransformationArray() - const -{ - return std::vector>(); -} - -template -Eigen::Matrix -NormalDistributionsTransformPCLGeneric::getHessian() const -{ - // return ndt_ptr_->getHessian(); - return Eigen::Matrix(); -} - -template -pcl::shared_ptr> -NormalDistributionsTransformPCLGeneric::getSearchMethodTarget() const -{ - return ndt_ptr_->getSearchMethodTarget(); -} - -template -double NormalDistributionsTransformPCLGeneric:: - calculateTransformationProbability(const pcl::PointCloud & trans_cloud) const -{ - (void)trans_cloud; - // return ndt_ptr_->calculateTransformationProbability(trans_cloud); - return 0.0; -} - -template -double NormalDistributionsTransformPCLGeneric:: - calculateNearestVoxelTransformationLikelihood( - const pcl::PointCloud & trans_cloud) const -{ - (void)trans_cloud; - // return ndt_ptr_->calculateNearestVoxelTransformationLikelihood(trans_cloud); - return 0.0; -} - -#endif // NDT__IMPL__PCL_GENERIC_HPP_ diff --git a/localization/ndt/include/ndt/impl/pcl_modified.hpp b/localization/ndt/include/ndt/impl/pcl_modified.hpp deleted file mode 100644 index f6c359fcb7158..0000000000000 --- a/localization/ndt/include/ndt/impl/pcl_modified.hpp +++ /dev/null @@ -1,190 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NDT__IMPL__PCL_MODIFIED_HPP_ -#define NDT__IMPL__PCL_MODIFIED_HPP_ - -#include "ndt/pcl_modified.hpp" - -#include - -template -NormalDistributionsTransformPCLModified< - PointSource, PointTarget>::NormalDistributionsTransformPCLModified() -: ndt_ptr_(new pcl::NormalDistributionsTransformModified) -{ -} - -template -void NormalDistributionsTransformPCLModified::align( - pcl::PointCloud & output, const Eigen::Matrix4f & guess) -{ - ndt_ptr_->align(output, guess); -} - -template -void NormalDistributionsTransformPCLModified::setInputTarget( - const pcl::shared_ptr> & map_ptr) -{ - ndt_ptr_->setInputTarget(map_ptr); -} - -template -void NormalDistributionsTransformPCLModified::setInputSource( - const pcl::shared_ptr> & scan_ptr) -{ - ndt_ptr_->setInputSource(scan_ptr); -} - -template -void NormalDistributionsTransformPCLModified::setMaximumIterations( - int max_iter) -{ - ndt_ptr_->setMaximumIterations(max_iter); -} - -template -void NormalDistributionsTransformPCLModified::setResolution(float res) -{ - ndt_ptr_->setResolution(res); -} - -template -void NormalDistributionsTransformPCLModified::setStepSize( - double step_size) -{ - ndt_ptr_->setStepSize(step_size); -} - -template -void NormalDistributionsTransformPCLModified::setTransformationEpsilon( - double trans_eps) -{ - ndt_ptr_->setTransformationEpsilon(trans_eps); -} - -template -int NormalDistributionsTransformPCLModified::getMaximumIterations() -{ - return ndt_ptr_->getMaximumIterations(); -} - -template -int NormalDistributionsTransformPCLModified::getFinalNumIteration() const -{ - return ndt_ptr_->getFinalNumIteration(); -} - -template -float NormalDistributionsTransformPCLModified::getResolution() const -{ - return ndt_ptr_->getResolution(); -} - -template -double NormalDistributionsTransformPCLModified::getStepSize() const -{ - return ndt_ptr_->getStepSize(); -} - -template -double NormalDistributionsTransformPCLModified::getTransformationEpsilon() -{ - return ndt_ptr_->getTransformationEpsilon(); -} - -template -double NormalDistributionsTransformPCLModified< - PointSource, PointTarget>::getTransformationProbability() const -{ - return ndt_ptr_->getTransformationProbability(); -} - -template -double NormalDistributionsTransformPCLModified< - PointSource, PointTarget>::getNearestVoxelTransformationLikelihood() const -{ - // return ndt_ptr_->getTransformationLikelihood(); - return 0.0; -} - -template -double NormalDistributionsTransformPCLModified::getFitnessScore() -{ - return ndt_ptr_->getFitnessScore(); -} - -template -pcl::shared_ptr> -NormalDistributionsTransformPCLModified::getInputTarget() const -{ - return ndt_ptr_->getInputTarget(); -} - -template -pcl::shared_ptr> -NormalDistributionsTransformPCLModified::getInputSource() const -{ - return ndt_ptr_->getInputSource(); -} - -template -Eigen::Matrix4f -NormalDistributionsTransformPCLModified::getFinalTransformation() const -{ - return ndt_ptr_->getFinalTransformation(); -} - -template -std::vector> -NormalDistributionsTransformPCLModified::getFinalTransformationArray() - const -{ - return ndt_ptr_->getFinalTransformationArray(); -} - -template -Eigen::Matrix -NormalDistributionsTransformPCLModified::getHessian() const -{ - return ndt_ptr_->getHessian(); -} - -template -pcl::shared_ptr> -NormalDistributionsTransformPCLModified::getSearchMethodTarget() const -{ - return ndt_ptr_->getSearchMethodTarget(); -} - -template -double NormalDistributionsTransformPCLModified:: - calculateTransformationProbability(const pcl::PointCloud & trans_cloud) const -{ - (void)trans_cloud; - // return ndt_ptr_->calculateTransformationProbability(trans_cloud); - return 0.0; -} - -template -double NormalDistributionsTransformPCLModified:: - calculateNearestVoxelTransformationLikelihood( - const pcl::PointCloud & trans_cloud) const -{ - (void)trans_cloud; - // return ndt_ptr_->calculateNearestVoxelTransformationLikelihood(trans_cloud); - return 0.0; -} - -#endif // NDT__IMPL__PCL_MODIFIED_HPP_ diff --git a/localization/ndt/include/ndt/omp.hpp b/localization/ndt/include/ndt/omp.hpp deleted file mode 100644 index 4488fabbfb7dd..0000000000000 --- a/localization/ndt/include/ndt/omp.hpp +++ /dev/null @@ -1,83 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NDT__OMP_HPP_ -#define NDT__OMP_HPP_ - -#include "ndt/base.hpp" - -#include -#include -#include -#include - -#include - -template -class NormalDistributionsTransformOMP -: public NormalDistributionsTransformBase -{ -public: - NormalDistributionsTransformOMP(); - ~NormalDistributionsTransformOMP() = default; - - void align(pcl::PointCloud & output, const Eigen::Matrix4f & guess) override; - void setInputTarget(const pcl::shared_ptr> & map_ptr) override; - void setInputSource(const pcl::shared_ptr> & scan_ptr) override; - - void setMaximumIterations(int max_iter) override; - void setResolution(float res) override; - void setStepSize(double step_size) override; - void setTransformationEpsilon(double trans_eps) override; - - int getMaximumIterations() override; - int getFinalNumIteration() const override; - float getResolution() const override; - double getStepSize() const override; - double getTransformationEpsilon() override; - double getTransformationProbability() const override; - double getNearestVoxelTransformationLikelihood() const override; - double getFitnessScore() override; - pcl::shared_ptr> getInputTarget() const override; - pcl::shared_ptr> getInputSource() const override; - Eigen::Matrix4f getFinalTransformation() const override; - std::vector> - getFinalTransformationArray() const override; - - Eigen::Matrix getHessian() const override; - void setRegularizationScaleFactor(const float regularization_scale_factor) override; - void setRegularizationPose(const Eigen::Matrix4f & regularization_pose) override; - void unsetRegularizationPose() override; - - pcl::shared_ptr> getSearchMethodTarget() const override; - - double calculateTransformationProbability( - const pcl::PointCloud & trans_cloud) const override; - double calculateNearestVoxelTransformationLikelihood( - const pcl::PointCloud & trans_cloud) const override; - - // only OMP Impl - void setNumThreads(int n); - void setNeighborhoodSearchMethod(pclomp::NeighborSearchMethod method); - - int getNumThreads() const; - pclomp::NeighborSearchMethod getNeighborhoodSearchMethod() const; - -private: - pcl::shared_ptr> ndt_ptr_; -}; - -#include "ndt/impl/omp.hpp" - -#endif // NDT__OMP_HPP_ diff --git a/localization/ndt/include/ndt/pcl_generic.hpp b/localization/ndt/include/ndt/pcl_generic.hpp deleted file mode 100644 index 0af5fa1005da4..0000000000000 --- a/localization/ndt/include/ndt/pcl_generic.hpp +++ /dev/null @@ -1,76 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NDT__PCL_GENERIC_HPP_ -#define NDT__PCL_GENERIC_HPP_ - -#include "ndt/base.hpp" - -#include -#include -#include -#include - -#include - -template -class NormalDistributionsTransformPCLGeneric -: public NormalDistributionsTransformBase -{ -public: - NormalDistributionsTransformPCLGeneric(); - ~NormalDistributionsTransformPCLGeneric() = default; - - void align(pcl::PointCloud & output, const Eigen::Matrix4f & guess) override; - void setInputTarget(const pcl::shared_ptr> & map_ptr) override; - void setInputSource(const pcl::shared_ptr> & scan_ptr) override; - - void setMaximumIterations(int max_iter) override; - void setResolution(float res) override; - void setStepSize(double step_size) override; - void setTransformationEpsilon(double trans_eps) override; - - int getMaximumIterations() override; - int getFinalNumIteration() const override; - float getResolution() const override; - double getStepSize() const override; - double getTransformationEpsilon() override; - double getTransformationProbability() const override; - double getNearestVoxelTransformationLikelihood() const override; - double getFitnessScore() override; - pcl::shared_ptr> getInputTarget() const override; - pcl::shared_ptr> getInputSource() const override; - Eigen::Matrix4f getFinalTransformation() const override; - std::vector> - getFinalTransformationArray() const override; - - Eigen::Matrix getHessian() const override; - void setRegularizationScaleFactor(const float) override{}; - void setRegularizationPose(const Eigen::Matrix4f &) override {} - void unsetRegularizationPose() override {} - - pcl::shared_ptr> getSearchMethodTarget() const override; - - double calculateTransformationProbability( - const pcl::PointCloud & trans_cloud) const override; - double calculateNearestVoxelTransformationLikelihood( - const pcl::PointCloud & trans_cloud) const override; - -private: - pcl::shared_ptr> ndt_ptr_; -}; - -#include "ndt/impl/pcl_generic.hpp" - -#endif // NDT__PCL_GENERIC_HPP_ diff --git a/localization/ndt/include/ndt/pcl_modified.hpp b/localization/ndt/include/ndt/pcl_modified.hpp deleted file mode 100644 index 3508a98d6b3c4..0000000000000 --- a/localization/ndt/include/ndt/pcl_modified.hpp +++ /dev/null @@ -1,77 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NDT__PCL_MODIFIED_HPP_ -#define NDT__PCL_MODIFIED_HPP_ - -#include "ndt/base.hpp" - -#include - -#include -#include -#include - -#include - -template -class NormalDistributionsTransformPCLModified -: public NormalDistributionsTransformBase -{ -public: - NormalDistributionsTransformPCLModified(); - ~NormalDistributionsTransformPCLModified() = default; - - void align(pcl::PointCloud & output, const Eigen::Matrix4f & guess) override; - void setInputTarget(const pcl::shared_ptr> & map_ptr) override; - void setInputSource(const pcl::shared_ptr> & scan_ptr) override; - - void setMaximumIterations(int max_iter) override; - void setResolution(float res) override; - void setStepSize(double step_size) override; - void setTransformationEpsilon(double trans_eps) override; - - int getMaximumIterations() override; - int getFinalNumIteration() const override; - float getResolution() const override; - double getStepSize() const override; - double getTransformationEpsilon() override; - double getTransformationProbability() const override; - double getNearestVoxelTransformationLikelihood() const override; - double getFitnessScore() override; - pcl::shared_ptr> getInputTarget() const override; - pcl::shared_ptr> getInputSource() const override; - Eigen::Matrix4f getFinalTransformation() const override; - std::vector> - getFinalTransformationArray() const override; - - Eigen::Matrix getHessian() const override; - void setRegularizationScaleFactor(const float) override{}; - void setRegularizationPose(const Eigen::Matrix4f &) override {} - void unsetRegularizationPose() override {} - - pcl::shared_ptr> getSearchMethodTarget() const override; - - double calculateTransformationProbability( - const pcl::PointCloud & trans_cloud) const override; - double calculateNearestVoxelTransformationLikelihood( - const pcl::PointCloud & trans_cloud) const override; - -private: - pcl::shared_ptr> ndt_ptr_; -}; - -#include "ndt/impl/pcl_modified.hpp" - -#endif // NDT__PCL_MODIFIED_HPP_ diff --git a/localization/ndt/package.xml b/localization/ndt/package.xml deleted file mode 100644 index a573b06dd8e29..0000000000000 --- a/localization/ndt/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - ndt - 0.1.0 - The ndt package - Yamato Ando - Apache License 2.0 - Yamato Ando - - ament_cmake - - autoware_cmake - - libpcl-all-dev - ndt_omp - ndt_pcl_modified - - ament_cmake_cppcheck - ament_lint_auto - - - ament_cmake - - diff --git a/localization/ndt/src/base.cpp b/localization/ndt/src/base.cpp deleted file mode 100644 index 3d869d12398a3..0000000000000 --- a/localization/ndt/src/base.cpp +++ /dev/null @@ -1,20 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ndt/base.hpp" - -#include "ndt/impl/base.hpp" - -template class NormalDistributionsTransformBase; -template class NormalDistributionsTransformBase; diff --git a/localization/ndt/src/omp.cpp b/localization/ndt/src/omp.cpp deleted file mode 100644 index 075b8e9a6c451..0000000000000 --- a/localization/ndt/src/omp.cpp +++ /dev/null @@ -1,20 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ndt/omp.hpp" - -#include "ndt/impl/omp.hpp" - -template class NormalDistributionsTransformOMP; -template class NormalDistributionsTransformOMP; diff --git a/localization/ndt/src/pcl_generic.cpp b/localization/ndt/src/pcl_generic.cpp deleted file mode 100644 index a6c8297e33456..0000000000000 --- a/localization/ndt/src/pcl_generic.cpp +++ /dev/null @@ -1,20 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ndt/pcl_generic.hpp" - -#include "ndt/impl/pcl_generic.hpp" - -template class NormalDistributionsTransformPCLGeneric; -template class NormalDistributionsTransformPCLGeneric; diff --git a/localization/ndt/src/pcl_modified.cpp b/localization/ndt/src/pcl_modified.cpp deleted file mode 100644 index 36ec65ba84acb..0000000000000 --- a/localization/ndt/src/pcl_modified.cpp +++ /dev/null @@ -1,20 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ndt/pcl_modified.hpp" - -#include "ndt/impl/pcl_modified.hpp" - -template class NormalDistributionsTransformPCLModified; -template class NormalDistributionsTransformPCLModified; diff --git a/localization/ndt_pcl_modified/CMakeLists.txt b/localization/ndt_pcl_modified/CMakeLists.txt deleted file mode 100644 index 6842da4696053..0000000000000 --- a/localization/ndt_pcl_modified/CMakeLists.txt +++ /dev/null @@ -1,60 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(ndt_pcl_modified) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -# Compile flags for SIMD instructions -# Be careful to change these options, especially when `ndt_omp` implementation is used. -# All packages linked to `ndt_omp` should use the same SIMD instruction set. -# In case mismatched instruction set are used, program causes a crash at its initialization -# because of a misaligned access to the `Eigen` libraries' data structure. -if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "x86_64") - # For x86_64 architecture, SIMD instruction set is fixed below versions, - # because the `ndt_omp` is optimized to these versions. - add_compile_options(-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2) -else() - # For other architecture, like arm64, compile flags are generally prepared by compiler - # march=native is disabled as default for specific depending pcl libraries - # or pre-building packages for other computers. - if(BUILD_WITH_MARCH_NATIVE) - add_compile_options(-march=native) - endif() -endif() - -find_package(PCL REQUIRED COMPONENTS common) - -add_library(ndt_pcl_modified - src/ndt.cpp -) - -target_include_directories(ndt_pcl_modified - PUBLIC - $ - $ -) - -target_include_directories(ndt_pcl_modified - SYSTEM PUBLIC - ${PCL_INCLUDE_DIRS} -) - -ament_target_dependencies(ndt_pcl_modified PCL) -ament_export_targets(export_ndt_pcl_modified HAS_LIBRARY_TARGET) -ament_export_dependencies(PCL) - -install( - DIRECTORY include/ - DESTINATION include -) - -install( - TARGETS ndt_pcl_modified - EXPORT export_ndt_pcl_modified - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include -) - -ament_package() diff --git a/localization/ndt_pcl_modified/README.md b/localization/ndt_pcl_modified/README.md deleted file mode 100644 index 26950c20128c3..0000000000000 --- a/localization/ndt_pcl_modified/README.md +++ /dev/null @@ -1,11 +0,0 @@ -# ndt_pcl_modified - -## Purpose - -This is a modification of [PCL](https://github.com/PointCloudLibrary/pcl)'s NDT. - -## Modifications - -- You can get the Hessian matrix by getHessian(). -- You can get the estimated position for each iteration by getFinalTransformationArray(). -- It optimizes rotational axes first, then jointly optimizes rotational and translational axes. [experimental feature] diff --git a/localization/ndt_pcl_modified/include/ndt_pcl_modified/impl/ndt.hpp b/localization/ndt_pcl_modified/include/ndt_pcl_modified/impl/ndt.hpp deleted file mode 100644 index dc4da99e6222b..0000000000000 --- a/localization/ndt_pcl_modified/include/ndt_pcl_modified/impl/ndt.hpp +++ /dev/null @@ -1,386 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -/* - * Software License Agreement (BSD License) - * - * Point Cloud Library (PCL) - www.pointclouds.org - * Copyright (c) 2010-2011, Willow Garage, Inc. - * Copyright (c) 2012-, Open Perception, Inc. - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder(s) nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * $Id$ - * - */ - -#ifndef NDT_PCL_MODIFIED__IMPL__NDT_HPP_ -#define NDT_PCL_MODIFIED__IMPL__NDT_HPP_ - -#include -#include - -template -void pcl::NormalDistributionsTransformModified::computeTransformation( - PointCloudSource & output, const Eigen::Matrix4f & guess) -{ - nr_iterations_ = 0; - converged_ = false; - - double gauss_c1, gauss_c2, gauss_d3; - - // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009] - gauss_c1 = 10 * (1 - outlier_ratio_); - gauss_c2 = outlier_ratio_ / pow(resolution_, 3); - gauss_d3 = -log(gauss_c2); - gauss_d1_ = -log(gauss_c1 + gauss_c2) - gauss_d3; - gauss_d2_ = -2 * log((-log(gauss_c1 * exp(-0.5) + gauss_c2) - gauss_d3) / gauss_d1_); - - if (guess != Eigen::Matrix4f::Identity()) { - // Initialise final transformation to the guessed one - final_transformation_ = guess; - // Apply guessed transformation prior to search for neighbours - transformPointCloud(output, output, guess); - } - - // Initialize Point Gradient and Hessian -#if PCL_VERSION >= PCL_VERSION_CALC(1, 12, 1) - point_jacobian_.setZero(); - point_jacobian_.block(0, 0, 3, 3).setIdentity(); -#else - point_gradient_.setZero(); - point_gradient_.block(0, 0, 3, 3).setIdentity(); -#endif - point_hessian_.setZero(); - - Eigen::Transform eig_transformation; - eig_transformation.matrix() = final_transformation_; - - // Convert initial guess matrix to 6 element transformation vector - Eigen::Matrix p, delta_p, score_gradient; - Eigen::Vector3f init_translation = eig_transformation.translation(); - Eigen::Vector3f init_rotation = eig_transformation.rotation().eulerAngles(0, 1, 2); - p << init_translation(0), init_translation(1), init_translation(2), init_rotation(0), - init_rotation(1), init_rotation(2); - - Eigen::Matrix hessian; - - double score = 0; - double delta_p_norm; - // Calculate derivatives of initial transform vector, subsequent derivative calculations are done - // in the step length determination. - score = NormalDistributionsTransformModified::computeDerivatives( - score_gradient, hessian, output, p); - - transformation_array_.clear(); - transformation_array_.push_back(final_transformation_); - bool converged_rotation = false; - while (!converged_) { - // Store previous transformation - previous_transformation_ = transformation_; - - // Solve for decent direction using newton method, line 23 in Algorithm 2 [Magnusson 2009] - Eigen::JacobiSVD> sv( - hessian, Eigen::ComputeFullU | Eigen::ComputeFullV); - // Negative for maximization as opposed to minimization - delta_p = sv.solve(-score_gradient); - - // Calculate step length with guaranteed sufficient decrease [More, Thuente 1994] - delta_p_norm = delta_p.norm(); - - if (delta_p_norm == 0 || delta_p_norm != delta_p_norm) { - trans_probability_ = score / static_cast(input_->points.size()); - converged_ = delta_p_norm == delta_p_norm; - return; - } - - Eigen::Matrix delta_p_rotation = delta_p; - delta_p_rotation(0) = delta_p_rotation(1) = delta_p_rotation(2) = 0; - double delta_p_rotation_norm = delta_p_rotation.norm(); - - Eigen::Matrix score_gradient_rotation = score_gradient; - score_gradient_rotation(0) = score_gradient_rotation(1) = score_gradient_rotation(2) = 0; - - delta_p.normalize(); - delta_p_rotation.normalize(); - - if (!converged_rotation && delta_p_rotation_norm > 0.001 && nr_iterations_ < 10) { - delta_p = delta_p_rotation; - delta_p_norm = delta_p_rotation_norm; - score_gradient = score_gradient_rotation; - step_size_ = 0.01; - transformation_epsilon_ = 0.001; - delta_p_norm = computeStepLengthMT( - p, delta_p, delta_p_norm, step_size_, transformation_epsilon_ / 2, score, score_gradient, - hessian, output); - } else { - converged_rotation = true; - transformation_epsilon_ = 0.01; - step_size_ = 0.1; - delta_p_norm = computeStepLengthMT( - p, delta_p, delta_p_norm, step_size_, transformation_epsilon_ / 2, score, score_gradient, - hessian, output); - } - - delta_p *= delta_p_norm; - - transformation_ = - (Eigen::Translation( - static_cast(delta_p(0)), static_cast(delta_p(1)), - static_cast(delta_p(2))) * - Eigen::AngleAxis(static_cast(delta_p(3)), Eigen::Vector3f::UnitX()) * - Eigen::AngleAxis(static_cast(delta_p(4)), Eigen::Vector3f::UnitY()) * - Eigen::AngleAxis(static_cast(delta_p(5)), Eigen::Vector3f::UnitZ())) - .matrix(); - - transformation_array_.push_back(final_transformation_); - p = p + delta_p; - - // Update Visualizer (untested) - if (update_visualizer_ != 0) { - update_visualizer_(output, std::vector(), *target_, std::vector()); - } - - if ( - nr_iterations_ > max_iterations_ || (converged_rotation && nr_iterations_ && - (std::fabs(delta_p_norm) < transformation_epsilon_))) { - converged_ = true; - } - - nr_iterations_++; - } - - // Store transformation probability. The realtive differences within each scan registration are - // accurate but the normalization constants need to be modified for it to be globally accurate - trans_probability_ = score / static_cast(input_->points.size()); - - hessian_ = hessian; -} - -////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template -double pcl::NormalDistributionsTransformModified::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; - // Set the value of phi'(0), Equation 1.3 [More, Thuente 1994] - double d_phi_0 = -(score_gradient.dot(step_dir)); - - Eigen::Matrix x_t; - - if (d_phi_0 >= 0) { - // Not a decent direction - if (d_phi_0 == 0) { - return 0; - } else { - // Reverse step direction and calculate optimal step. - d_phi_0 *= -1; - step_dir *= -1; - } - } - - // The Search Algorithm for T(mu) [More, Thuente 1994] - - int max_step_iterations = 10; - int step_iterations = 0; - - // Sufficient decrease constant, Equation 1.1 [More, Thuete 1994] - double mu = 1.e-4; - // Curvature condition constant, Equation 1.2 [More, Thuete 1994] - double nu = 0.9; - - // Initial endpoints of Interval I, - double a_l = 0, a_u = 0; - - // Auxiliary function psi is used until I is determined ot be a closed interval, Equation 2.1 - // [More, Thuente 1994] - double f_l = - NormalDistributionsTransformModified::auxilaryFunction_PsiMT( - a_l, phi_0, phi_0, d_phi_0, mu); - double g_l = - NormalDistributionsTransformModified::auxilaryFunction_dPsiMT( - d_phi_0, d_phi_0, mu); - - double f_u = - NormalDistributionsTransformModified::auxilaryFunction_PsiMT( - a_u, phi_0, phi_0, d_phi_0, mu); - double g_u = - NormalDistributionsTransformModified::auxilaryFunction_dPsiMT( - d_phi_0, d_phi_0, mu); - - // Check used to allow More-Thuente step length calculation to be skipped by making step_min == - // step_max - bool interval_converged = (step_max - step_min) > 0, open_interval = true; - - double a_t = step_init; - a_t = std::min(a_t, step_max); - a_t = std::max(a_t, step_min); - - x_t = x + step_dir * a_t; - - final_transformation_ = - (Eigen::Translation( - static_cast(x_t(0)), static_cast(x_t(1)), static_cast(x_t(2))) * - Eigen::AngleAxis(static_cast(x_t(3)), Eigen::Vector3f::UnitX()) * - Eigen::AngleAxis(static_cast(x_t(4)), Eigen::Vector3f::UnitY()) * - Eigen::AngleAxis(static_cast(x_t(5)), Eigen::Vector3f::UnitZ())) - .matrix(); - - // New transformed point cloud - transformPointCloud(*input_, trans_cloud, final_transformation_); - - // Updates score, gradient and hessian. Hessian calculation is unnecessary but testing showed - // that most step calculations use the initial step suggestion and recalculation the reusable - // portions of the hessian would intail more computation time. - score = NormalDistributionsTransformModified::computeDerivatives( - score_gradient, hessian, trans_cloud, x_t, true); - - // Calculate phi(alpha_t) - double phi_t = -score; - // Calculate phi'(alpha_t) - double d_phi_t = -(score_gradient.dot(step_dir)); - - // Calculate psi(alpha_t) - double psi_t = - NormalDistributionsTransformModified::auxilaryFunction_PsiMT( - a_t, phi_t, phi_0, d_phi_0, mu); - // Calculate psi'(alpha_t) - double d_psi_t = - NormalDistributionsTransformModified::auxilaryFunction_dPsiMT( - d_phi_t, d_phi_0, mu); - - // Iterate until max number of iterations, interval convergence or a value satisfies the - // sufficient decrease, Equation 1.1, and curvature condition, Equation 1.2 [More, Thuente 1994] - - while (!interval_converged && step_iterations < max_step_iterations && - !(psi_t <= 0 /*Sufficient Decrease*/ && d_phi_t <= -nu * d_phi_0 /*Curvature Condition*/)) - // while (!interval_converged && step_iterations < max_step_iterations && !(psi_t <= 0)) - { - // Use auxiliary function if interval I is not closed - if (open_interval) { - a_t = NormalDistributionsTransformModified::trialValueSelectionMT( - a_l, f_l, g_l, a_u, f_u, g_u, a_t, psi_t, d_psi_t); - } else { - a_t = NormalDistributionsTransformModified::trialValueSelectionMT( - a_l, f_l, g_l, a_u, f_u, g_u, a_t, phi_t, d_phi_t); - } - - a_t = std::min(a_t, step_max); - a_t = std::max(a_t, step_min); - - x_t = x + step_dir * a_t; - - final_transformation_ = - (Eigen::Translation( - static_cast(x_t(0)), static_cast(x_t(1)), static_cast(x_t(2))) * - Eigen::AngleAxis(static_cast(x_t(3)), Eigen::Vector3f::UnitX()) * - Eigen::AngleAxis(static_cast(x_t(4)), Eigen::Vector3f::UnitY()) * - Eigen::AngleAxis(static_cast(x_t(5)), Eigen::Vector3f::UnitZ())) - .matrix(); - - // New transformed point cloud - // Done on final cloud to prevent wasted computation - transformPointCloud(*input_, trans_cloud, final_transformation_); - - // Updates score, gradient. Values stored to prevent wasted computation. - score = NormalDistributionsTransformModified::computeDerivatives( - score_gradient, hessian, trans_cloud, x_t, false); - - // Calculate phi(alpha_t+) - phi_t = -score; - // Calculate phi'(alpha_t+) - d_phi_t = -(score_gradient.dot(step_dir)); - - // Calculate psi(alpha_t+) - psi_t = NormalDistributionsTransformModified::auxilaryFunction_PsiMT( - a_t, phi_t, phi_0, d_phi_0, mu); - // Calculate psi'(alpha_t+) - d_psi_t = - NormalDistributionsTransformModified::auxilaryFunction_dPsiMT( - d_phi_t, d_phi_0, mu); - - // Check if I is now a closed interval - if (open_interval && (psi_t <= 0 && d_psi_t >= 0)) { - open_interval = false; - - // Converts f_l and g_l from psi to phi - f_l = f_l + phi_0 - mu * d_phi_0 * a_l; - g_l = g_l + mu * d_phi_0; - - // Converts f_u and g_u from psi to phi - f_u = f_u + phi_0 - mu * d_phi_0 * a_u; - g_u = g_u + mu * d_phi_0; - } - - if (open_interval) { - // Update interval end points using Updating Algorithm [More, Thuente 1994] - interval_converged = - NormalDistributionsTransformModified::updateIntervalMT( - a_l, f_l, g_l, a_u, f_u, g_u, a_t, psi_t, d_psi_t); - } else { - // Update interval end points using Modified Updating Algorithm [More, Thuente 1994] - interval_converged = - NormalDistributionsTransformModified::updateIntervalMT( - a_l, f_l, g_l, a_u, f_u, g_u, a_t, phi_t, d_phi_t); - } - - step_iterations++; - } - - // 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) { -#if PCL_VERSION >= PCL_VERSION_CALC(1, 12, 1) - NormalDistributionsTransformModified::computeHessian( - hessian, trans_cloud); -#else - NormalDistributionsTransformModified::computeHessian( - hessian, trans_cloud, x_t); -#endif - } - - return a_t; -} - -#endif // NDT_PCL_MODIFIED__IMPL__NDT_HPP_ diff --git a/localization/ndt_pcl_modified/include/ndt_pcl_modified/ndt.hpp b/localization/ndt_pcl_modified/include/ndt_pcl_modified/ndt.hpp deleted file mode 100644 index 7a9958bfdd441..0000000000000 --- a/localization/ndt_pcl_modified/include/ndt_pcl_modified/ndt.hpp +++ /dev/null @@ -1,124 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -/* - * Software License Agreement (BSD License) - * - * Point Cloud Library (PCL) - www.pointclouds.org - * Copyright (c) 2010-2012, Willow Garage, Inc. - * Copyright (c) 2012-, Open Perception, Inc. - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder(s) nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * $Id$ - * - */ - -#ifndef NDT_PCL_MODIFIED__NDT_HPP_ -#define NDT_PCL_MODIFIED__NDT_HPP_ - -#include - -#include - -#include - -namespace pcl -{ -template -class NormalDistributionsTransformModified -: public NormalDistributionsTransform -{ -protected: - typedef typename Registration::PointCloudSource PointCloudSource; - -public: - void computeTransformation(PointCloudSource & output, const Eigen::Matrix4f & guess) override; - - inline const Eigen::Matrix getHessian() const { return hessian_; } - - inline const std::vector> - getFinalTransformationArray() const - { - return transformation_array_; - } - -protected: - virtual double 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); - - using Registration::input_; - using Registration::target_; - using Registration::nr_iterations_; - using Registration::max_iterations_; - using Registration::previous_transformation_; - using Registration::final_transformation_; - using Registration::transformation_; - using Registration::transformation_epsilon_; - using Registration::converged_; - using Registration::update_visualizer_; - - using NormalDistributionsTransform::outlier_ratio_; - using NormalDistributionsTransform::resolution_; - using NormalDistributionsTransform::step_size_; - using NormalDistributionsTransform::gauss_d1_; - using NormalDistributionsTransform::gauss_d2_; -#if PCL_VERSION >= PCL_VERSION_CALC(1, 12, 1) - using NormalDistributionsTransform::point_jacobian_; -#else - using NormalDistributionsTransform::point_gradient_; -#endif - using NormalDistributionsTransform::point_hessian_; - using NormalDistributionsTransform::trans_probability_; - - Eigen::Matrix hessian_; - std::vector> transformation_array_; - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -} // namespace pcl - -#include "ndt_pcl_modified/impl/ndt.hpp" - -#endif // NDT_PCL_MODIFIED__NDT_HPP_ diff --git a/localization/ndt_pcl_modified/package.xml b/localization/ndt_pcl_modified/package.xml deleted file mode 100644 index 8da6a7e472d6b..0000000000000 --- a/localization/ndt_pcl_modified/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - ndt_pcl_modified - 0.1.0 - The ndt_pcl_modified package - Yamato Ando - Apache License 2.0 - Yamato Ando - - ament_cmake - - autoware_cmake - - libpcl-all-dev - - ament_cmake_cppcheck - ament_lint_auto - - - ament_cmake - - diff --git a/localization/ndt_pcl_modified/src/ndt.cpp b/localization/ndt_pcl_modified/src/ndt.cpp deleted file mode 100644 index 56345e5d8ab93..0000000000000 --- a/localization/ndt_pcl_modified/src/ndt.cpp +++ /dev/null @@ -1,50 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Point Cloud Library (PCL) - www.pointclouds.org - * Copyright (c) 2010-2011, Willow Garage, Inc. - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * $Id$ - * - */ - -#include "ndt_pcl_modified/ndt.hpp" - -#include - -#include - -template class PCL_EXPORTS pcl::NormalDistributionsTransformModified; -template class PCL_EXPORTS - pcl::NormalDistributionsTransformModified; -template class PCL_EXPORTS - pcl::NormalDistributionsTransformModified; diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index d2d5568e265a9..bc8c6fb12534c 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -22,6 +22,9 @@ else() endif() endif() +find_package(PCL REQUIRED COMPONENTS common io registration) +include_directories(${PCL_INCLUDE_DIRS}) + ament_auto_add_executable(ndt_scan_matcher src/debug.cpp src/ndt_scan_matcher_node.cpp @@ -31,6 +34,9 @@ ament_auto_add_executable(ndt_scan_matcher src/tf2_listener_module.cpp ) +link_directories(${PCL_LIBRARY_DIRS}) +target_link_libraries(ndt_scan_matcher ${PCL_LIBRARIES}) + ament_auto_package( INSTALL_TO_SHARE launch diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index e1762eaccc87e..128214843d0a9 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -56,14 +56,16 @@ One optional function is regularization. Please see the regularization chapter i | --------------------------------------- | ------ | ----------------------------------------------------------------------------------------------- | | `base_frame` | string | Vehicle reference frame | | `input_sensor_points_queue_size` | int | Subscriber queue size | -| `ndt_implement_type` | int | NDT implementation type (0=PCL_GENERIC, 1=PCL_MODIFIED, 2=OMP) | | `trans_epsilon` | double | The maximum difference between two consecutive transformations in order to consider convergence | | `step_size` | double | The newton line search maximum step length | | `resolution` | double | The ND voxel grid resolution [m] | | `max_iterations` | int | The number of iterations required to calculate alignment | +| `converged_param_type` | int | The type of indicators for scan matching score (0: TP, 1: NVTL) | | `converged_param_transform_probability` | double | Threshold for deciding whether to trust the estimation result | -| `omp_neighborhood_search_method` | int | neighborhood search method in OMP (0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1) | -| `omp_num_threads` | int | Number of threads used for parallel computing | +| `neighborhood_search_method` | int | neighborhood search method (0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1) | +| `num_threads` | int | Number of threads used for parallel computing | + +(TP: Transform Probability, NVTL: Nearest Voxel Transform Probability) ## Regularization @@ -110,7 +112,6 @@ $$ Regularization is disabled by default. If you wish to use it, please edit the following parameters to enable it. -Regularization is only available for `NDT_OMP` and not for other NDT implementation types (`PCL_GENERIC`, `PCL_MODIFIED`). #### Where is regularization available diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index 58e7d91826777..decc41795f496 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -7,10 +7,6 @@ # Subscriber queue size input_sensor_points_queue_size: 1 - # NDT implementation type - # 0=PCL_GENERIC, 1=PCL_MODIFIED, 2=OMP - ndt_implement_type: 2 - # The maximum difference between two consecutive # transformations in order to consider convergence trans_epsilon: 0.01 @@ -26,7 +22,6 @@ # Converged param type # 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD - # NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD is only available when NDTImplementType::OMP is selected converged_param_type: 1 # If converged_param_type is 0 @@ -46,12 +41,12 @@ # Tolerance of distance difference between two initial poses used for linear interpolation. [m] initial_pose_distance_tolerance_m: 10.0 - # neighborhood search method in OMP + # neighborhood search method # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1 - omp_neighborhood_search_method: 0 + neighborhood_search_method: 0 # Number of threads used for parallel computing - omp_num_threads: 4 + num_threads: 4 # The covariance of output pose # Do note that this covariance matrix is empirically derived diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index f9bf43946b670..0dd3ae9015e7a 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -20,9 +20,6 @@ #include "ndt_scan_matcher/particle.hpp" #include "ndt_scan_matcher/tf2_listener_module.hpp" -#include -#include -#include #include #include @@ -37,6 +34,8 @@ #include #include +#include +#include #include #ifdef ROS_DISTRO_GALACTIC @@ -62,7 +61,6 @@ #include #include -enum class NDTImplementType { PCL_GENERIC = 0, PCL_MODIFIED = 1, OMP = 2 }; enum class ConvergedParamType { TRANSFORM_PROBABILITY = 0, NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1 @@ -77,40 +75,23 @@ struct NdtResult std::vector transformation_array; }; -template -std::shared_ptr> get_ndt( - const NDTImplementType & ndt_mode) +struct NDTParams { - std::shared_ptr> ndt_ptr; - if (ndt_mode == NDTImplementType::PCL_GENERIC) { - ndt_ptr.reset(new NormalDistributionsTransformPCLGeneric); - return ndt_ptr; - } - if (ndt_mode == NDTImplementType::PCL_MODIFIED) { - ndt_ptr.reset(new NormalDistributionsTransformPCLModified); - return ndt_ptr; - } - if (ndt_mode == NDTImplementType::OMP) { - ndt_ptr.reset(new NormalDistributionsTransformOMP); - return ndt_ptr; - } - - const std::string s = fmt::format("Unknown NDT type {}", static_cast(ndt_mode)); - throw std::runtime_error(s); -} + double trans_epsilon; + double step_size; + double resolution; + int max_iterations; + pclomp::NeighborSearchMethod search_method; + int num_threads; + float regularization_scale_factor; +}; class NDTScanMatcher : public rclcpp::Node { using PointSource = pcl::PointXYZ; using PointTarget = pcl::PointXYZ; - - // TODO(Tier IV): move file - struct OMPParams - { - OMPParams() : search_method(pclomp::NeighborSearchMethod::KDTREE), num_threads(1) {} - pclomp::NeighborSearchMethod search_method; - int num_threads; - }; + using NormalDistributionsTransform = + pclomp::NormalDistributionsTransform; public: NDTScanMatcher(); @@ -132,7 +113,7 @@ class NDTScanMatcher : public rclcpp::Node NdtResult align(const geometry_msgs::msg::Pose & initial_pose_msg); geometry_msgs::msg::PoseWithCovarianceStamped align_using_monte_carlo( - const std::shared_ptr> & ndt_ptr, + const std::shared_ptr & ndt_ptr, const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov); void transform_sensor_measurement( @@ -202,8 +183,7 @@ class NDTScanMatcher : public rclcpp::Node tf2_ros::TransformBroadcaster tf2_broadcaster_; - NDTImplementType ndt_implement_type_; - std::shared_ptr> ndt_ptr_; + std::shared_ptr ndt_ptr_; Eigen::Matrix4f base_to_sensor_matrix_; std::string base_frame_; @@ -226,14 +206,13 @@ class NDTScanMatcher : public rclcpp::Node std::mutex ndt_ptr_mtx_; std::mutex initial_pose_array_mtx_; - OMPParams omp_params_; + NDTParams ndt_params_; std::thread diagnostic_thread_; std::map key_value_stdmap_; // variables for regularization const bool regularization_enabled_; - const float regularization_scale_factor_; std::deque regularization_pose_msg_ptr_array_; diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index 0f862729b1143..13b0f186ceb98 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -17,10 +17,9 @@ diagnostic_msgs fmt geometry_msgs + libpcl-all-dev nav_msgs - ndt ndt_omp - ndt_pcl_modified pcl_conversions rclcpp sensor_msgs diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 5ddcd167bc549..4c4e1beeb772f 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -82,7 +82,7 @@ bool validate_local_optimal_solution_oscillation( NDTScanMatcher::NDTScanMatcher() : Node("ndt_scan_matcher"), tf2_broadcaster_(*this), - ndt_implement_type_(NDTImplementType::PCL_GENERIC), + ndt_ptr_(new NormalDistributionsTransform), base_frame_("base_link"), ndt_base_frame_("ndt_base_link"), map_frame_("map"), @@ -94,40 +94,11 @@ NDTScanMatcher::NDTScanMatcher() initial_pose_distance_tolerance_m_(10.0), inversion_vector_threshold_(-0.9), oscillation_threshold_(10), - regularization_enabled_(declare_parameter("regularization_enabled", false)), - regularization_scale_factor_(declare_parameter("regularization_scale_factor", 0.01)) + regularization_enabled_(declare_parameter("regularization_enabled", false)) { key_value_stdmap_["state"] = "Initializing"; is_activated_ = false; - int ndt_implement_type_tmp = this->declare_parameter("ndt_implement_type", 0); - ndt_implement_type_ = static_cast(ndt_implement_type_tmp); - - RCLCPP_INFO(get_logger(), "NDT Implement Type is %d", ndt_implement_type_tmp); - try { - ndt_ptr_ = get_ndt(ndt_implement_type_); - } catch (std::exception & e) { - RCLCPP_ERROR(get_logger(), "%s", e.what()); - return; - } - - if (ndt_implement_type_ == NDTImplementType::OMP) { - using T = NormalDistributionsTransformOMP; - - // FIXME(IshitaTakeshi) Not sure if this is safe - std::shared_ptr ndt_omp_ptr = std::dynamic_pointer_cast(ndt_ptr_); - int search_method = static_cast(omp_params_.search_method); - search_method = this->declare_parameter("omp_neighborhood_search_method", search_method); - omp_params_.search_method = static_cast(search_method); - // TODO(Tier IV): check search_method is valid value. - ndt_omp_ptr->setNeighborhoodSearchMethod(omp_params_.search_method); - - omp_params_.num_threads = this->declare_parameter("omp_num_threads", omp_params_.num_threads); - omp_params_.num_threads = std::max(omp_params_.num_threads, 1); - ndt_omp_ptr->setNumThreads(omp_params_.num_threads); - ndt_ptr_ = ndt_omp_ptr; - } - int points_queue_size = this->declare_parameter("input_sensor_points_queue_size", 0); points_queue_size = std::max(points_queue_size, 0); RCLCPP_INFO(get_logger(), "points_queue_size: %d", points_queue_size); @@ -138,35 +109,30 @@ NDTScanMatcher::NDTScanMatcher() ndt_base_frame_ = this->declare_parameter("ndt_base_frame", ndt_base_frame_); RCLCPP_INFO(get_logger(), "ndt_base_frame_id: %s", ndt_base_frame_.c_str()); - double trans_epsilon = ndt_ptr_->getTransformationEpsilon(); - double step_size = ndt_ptr_->getStepSize(); - double resolution = ndt_ptr_->getResolution(); - int max_iterations = ndt_ptr_->getMaximumIterations(); - trans_epsilon = this->declare_parameter("trans_epsilon", trans_epsilon); - step_size = this->declare_parameter("step_size", step_size); - resolution = this->declare_parameter("resolution", resolution); - max_iterations = this->declare_parameter("max_iterations", max_iterations); - ndt_ptr_->setTransformationEpsilon(trans_epsilon); - ndt_ptr_->setStepSize(step_size); - ndt_ptr_->setResolution(resolution); - ndt_ptr_->setMaximumIterations(max_iterations); - ndt_ptr_->setRegularizationScaleFactor(regularization_scale_factor_); + ndt_params_.trans_epsilon = this->declare_parameter("trans_epsilon"); + ndt_params_.step_size = this->declare_parameter("step_size"); + ndt_params_.resolution = this->declare_parameter("resolution"); + ndt_params_.max_iterations = this->declare_parameter("max_iterations"); + int search_method = this->declare_parameter("neighborhood_search_method"); + ndt_params_.search_method = static_cast(search_method); + ndt_params_.num_threads = this->declare_parameter("num_threads"); + ndt_params_.num_threads = std::max(ndt_params_.num_threads, 1); + + ndt_ptr_->setTransformationEpsilon(ndt_params_.trans_epsilon); + ndt_ptr_->setStepSize(ndt_params_.step_size); + ndt_ptr_->setResolution(ndt_params_.resolution); + ndt_ptr_->setMaximumIterations(ndt_params_.max_iterations); + ndt_ptr_->setRegularizationScaleFactor(ndt_params_.regularization_scale_factor); + ndt_ptr_->setNeighborhoodSearchMethod(ndt_params_.search_method); + ndt_ptr_->setNumThreads(ndt_params_.num_threads); RCLCPP_INFO( get_logger(), "trans_epsilon: %lf, step_size: %lf, resolution: %lf, max_iterations: %d", - trans_epsilon, step_size, resolution, max_iterations); + ndt_params_.trans_epsilon, ndt_params_.step_size, ndt_params_.resolution, + ndt_params_.max_iterations); int converged_param_type_tmp = this->declare_parameter("converged_param_type", 0); converged_param_type_ = static_cast(converged_param_type_tmp); - if ( - ndt_implement_type_ != NDTImplementType::OMP && - converged_param_type_ == ConvergedParamType::NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) { - RCLCPP_ERROR( - get_logger(), - "ConvergedParamType::NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD is only available when " - "NDTImplementType::OMP is selected."); - return; - } converged_param_transform_probability_ = this->declare_parameter( "converged_param_transform_probability", converged_param_transform_probability_); @@ -395,29 +361,14 @@ void NDTScanMatcher::callback_regularization_pose( void NDTScanMatcher::callback_map_points( sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr) { - const auto trans_epsilon = ndt_ptr_->getTransformationEpsilon(); - const auto step_size = ndt_ptr_->getStepSize(); - const auto resolution = ndt_ptr_->getResolution(); - const auto max_iterations = ndt_ptr_->getMaximumIterations(); - - using NDTBase = NormalDistributionsTransformBase; - std::shared_ptr new_ndt_ptr = get_ndt(ndt_implement_type_); - - if (ndt_implement_type_ == NDTImplementType::OMP) { - using T = NormalDistributionsTransformOMP; - - // FIXME(IshitaTakeshi) Not sure if this is safe - std::shared_ptr ndt_omp_ptr = std::dynamic_pointer_cast(ndt_ptr_); - ndt_omp_ptr->setNeighborhoodSearchMethod(omp_params_.search_method); - ndt_omp_ptr->setNumThreads(omp_params_.num_threads); - new_ndt_ptr = ndt_omp_ptr; - } - - new_ndt_ptr->setTransformationEpsilon(trans_epsilon); - new_ndt_ptr->setStepSize(step_size); - new_ndt_ptr->setResolution(resolution); - new_ndt_ptr->setMaximumIterations(max_iterations); - new_ndt_ptr->setRegularizationScaleFactor(regularization_scale_factor_); + std::shared_ptr new_ndt_ptr(new NormalDistributionsTransform); + new_ndt_ptr->setTransformationEpsilon(ndt_params_.trans_epsilon); + new_ndt_ptr->setStepSize(ndt_params_.step_size); + new_ndt_ptr->setResolution(ndt_params_.resolution); + new_ndt_ptr->setMaximumIterations(ndt_params_.max_iterations); + new_ndt_ptr->setRegularizationScaleFactor(ndt_params_.regularization_scale_factor); + new_ndt_ptr->setNeighborhoodSearchMethod(ndt_params_.search_method); + new_ndt_ptr->setNumThreads(ndt_params_.num_threads); pcl::shared_ptr> map_points_ptr(new pcl::PointCloud); pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr); @@ -469,8 +420,7 @@ void NDTScanMatcher::callback_sensor_points( initial_pose_array_lock.unlock(); // if regularization is enabled and available, set pose to NDT for regularization - if (regularization_enabled_ && (ndt_implement_type_ == NDTImplementType::OMP)) - add_regularization_pose(sensor_ros_time); + if (regularization_enabled_) add_regularization_pose(sensor_ros_time); if (ndt_ptr_->getInputTarget() == nullptr) { RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1, "No MAP!"); @@ -592,7 +542,7 @@ NdtResult NDTScanMatcher::align(const geometry_msgs::msg::Pose & initial_pose_ms } geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_using_monte_carlo( - const std::shared_ptr> & ndt_ptr, + const std::shared_ptr & ndt_ptr, const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov) { if (ndt_ptr->getInputTarget() == nullptr || ndt_ptr->getInputSource() == nullptr) {