diff --git a/localization/ndt/CMakeLists.txt b/localization/ndt/CMakeLists.txt new file mode 100644 index 0000000000000..efecc45b0b901 --- /dev/null +++ b/localization/ndt/CMakeLists.txt @@ -0,0 +1,60 @@ +cmake_minimum_required(VERSION 3.5) +project(ndt) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(PCL REQUIRED COMPONENTS common io registration) +find_package(ndt_omp REQUIRED) +find_package(ndt_pcl_modified REQUIRED) + +add_library(ndt + src/base.cpp + src/pcl_generic.cpp + src/pcl_modified.cpp + src/omp.cpp +) + +target_include_directories(ndt + PUBLIC + $ + $ +) + +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 +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/localization/ndt/README.md b/localization/ndt/README.md new file mode 100644 index 0000000000000..a0f1b358185f6 --- /dev/null +++ b/localization/ndt/README.md @@ -0,0 +1,12 @@ +# 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 new file mode 100644 index 0000000000000..b492faffabbc7 --- /dev/null +++ b/localization/ndt/include/ndt/base.hpp @@ -0,0 +1,60 @@ +// 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 NORMAL_DISTRIBUTIONS_TRANSFORM_BASE_H +#define NORMAL_DISTRIBUTIONS_TRANSFORM_BASE_H + +#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 boost::shared_ptr> & map_ptr) = 0; + virtual void setInputSource(const boost::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 getFitnessScore() = 0; + virtual boost::shared_ptr> getInputTarget() const = 0; + virtual boost::shared_ptr> getInputSource() const = 0; + virtual Eigen::Matrix4f getFinalTransformation() const = 0; + virtual std::vector getFinalTransformationArray() const = 0; + + virtual Eigen::Matrix getHessian() const = 0; + + virtual boost::shared_ptr> getSearchMethodTarget() const = 0; +}; + +#include "ndt/impl/base.hpp" + +#endif diff --git a/localization/ndt/include/ndt/impl/base.hpp b/localization/ndt/include/ndt/impl/base.hpp new file mode 100644 index 0000000000000..897712bfcd038 --- /dev/null +++ b/localization/ndt/include/ndt/impl/base.hpp @@ -0,0 +1,25 @@ +// 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 NORMAL_DISTRIBUTIONS_TRANSFORM_BASE_HPP +#define NORMAL_DISTRIBUTIONS_TRANSFORM_BASE_HPP + +#include "ndt/base.hpp" + +template +NormalDistributionsTransformBase::NormalDistributionsTransformBase() +{ +} + +#endif // NORMAL_DISTRIBUTIONS_TRANSFORM_BASE_HPP diff --git a/localization/ndt/include/ndt/impl/omp.hpp b/localization/ndt/include/ndt/impl/omp.hpp new file mode 100644 index 0000000000000..63849e07be481 --- /dev/null +++ b/localization/ndt/include/ndt/impl/omp.hpp @@ -0,0 +1,186 @@ +// 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 NORMAL_DISTRIBUTIONS_TRANSFORM_OMP_HPP +#define NORMAL_DISTRIBUTIONS_TRANSFORM_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 boost::shared_ptr> & map_ptr) +{ + ndt_ptr_->setInputTarget(map_ptr); +} + +template +void NormalDistributionsTransformOMP::setInputSource( + const boost::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::getFitnessScore() +{ + return ndt_ptr_->getFitnessScore(); +} + +template +boost::shared_ptr> +NormalDistributionsTransformOMP::getInputTarget() const +{ + return ndt_ptr_->getInputTarget(); +} + +template +boost::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 +boost::shared_ptr> +NormalDistributionsTransformOMP::getSearchMethodTarget() const +{ + return ndt_ptr_->getSearchMethodTarget(); +} + +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 // NORMAL_DISTRIBUTIONS_TRANSFORM_OMP_HPP diff --git a/localization/ndt/include/ndt/impl/pcl_generic.hpp b/localization/ndt/include/ndt/impl/pcl_generic.hpp new file mode 100644 index 0000000000000..f2616d443f78a --- /dev/null +++ b/localization/ndt/include/ndt/impl/pcl_generic.hpp @@ -0,0 +1,162 @@ +// 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 NORMAL_DISTRIBUTIONS_TRANSFORM_PCL_GENERIC_HPP +#define NORMAL_DISTRIBUTIONS_TRANSFORM_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 boost::shared_ptr> & map_ptr) +{ + ndt_ptr_->setInputTarget(map_ptr); +} + +template +void NormalDistributionsTransformPCLGeneric::setInputSource( + const boost::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::getFitnessScore() +{ + return ndt_ptr_->getFitnessScore(); +} + +template +boost::shared_ptr> +NormalDistributionsTransformPCLGeneric::getInputTarget() const +{ + return ndt_ptr_->getInputTarget(); +} + +template +boost::shared_ptr> +NormalDistributionsTransformPCLGeneric::getInputSource() const +{ + return ndt_ptr_->getInputSource(); +} + +template +Eigen::Matrix4f +NormalDistributionsTransformPCLGeneric::getFinalTransformation() const +{ + return ndt_ptr_->getFinalTransformation(); +} + +template +std::vector NormalDistributionsTransformPCLGeneric< + PointSource, PointTarget>::getFinalTransformationArray() const +{ + return std::vector(); +} + +template +Eigen::Matrix +NormalDistributionsTransformPCLGeneric::getHessian() const +{ + // return ndt_ptr_->getHessian(); + return Eigen::Matrix(); +} + +template +boost::shared_ptr> +NormalDistributionsTransformPCLGeneric::getSearchMethodTarget() const +{ + return ndt_ptr_->getSearchMethodTarget(); +} + +#endif // NORMAL_DISTRIBUTIONS_TRANSFORM_PCL_GENERIC_HPP diff --git a/localization/ndt/include/ndt/impl/pcl_modified.hpp b/localization/ndt/include/ndt/impl/pcl_modified.hpp new file mode 100644 index 0000000000000..ff6c9ef045a7e --- /dev/null +++ b/localization/ndt/include/ndt/impl/pcl_modified.hpp @@ -0,0 +1,162 @@ +// 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 NORMAL_DISTRIBUTIONS_TRANSFORM_PCL_MODIFIED_HPP +#define NORMAL_DISTRIBUTIONS_TRANSFORM_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 boost::shared_ptr> & map_ptr) +{ + ndt_ptr_->setInputTarget(map_ptr); +} + +template +void NormalDistributionsTransformPCLModified::setInputSource( + const boost::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::getFitnessScore() +{ + return ndt_ptr_->getFitnessScore(); +} + +template +boost::shared_ptr> +NormalDistributionsTransformPCLModified::getInputTarget() const +{ + return ndt_ptr_->getInputTarget(); +} + +template +boost::shared_ptr> +NormalDistributionsTransformPCLModified::getInputSource() const +{ + return ndt_ptr_->getInputSource(); +} + +template +Eigen::Matrix4f +NormalDistributionsTransformPCLModified::getFinalTransformation() const +{ + return ndt_ptr_->getFinalTransformation(); +} + +template +std::vector NormalDistributionsTransformPCLModified< + PointSource, PointTarget>::getFinalTransformationArray() const +{ + return ndt_ptr_->getFinalTransformationArray(); +} + +template +Eigen::Matrix +NormalDistributionsTransformPCLModified::getHessian() const +{ + return ndt_ptr_->getHessian(); +} + +template +boost::shared_ptr> +NormalDistributionsTransformPCLModified::getSearchMethodTarget() const +{ + return ndt_ptr_->getSearchMethodTarget(); +} + +#endif // NORMAL_DISTRIBUTIONS_TRANSFORM_PCL_MODIFIED_HPP diff --git a/localization/ndt/include/ndt/omp.hpp b/localization/ndt/include/ndt/omp.hpp new file mode 100644 index 0000000000000..6515625e70dc4 --- /dev/null +++ b/localization/ndt/include/ndt/omp.hpp @@ -0,0 +1,73 @@ +// 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 NORMAL_DISTRIBUTIONS_TRANSFORM_OMP_H +#define NORMAL_DISTRIBUTIONS_TRANSFORM_OMP_H + +#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 boost::shared_ptr> & map_ptr) override; + void setInputSource(const boost::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 getFitnessScore() override; + boost::shared_ptr> getInputTarget() const override; + boost::shared_ptr> getInputSource() const override; + Eigen::Matrix4f getFinalTransformation() const override; + std::vector getFinalTransformationArray() const override; + + Eigen::Matrix getHessian() const override; + + boost::shared_ptr> getSearchMethodTarget() const override; + + // only OMP Impl + void setNumThreads(int n); + void setNeighborhoodSearchMethod(pclomp::NeighborSearchMethod method); + + int getNumThreads() const; + pclomp::NeighborSearchMethod getNeighborhoodSearchMethod() const; + +private: + boost::shared_ptr> ndt_ptr_; +}; + +#include "ndt/impl/omp.hpp" + +#endif diff --git a/localization/ndt/include/ndt/pcl_generic.hpp b/localization/ndt/include/ndt/pcl_generic.hpp new file mode 100644 index 0000000000000..859d77affd879 --- /dev/null +++ b/localization/ndt/include/ndt/pcl_generic.hpp @@ -0,0 +1,66 @@ +// 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 NORMAL_DISTRIBUTIONS_TRANSFORM_PCL_GENERIC_H +#define NORMAL_DISTRIBUTIONS_TRANSFORM_PCL_GENERIC_H + +#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 boost::shared_ptr> & map_ptr) override; + void setInputSource(const boost::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 getFitnessScore() override; + boost::shared_ptr> getInputTarget() const override; + boost::shared_ptr> getInputSource() const override; + Eigen::Matrix4f getFinalTransformation() const override; + std::vector getFinalTransformationArray() const override; + + Eigen::Matrix getHessian() const override; + + boost::shared_ptr> getSearchMethodTarget() const override; + +private: + boost::shared_ptr> ndt_ptr_; +}; + +#include "ndt/impl/pcl_generic.hpp" + +#endif diff --git a/localization/ndt/include/ndt/pcl_modified.hpp b/localization/ndt/include/ndt/pcl_modified.hpp new file mode 100644 index 0000000000000..e657ff56f45f9 --- /dev/null +++ b/localization/ndt/include/ndt/pcl_modified.hpp @@ -0,0 +1,67 @@ +// 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 NORMAL_DISTRIBUTIONS_TRANSFORM_PCL_MODIFIED_H +#define NORMAL_DISTRIBUTIONS_TRANSFORM_PCL_MODIFIED_H + +#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 boost::shared_ptr> & map_ptr) override; + void setInputSource(const boost::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 getFitnessScore() override; + boost::shared_ptr> getInputTarget() const override; + boost::shared_ptr> getInputSource() const override; + Eigen::Matrix4f getFinalTransformation() const override; + std::vector getFinalTransformationArray() const override; + + Eigen::Matrix getHessian() const override; + + boost::shared_ptr> getSearchMethodTarget() const override; + +private: + boost::shared_ptr> ndt_ptr_; +}; + +#include "ndt/impl/pcl_modified.hpp" + +#endif diff --git a/localization/ndt/package.xml b/localization/ndt/package.xml new file mode 100644 index 0000000000000..e9bd1037a3b77 --- /dev/null +++ b/localization/ndt/package.xml @@ -0,0 +1,21 @@ + + + + ndt + 0.1.0 + The ndt package + Yamato Ando + Apache License 2.0 + ament_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 new file mode 100644 index 0000000000000..3d869d12398a3 --- /dev/null +++ b/localization/ndt/src/base.cpp @@ -0,0 +1,20 @@ +// 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 new file mode 100644 index 0000000000000..075b8e9a6c451 --- /dev/null +++ b/localization/ndt/src/omp.cpp @@ -0,0 +1,20 @@ +// 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 new file mode 100644 index 0000000000000..a6c8297e33456 --- /dev/null +++ b/localization/ndt/src/pcl_generic.cpp @@ -0,0 +1,20 @@ +// 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 new file mode 100644 index 0000000000000..36ec65ba84acb --- /dev/null +++ b/localization/ndt/src/pcl_modified.cpp @@ -0,0 +1,20 @@ +// 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 new file mode 100644 index 0000000000000..209da7a715bbc --- /dev/null +++ b/localization/ndt_pcl_modified/CMakeLists.txt @@ -0,0 +1,49 @@ +cmake_minimum_required(VERSION 3.5) +project(ndt_pcl_modified) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(ament_cmake REQUIRED) +find_package(PCL REQUIRED COMPONENTS common) + +add_library(ndt_pcl_modified + src/ndt.cpp +) + +target_include_directories(ndt_pcl_modified + PUBLIC + $ + $ +) + +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 +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/localization/ndt_pcl_modified/README.md b/localization/ndt_pcl_modified/README.md new file mode 100644 index 0000000000000..26950c20128c3 --- /dev/null +++ b/localization/ndt_pcl_modified/README.md @@ -0,0 +1,11 @@ +# 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 new file mode 100644 index 0000000000000..492bc57d84075 --- /dev/null +++ b/localization/ndt_pcl_modified/include/ndt_pcl_modified/impl/ndt.hpp @@ -0,0 +1,376 @@ +// 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 PCL_REGISTRATION_NDT_MODIFIED_IMPL_H_ +#define PCL_REGISTRATION_NDT_MODIFIED_IMPL_H_ + +#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 + point_gradient_.setZero(); + point_gradient_.block(0, 0, 3, 3).setIdentity(); + 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) { + NormalDistributionsTransformModified::computeHessian( + hessian, trans_cloud, x_t); + } + + return a_t; +} + +#endif // PCL_REGISTRATION_NDT_IMPL_MODIFIED_H_ diff --git a/localization/ndt_pcl_modified/include/ndt_pcl_modified/ndt.hpp b/localization/ndt_pcl_modified/include/ndt_pcl_modified/ndt.hpp new file mode 100644 index 0000000000000..d2306cc49dcbb --- /dev/null +++ b/localization/ndt_pcl_modified/include/ndt_pcl_modified/ndt.hpp @@ -0,0 +1,119 @@ +// 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 PCL_REGISTRATION_NDT_MODIFIED_H_ +#define PCL_REGISTRATION_NDT_MODIFIED_H_ + +#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_; + using NormalDistributionsTransform::point_gradient_; + 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 // PCL_REGISTRATION_NDT_MODIFIED_H_ diff --git a/localization/ndt_pcl_modified/package.xml b/localization/ndt_pcl_modified/package.xml new file mode 100644 index 0000000000000..8d73051072765 --- /dev/null +++ b/localization/ndt_pcl_modified/package.xml @@ -0,0 +1,19 @@ + + + + ndt_pcl_modified + 0.1.0 + The ndt_pcl_modified package + Yamato Ando + Apache License 2.0 + ament_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 new file mode 100644 index 0000000000000..56345e5d8ab93 --- /dev/null +++ b/localization/ndt_pcl_modified/src/ndt.cpp @@ -0,0 +1,50 @@ +/* + * 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 new file mode 100644 index 0000000000000..b8749d5cf836f --- /dev/null +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.5) +project(ndt_scan_matcher) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_executable(ndt_scan_matcher + src/debug.cpp + src/ndt_scan_matcher_node.cpp + src/ndt_scan_matcher_core.cpp + src/util_func.cpp +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md new file mode 100644 index 0000000000000..82d9b19577047 --- /dev/null +++ b/localization/ndt_scan_matcher/README.md @@ -0,0 +1,61 @@ +# ndt_scan_matcher + +## Purpose + +ndt_scan_matcher is a package for position estimation using the NDT scan matching method. + +There are two main functions in this package: + +- estimate position by scan matching +- estimate initial position via the ROS service using the Monte Carlo method + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| -------------------------- | ----------------------------------------------- | ----------------- | +| `ekf_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | initial pose | +| `pointcloud_map` | `sensor_msgs::msg::PointCloud2` | map pointcloud | +| `points_raw` | `sensor_msgs::msg::PointCloud2` | sensor pointcloud | + +### Output + +| Name | Type | Description | +| --------------------------------- | ----------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------- | +| `ndt_pose` | `geometry_msgs::msg::PoseStamped` | estimated pose | +| `ndt_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | estimated pose with covariance | +| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | diagnostics | +| `points_aligned` | `sensor_msgs::msg::PointCloud2` | [debug topic] pointcloud aligned by scan matching | +| `initial_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | [debug topic] initial pose used in scan matching | +| `exe_time_ms` | `autoware_debug_msgs::msg::Float32Stamped` | [debug topic] execution time for scan matching [ms] | +| `transform_probability` | `autoware_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching | +| `iteration_num` | `autoware_debug_msgs::msg::Int32Stamped` | [debug topic] number of scan matching iterations | +| `initial_to_result_distance` | `autoware_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the initial point and the convergence point [m] | +| `initial_to_result_distance_old` | `autoware_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the older of the two initial points used in linear interpolation and the convergence point [m] | +| `initial_to_result_distance_new` | `autoware_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the newer of the two initial points used in linear interpolation and the convergence point [m] | +| `ndt_marker` | `visualization_msgs::msg::MarkerArray` | [debug topic] markers for debugging | +| `monte_carlo_initial_pose_marker` | `visualization_msgs::msg::MarkerArray` | [debug topic] particles used in initial position estimation | + +### Service + +| Name | Type | Description | +| --------------- | ------------------------------------------------------------ | -------------------------------- | +| `ndt_align_srv` | `autoware_localization_srvs::srv::PoseWithCovarianceStamped` | service to estimate initial pose | + +## Parameters + +### Core Parameters + +| Name | Type | Description | +| --------------------------------------- | ------ | ----------------------------------------------------------------------------------------------- | +| `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_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 | diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml new file mode 100644 index 0000000000000..0f889914abf76 --- /dev/null +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -0,0 +1,35 @@ +/**: + 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 + + # The newton line search maximum step length + step_size: 0.1 + + # The ND voxel grid resolution + resolution: 2.0 + + # The number of iterations required to calculate alignment + max_iterations: 30 + + # Threshold for deciding whetherto trust the estimation result + converged_param_transform_probability: 3.0 + + # neighborhood search method in OMP + # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1 + omp_neighborhood_search_method: 0 + + # Number of threads used for parallel computing + omp_num_threads: 4 diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/debug.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/debug.hpp new file mode 100644 index 0000000000000..4e63f7fb7f6fb --- /dev/null +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/debug.hpp @@ -0,0 +1,28 @@ +// 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_SCAN_MATCHER__DEBUG_HPP_ +#define NDT_SCAN_MATCHER__DEBUG_HPP_ + +#include "ndt_scan_matcher/particle.hpp" + +#include + +#include + +visualization_msgs::msg::MarkerArray makeDebugMarkers( + const builtin_interfaces::msg::Time & stamp, const std::string & map_frame_, + const geometry_msgs::msg::Vector3 & scale, const Particle & particle, const size_t i); + +#endif // NDT_SCAN_MATCHER__DEBUG_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/matrix_type.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/matrix_type.hpp new file mode 100644 index 0000000000000..61084bcf0d4cb --- /dev/null +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/matrix_type.hpp @@ -0,0 +1,23 @@ +// Copyright 2021 TierIV +// +// 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_SCAN_MATCHER__MATRIX_TYPE_HPP__ +#define NDT_SCAN_MATCHER__MATRIX_TYPE_HPP__ + +#include + +using Matrix6d = Eigen::Matrix; +using RowMatrixXd = Eigen::Matrix; + +#endif // NDT_SCAN_MATCHER__MATRIX_TYPE_HPP__ 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 new file mode 100644 index 0000000000000..9c20e9989208a --- /dev/null +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -0,0 +1,169 @@ +// 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_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ +#define NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ + +#define FMT_HEADER_ONLY + +#include "ndt_scan_matcher/particle.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +enum class NDTImplementType { PCL_GENERIC = 0, PCL_MODIFIED = 1, OMP = 2 }; + +template +std::shared_ptr> getNDT( + const NDTImplementType & ndt_mode) +{ + 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); +} + +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; + }; + +public: + NDTScanMatcher(); + +private: + void serviceNDTAlign( + const autoware_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, + autoware_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res); + + void callbackMapPoints(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr); + void callbackSensorPoints(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr); + void callbackInitialPose( + geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr); + + geometry_msgs::msg::PoseWithCovarianceStamped alignUsingMonteCarlo( + const std::shared_ptr> & ndt_ptr, + const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov); + + void updateTransforms(); + + void publishTF( + const std::string & child_frame_id, const geometry_msgs::msg::PoseStamped & pose_msg); + bool getTransform( + const std::string & target_frame, const std::string & source_frame, + const geometry_msgs::msg::TransformStamped::SharedPtr & transform_stamped_ptr); + + void timerDiagnostic(); + + rclcpp::Subscription::SharedPtr initial_pose_sub_; + rclcpp::Subscription::SharedPtr map_points_sub_; + rclcpp::Subscription::SharedPtr sensor_points_sub_; + + rclcpp::Publisher::SharedPtr sensor_aligned_pose_pub_; + rclcpp::Publisher::SharedPtr ndt_pose_pub_; + rclcpp::Publisher::SharedPtr + ndt_pose_with_covariance_pub_; + rclcpp::Publisher::SharedPtr + initial_pose_with_covariance_pub_; + rclcpp::Publisher::SharedPtr exe_time_pub_; + rclcpp::Publisher::SharedPtr transform_probability_pub_; + rclcpp::Publisher::SharedPtr iteration_num_pub_; + rclcpp::Publisher::SharedPtr + initial_to_result_distance_pub_; + rclcpp::Publisher::SharedPtr + initial_to_result_distance_old_pub_; + rclcpp::Publisher::SharedPtr + initial_to_result_distance_new_pub_; + rclcpp::Publisher::SharedPtr ndt_marker_pub_; + rclcpp::Publisher::SharedPtr + ndt_monte_carlo_initial_pose_marker_pub_; + rclcpp::Publisher::SharedPtr diagnostics_pub_; + + rclcpp::Service::SharedPtr service_; + + tf2_ros::Buffer tf2_buffer_; + tf2_ros::TransformListener tf2_listener_; + tf2_ros::TransformBroadcaster tf2_broadcaster_; + + NDTImplementType ndt_implement_type_; + std::shared_ptr> ndt_ptr_; + + Eigen::Matrix4f base_to_sensor_matrix_; + std::string base_frame_; + std::string ndt_base_frame_; + std::string map_frame_; + double converged_param_transform_probability_; + float inversion_vector_threshold_; + float oscillation_threshold_; + + std::deque + initial_pose_msg_ptr_array_; + std::mutex ndt_map_mtx_; + + OMPParams omp_params_; + + std::thread diagnostic_thread_; + std::map key_value_stdmap_; +}; + +#endif // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/particle.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/particle.hpp new file mode 100644 index 0000000000000..f9dc42f76a3d6 --- /dev/null +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/particle.hpp @@ -0,0 +1,34 @@ +// 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_SCAN_MATCHER__PARTICLE_HPP_ +#define NDT_SCAN_MATCHER__PARTICLE_HPP_ + +#include + +struct Particle +{ + Particle( + const geometry_msgs::msg::Pose & a_initial_pose, const geometry_msgs::msg::Pose & a_result_pose, + const double a_score, const int a_iteration) + : initial_pose(a_initial_pose), result_pose(a_result_pose), score(a_score), iteration(a_iteration) + { + } + geometry_msgs::msg::Pose initial_pose; + geometry_msgs::msg::Pose result_pose; + double score; + int iteration; +}; + +#endif // NDT_SCAN_MATCHER__DEBUG_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp new file mode 100644 index 0000000000000..9410a54a44eef --- /dev/null +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp @@ -0,0 +1,79 @@ +// 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_SCAN_MATCHER__UTIL_FUNC_HPP_ +#define NDT_SCAN_MATCHER__UTIL_FUNC_HPP_ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +// ref by http://takacity.blog.fc2.com/blog-entry-69.html +std_msgs::msg::ColorRGBA ExchangeColorCrc(double x); + +double calcDiffForRadian(const double lhs_rad, const double rhs_rad); + +// x: roll, y: pitch, z: yaw +geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Pose & pose); +geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::PoseStamped & pose); +geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); + +geometry_msgs::msg::Twist calcTwist( + const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b); + +void getNearestTimeStampPose( + const std::deque & + pose_cov_msg_ptr_array, + const rclcpp::Time & time_stamp, + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & output_old_pose_cov_msg_ptr, + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & output_new_pose_cov_msg_ptr); + +geometry_msgs::msg::PoseStamped interpolatePose( + const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b, + const rclcpp::Time & time_stamp); + +geometry_msgs::msg::PoseStamped interpolatePose( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp); + +void popOldPose( + std::deque & + pose_cov_msg_ptr_array, + const rclcpp::Time & time_stamp); + +Eigen::Affine3d fromRosPoseToEigen(const geometry_msgs::msg::Pose & ros_pose); + +std::vector createRandomPoseArray( + const geometry_msgs::msg::PoseWithCovarianceStamped & base_pose_with_cov, + const size_t particle_num); + +template +T transform(const T & input, const geometry_msgs::msg::TransformStamped & transform) +{ + T output; + tf2::doTransform(input, output, transform); + return output; +} + +#endif // NDT_SCAN_MATCHER__UTIL_FUNC_HPP_ diff --git a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml new file mode 100644 index 0000000000000..7c61137946531 --- /dev/null +++ b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml new file mode 100644 index 0000000000000..c4aba880a3fb3 --- /dev/null +++ b/localization/ndt_scan_matcher/package.xml @@ -0,0 +1,35 @@ + + + ndt_scan_matcher + 0.1.0 + The ndt_scan_matcher package + Yamato Ando + Apache License 2.0 + ament_cmake_auto + + autoware_debug_msgs + autoware_localization_msgs + autoware_utils + diagnostic_msgs + geometry_msgs + nav_msgs + ndt + ndt_omp + ndt_pcl_modified + pcl_conversions + rclcpp + sensor_msgs + std_msgs + tf2 + tf2_geometry_msgs + tf2_ros + tf2_sensor_msgs + visualization_msgs + + ament_cmake_cppcheck + ament_lint_auto + + + ament_cmake + + diff --git a/localization/ndt_scan_matcher/src/debug.cpp b/localization/ndt_scan_matcher/src/debug.cpp new file mode 100644 index 0000000000000..74dd89e76b4ea --- /dev/null +++ b/localization/ndt_scan_matcher/src/debug.cpp @@ -0,0 +1,66 @@ +// 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_scan_matcher/debug.hpp" + +#include "ndt_scan_matcher/util_func.hpp" + +visualization_msgs::msg::MarkerArray makeDebugMarkers( + const builtin_interfaces::msg::Time & stamp, const std::string & map_frame_, + const geometry_msgs::msg::Vector3 & scale, const Particle & particle, const size_t i) +{ + // TODO(Tier IV): getNumSubscribers + // TODO(Tier IV): clear old object + visualization_msgs::msg::MarkerArray marker_array; + + visualization_msgs::msg::Marker marker; + marker.header.stamp = stamp; + marker.header.frame_id = map_frame_; + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale = scale; + marker.id = i; + + marker.ns = "initial_pose_transform_probability_color_marker"; + marker.pose = particle.initial_pose; + marker.color = ExchangeColorCrc(particle.score / 4.5); + marker_array.markers.push_back(marker); + + marker.ns = "initial_pose_iteration_color_marker"; + marker.pose = particle.initial_pose; + marker.color = ExchangeColorCrc((1.0 * particle.iteration) / 30.0); + marker_array.markers.push_back(marker); + + marker.ns = "initial_pose_index_color_marker"; + marker.pose = particle.initial_pose; + marker.color = ExchangeColorCrc((1.0 * i) / 100); + marker_array.markers.push_back(marker); + + marker.ns = "result_pose_transform_probability_color_marker"; + marker.pose = particle.result_pose; + marker.color = ExchangeColorCrc(particle.score / 4.5); + marker_array.markers.push_back(marker); + + marker.ns = "result_pose_iteration_color_marker"; + marker.pose = particle.result_pose; + marker.color = ExchangeColorCrc((1.0 * particle.iteration) / 30.0); + marker_array.markers.push_back(marker); + + marker.ns = "result_pose_index_color_marker"; + marker.pose = particle.result_pose; + marker.color = ExchangeColorCrc((1.0 * i) / 100); + marker_array.markers.push_back(marker); + + return marker_array; +} diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp new file mode 100644 index 0000000000000..1a7600d3f15c3 --- /dev/null +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -0,0 +1,668 @@ +// 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_scan_matcher/ndt_scan_matcher_core.hpp" + +#include "ndt_scan_matcher/debug.hpp" +#include "ndt_scan_matcher/matrix_type.hpp" +#include "ndt_scan_matcher/particle.hpp" +#include "ndt_scan_matcher/util_func.hpp" + +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include + +autoware_debug_msgs::msg::Float32Stamped makeFloat32Stamped( + const builtin_interfaces::msg::Time & stamp, const float data) +{ + using T = autoware_debug_msgs::msg::Float32Stamped; + return autoware_debug_msgs::build().stamp(stamp).data(data); +} + +autoware_debug_msgs::msg::Int32Stamped makeInt32Stamped( + const builtin_interfaces::msg::Time & stamp, const int32_t data) +{ + using T = autoware_debug_msgs::msg::Int32Stamped; + return autoware_debug_msgs::build().stamp(stamp).data(data); +} + +geometry_msgs::msg::TransformStamped identityTransformStamped( + const builtin_interfaces::msg::Time & timestamp, const std::string & header_frame_id, + const std::string & child_frame_id) +{ + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = timestamp; + transform.header.frame_id = header_frame_id; + transform.child_frame_id = child_frame_id; + transform.transform.rotation = autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); + transform.transform.translation = autoware_utils::createTranslation(0.0, 0.0, 0.0); + return transform; +} + +double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) +{ + return std::sqrt( + std::pow(p1.x - p2.x, 2.0) + std::pow(p1.y - p2.y, 2.0) + std::pow(p1.z - p2.z, 2.0)); +} + +bool isLocalOptimalSolutionOscillation( + const std::vector & result_pose_matrix_array, const float oscillation_threshold, + const float inversion_vector_threshold) +{ + bool prev_oscillation = false; + int oscillation_cnt = 0; + for (size_t i = 2; i < result_pose_matrix_array.size(); ++i) { + const Eigen::Vector3f current_pose = result_pose_matrix_array.at(i).block(0, 3, 3, 1); + const Eigen::Vector3f prev_pose = result_pose_matrix_array.at(i - 1).block(0, 3, 3, 1); + const Eigen::Vector3f prev_prev_pose = result_pose_matrix_array.at(i - 2).block(0, 3, 3, 1); + const auto current_vec = (current_pose - prev_pose).normalized(); + const auto prev_vec = (prev_pose - prev_prev_pose).normalized(); + const bool oscillation = prev_vec.dot(current_vec) < inversion_vector_threshold; + if (prev_oscillation && oscillation) { + if (oscillation_cnt > oscillation_threshold) { + return true; + } + ++oscillation_cnt; + } else { + oscillation_cnt = 0; + } + prev_oscillation = oscillation; + } + return false; +} + +NDTScanMatcher::NDTScanMatcher() +: Node("ndt_scan_matcher"), + tf2_buffer_(this->get_clock()), + tf2_listener_(tf2_buffer_), + tf2_broadcaster_(*this), + ndt_implement_type_(NDTImplementType::PCL_GENERIC), + base_frame_("base_link"), + ndt_base_frame_("ndt_base_link"), + map_frame_("map"), + converged_param_transform_probability_(4.5), + inversion_vector_threshold_(-0.9), + oscillation_threshold_(10) +{ + key_value_stdmap_["state"] = "Initializing"; + + 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_ = getNDT(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); + + base_frame_ = this->declare_parameter("base_frame", base_frame_); + RCLCPP_INFO(get_logger(), "base_frame_id: %s", base_frame_.c_str()); + + 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); + RCLCPP_INFO( + get_logger(), "trans_epsilon: %lf, step_size: %lf, resolution: %lf, max_iterations: %d", + trans_epsilon, step_size, resolution, max_iterations); + + converged_param_transform_probability_ = this->declare_parameter( + "converged_param_transform_probability", converged_param_transform_probability_); + + initial_pose_sub_ = this->create_subscription( + "ekf_pose_with_covariance", 100, + std::bind(&NDTScanMatcher::callbackInitialPose, this, std::placeholders::_1)); + map_points_sub_ = this->create_subscription( + "pointcloud_map", rclcpp::QoS{1}.transient_local(), + std::bind(&NDTScanMatcher::callbackMapPoints, this, std::placeholders::_1)); + sensor_points_sub_ = this->create_subscription( + "points_raw", rclcpp::SensorDataQoS().keep_last(points_queue_size), + std::bind(&NDTScanMatcher::callbackSensorPoints, this, std::placeholders::_1)); + + sensor_aligned_pose_pub_ = + this->create_publisher("points_aligned", 10); + ndt_pose_pub_ = this->create_publisher("ndt_pose", 10); + ndt_pose_with_covariance_pub_ = + this->create_publisher( + "ndt_pose_with_covariance", 10); + initial_pose_with_covariance_pub_ = + this->create_publisher( + "initial_pose_with_covariance", 10); + exe_time_pub_ = + this->create_publisher("exe_time_ms", 10); + transform_probability_pub_ = + this->create_publisher("transform_probability", 10); + iteration_num_pub_ = + this->create_publisher("iteration_num", 10); + initial_to_result_distance_pub_ = + this->create_publisher( + "initial_to_result_distance", 10); + initial_to_result_distance_old_pub_ = + this->create_publisher( + "initial_to_result_distance_old", 10); + initial_to_result_distance_new_pub_ = + this->create_publisher( + "initial_to_result_distance_new", 10); + ndt_marker_pub_ = this->create_publisher("ndt_marker", 10); + ndt_monte_carlo_initial_pose_marker_pub_ = + this->create_publisher( + "monte_carlo_initial_pose_marker", 10); + diagnostics_pub_ = + this->create_publisher("/diagnostics", 10); + + service_ = this->create_service( + "ndt_align_srv", + std::bind( + &NDTScanMatcher::serviceNDTAlign, this, std::placeholders::_1, std::placeholders::_2)); + + diagnostic_thread_ = std::thread(&NDTScanMatcher::timerDiagnostic, this); + diagnostic_thread_.detach(); +} + +void NDTScanMatcher::timerDiagnostic() +{ + rclcpp::Rate rate(100); + while (rclcpp::ok()) { + diagnostic_msgs::msg::DiagnosticStatus diag_status_msg; + diag_status_msg.name = "ndt_scan_matcher"; + diag_status_msg.hardware_id = ""; + + for (const auto & key_value : key_value_stdmap_) { + diagnostic_msgs::msg::KeyValue key_value_msg; + key_value_msg.key = key_value.first; + key_value_msg.value = key_value.second; + diag_status_msg.values.push_back(key_value_msg); + } + + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diag_status_msg.message = ""; + if (key_value_stdmap_.count("state") && key_value_stdmap_["state"] == "Initializing") { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diag_status_msg.message += "Initializing State. "; + } + if ( + key_value_stdmap_.count("skipping_publish_num") && + std::stoi(key_value_stdmap_["skipping_publish_num"]) > 1) { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diag_status_msg.message += "skipping_publish_num > 1. "; + } + if ( + key_value_stdmap_.count("skipping_publish_num") && + std::stoi(key_value_stdmap_["skipping_publish_num"]) >= 5) { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + diag_status_msg.message += "skipping_publish_num exceed limit. "; + } + // Ignore local optimal solution + if ( + key_value_stdmap_.count("is_local_optimal_solution_oscillation") && + std::stoi(key_value_stdmap_["is_local_optimal_solution_oscillation"])) { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diag_status_msg.message = "local optimal solution oscillation occurred"; + } + + diagnostic_msgs::msg::DiagnosticArray diag_msg; + diag_msg.header.stamp = this->now(); + diag_msg.status.push_back(diag_status_msg); + + diagnostics_pub_->publish(diag_msg); + + rate.sleep(); + } +} + +void NDTScanMatcher::serviceNDTAlign( + const autoware_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, + autoware_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res) +{ + // get TF from pose_frame to map_frame + auto TF_pose_to_map_ptr = std::make_shared(); + getTransform(map_frame_, req->pose_with_covariance.header.frame_id, TF_pose_to_map_ptr); + + // transform pose_frame to map_frame + const auto mapTF_initial_pose_msg = transform(req->pose_with_covariance, *TF_pose_to_map_ptr); + + if (ndt_ptr_->getInputTarget() == nullptr) { + res->success = false; + res->seq = req->seq; + RCLCPP_WARN(get_logger(), "No InputTarget"); + return; + } + + if (ndt_ptr_->getInputSource() == nullptr) { + res->success = false; + res->seq = req->seq; + RCLCPP_WARN(get_logger(), "No InputSource"); + return; + } + + // mutex Map + std::lock_guard lock(ndt_map_mtx_); + + key_value_stdmap_["state"] = "Aligning"; + res->pose_with_covariance = alignUsingMonteCarlo(ndt_ptr_, mapTF_initial_pose_msg); + key_value_stdmap_["state"] = "Sleeping"; + res->success = true; + res->seq = req->seq; + res->pose_with_covariance.pose.covariance = req->pose_with_covariance.pose.covariance; +} + +void NDTScanMatcher::callbackInitialPose( + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr initial_pose_msg_ptr) +{ + // if rosbag restart, clear buffer + if (!initial_pose_msg_ptr_array_.empty()) { + const builtin_interfaces::msg::Time & t_front = + initial_pose_msg_ptr_array_.front()->header.stamp; + const builtin_interfaces::msg::Time & t_msg = initial_pose_msg_ptr->header.stamp; + if (t_front.sec > t_msg.sec || (t_front.sec == t_msg.sec && t_front.nanosec > t_msg.nanosec)) { + initial_pose_msg_ptr_array_.clear(); + } + } + + if (initial_pose_msg_ptr->header.frame_id == map_frame_) { + initial_pose_msg_ptr_array_.push_back(initial_pose_msg_ptr); + } else { + // get TF from pose_frame to map_frame + auto TF_pose_to_map_ptr = std::make_shared(); + getTransform(map_frame_, initial_pose_msg_ptr->header.frame_id, TF_pose_to_map_ptr); + + // transform pose_frame to map_frame + auto mapTF_initial_pose_msg_ptr = + std::make_shared(); + // mapTF_initial_pose_msg_ptr->header.stamp = initial_pose_msg_ptr->header.stamp; + *mapTF_initial_pose_msg_ptr = transform(*initial_pose_msg_ptr, *TF_pose_to_map_ptr); + initial_pose_msg_ptr_array_.push_back(mapTF_initial_pose_msg_ptr); + } +} + +void NDTScanMatcher::callbackMapPoints( + 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_ = getNDT(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); + + boost::shared_ptr> map_points_ptr(new pcl::PointCloud); + pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr); + new_ndt_ptr_->setInputTarget(map_points_ptr); + // create Thread + // detach + auto output_cloud = std::make_shared>(); + new_ndt_ptr_->align(*output_cloud, Eigen::Matrix4f::Identity()); + + // swap + ndt_map_mtx_.lock(); + ndt_ptr_ = new_ndt_ptr_; + ndt_map_mtx_.unlock(); +} + +void NDTScanMatcher::callbackSensorPoints( + sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_sensorTF_msg_ptr) +{ + const auto exe_start_time = std::chrono::system_clock::now(); + // mutex Map + std::lock_guard lock(ndt_map_mtx_); + + const std::string & sensor_frame = sensor_points_sensorTF_msg_ptr->header.frame_id; + const rclcpp::Time sensor_ros_time = sensor_points_sensorTF_msg_ptr->header.stamp; + + boost::shared_ptr> sensor_points_sensorTF_ptr( + new pcl::PointCloud); + pcl::fromROSMsg(*sensor_points_sensorTF_msg_ptr, *sensor_points_sensorTF_ptr); + // get TF base to sensor + auto TF_base_to_sensor_ptr = std::make_shared(); + getTransform(base_frame_, sensor_frame, TF_base_to_sensor_ptr); + const Eigen::Affine3d base_to_sensor_affine = tf2::transformToEigen(*TF_base_to_sensor_ptr); + const Eigen::Matrix4f base_to_sensor_matrix = base_to_sensor_affine.matrix().cast(); + boost::shared_ptr> sensor_points_baselinkTF_ptr( + new pcl::PointCloud); + pcl::transformPointCloud( + *sensor_points_sensorTF_ptr, *sensor_points_baselinkTF_ptr, base_to_sensor_matrix); + ndt_ptr_->setInputSource(sensor_points_baselinkTF_ptr); + + // check + if (initial_pose_msg_ptr_array_.size() <= 1) { + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1, "No Pose!"); + return; + } + // searchNNPose using timestamp + auto initial_pose_old_msg_ptr = std::make_shared(); + auto initial_pose_new_msg_ptr = std::make_shared(); + getNearestTimeStampPose( + initial_pose_msg_ptr_array_, sensor_ros_time, initial_pose_old_msg_ptr, + initial_pose_new_msg_ptr); + popOldPose(initial_pose_msg_ptr_array_, sensor_ros_time); + // TODO(Tier IV): check pose_timestamp - sensor_ros_time + const auto initial_pose_msg = + interpolatePose(*initial_pose_old_msg_ptr, *initial_pose_new_msg_ptr, sensor_ros_time); + + geometry_msgs::msg::PoseWithCovarianceStamped initial_pose_cov_msg; + initial_pose_cov_msg.header = initial_pose_msg.header; + initial_pose_cov_msg.pose.pose = initial_pose_msg.pose; + + if (ndt_ptr_->getInputTarget() == nullptr) { + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1, "No MAP!"); + return; + } + // align + const Eigen::Affine3d initial_pose_affine = fromRosPoseToEigen(initial_pose_cov_msg.pose.pose); + const Eigen::Matrix4f initial_pose_matrix = initial_pose_affine.matrix().cast(); + + auto output_cloud = std::make_shared>(); + key_value_stdmap_["state"] = "Aligning"; + ndt_ptr_->align(*output_cloud, initial_pose_matrix); + key_value_stdmap_["state"] = "Sleeping"; + + const Eigen::Matrix4f result_pose_matrix = ndt_ptr_->getFinalTransformation(); + Eigen::Affine3d result_pose_affine; + result_pose_affine.matrix() = result_pose_matrix.cast(); + const geometry_msgs::msg::Pose result_pose_msg = tf2::toMsg(result_pose_affine); + + const std::vector result_pose_matrix_array = + ndt_ptr_->getFinalTransformationArray(); + std::vector result_pose_msg_array; + for (const auto & pose_matrix : result_pose_matrix_array) { + Eigen::Affine3d pose_affine; + pose_affine.matrix() = pose_matrix.cast(); + const geometry_msgs::msg::Pose pose_msg = tf2::toMsg(pose_affine); + result_pose_msg_array.push_back(pose_msg); + } + + const auto exe_end_time = std::chrono::system_clock::now(); + const double exe_time = + std::chrono::duration_cast(exe_end_time - exe_start_time).count() / + 1000.0; + + const float transform_probability = ndt_ptr_->getTransformationProbability(); + + const int iteration_num = ndt_ptr_->getFinalNumIteration(); + + /***************************************************************************** + The reason the add 2 to the ndt_ptr_->getMaximumIterations() is that there are bugs in + implementation of ndt. + 1. gradient descent method ends when the iteration is greater than max_iteration if it does not + converge (be careful it's 'greater than' instead of 'greater equal than'.) + https://github.com/tier4/autoware.iv/blob/2323e5baa0b680d43a9219f5fb3b7a11dd9edc82/localization/pose_estimator/ndt_scan_matcher/ndt_omp/include/ndt_omp/ndt_omp_impl.hpp#L212 + 2. iterate iteration count when end of gradient descent function. + https://github.com/tier4/autoware.iv/blob/2323e5baa0b680d43a9219f5fb3b7a11dd9edc82/localization/pose_estimator/ndt_scan_matcher/ndt_omp/include/ndt_omp/ndt_omp_impl.hpp#L217 + + These bugs are now resolved in original pcl implementation. + https://github.com/PointCloudLibrary/pcl/blob/424c1c6a0ca97d94ca63e5daff4b183a4db8aae4/registration/include/pcl/registration/impl/ndt.hpp#L73-L180 + *****************************************************************************/ + bool is_local_optimal_solution_oscillation = false; + if (iteration_num >= ndt_ptr_->getMaximumIterations() + 2) { + is_local_optimal_solution_oscillation = isLocalOptimalSolutionOscillation( + result_pose_matrix_array, oscillation_threshold_, inversion_vector_threshold_); + } + + bool is_converged = true; + static size_t skipping_publish_num = 0; + if ( + iteration_num >= ndt_ptr_->getMaximumIterations() + 2 || + transform_probability < converged_param_transform_probability_) { + is_converged = false; + ++skipping_publish_num; + RCLCPP_WARN(get_logger(), "Not Converged"); + } else { + skipping_publish_num = 0; + } + + // publish + geometry_msgs::msg::PoseStamped result_pose_stamped_msg; + result_pose_stamped_msg.header.stamp = sensor_ros_time; + result_pose_stamped_msg.header.frame_id = map_frame_; + result_pose_stamped_msg.pose = result_pose_msg; + + geometry_msgs::msg::PoseWithCovarianceStamped result_pose_with_cov_msg; + result_pose_with_cov_msg.header.stamp = sensor_ros_time; + result_pose_with_cov_msg.header.frame_id = map_frame_; + result_pose_with_cov_msg.pose.pose = result_pose_msg; + + // TODO(Tier IV): temporary value + Eigen::Map covariance(&result_pose_with_cov_msg.pose.covariance[0], 6, 6); + covariance(0, 0) = 0.025; + covariance(1, 1) = 0.025; + covariance(2, 2) = 0.025; + covariance(3, 3) = 0.000625; + covariance(4, 4) = 0.000625; + covariance(5, 5) = 0.000625; + + if (is_converged) { + ndt_pose_pub_->publish(result_pose_stamped_msg); + ndt_pose_with_covariance_pub_->publish(result_pose_with_cov_msg); + } + + publishTF(ndt_base_frame_, result_pose_stamped_msg); + + auto sensor_points_mapTF_ptr = std::make_shared>(); + pcl::transformPointCloud( + *sensor_points_baselinkTF_ptr, *sensor_points_mapTF_ptr, result_pose_matrix); + sensor_msgs::msg::PointCloud2 sensor_points_mapTF_msg; + pcl::toROSMsg(*sensor_points_mapTF_ptr, sensor_points_mapTF_msg); + sensor_points_mapTF_msg.header.stamp = sensor_ros_time; + sensor_points_mapTF_msg.header.frame_id = map_frame_; + sensor_aligned_pose_pub_->publish(sensor_points_mapTF_msg); + + initial_pose_with_covariance_pub_->publish(initial_pose_cov_msg); + + visualization_msgs::msg::MarkerArray marker_array; + visualization_msgs::msg::Marker marker; + marker.header.stamp = sensor_ros_time; + marker.header.frame_id = map_frame_; + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale = autoware_utils::createMarkerScale(0.3, 0.1, 0.1); + int i = 0; + marker.ns = "result_pose_matrix_array"; + marker.action = visualization_msgs::msg::Marker::ADD; + for (const auto & pose_msg : result_pose_msg_array) { + marker.id = i++; + marker.pose = pose_msg; + marker.color = ExchangeColorCrc((1.0 * i) / 15.0); + marker_array.markers.push_back(marker); + } + // TODO(Tier IV): delete old marker + for (; i < ndt_ptr_->getMaximumIterations() + 2;) { + marker.id = i++; + marker.pose = geometry_msgs::msg::Pose(); + marker.color = ExchangeColorCrc(0); + marker_array.markers.push_back(marker); + } + ndt_marker_pub_->publish(marker_array); + + exe_time_pub_->publish(makeFloat32Stamped(sensor_ros_time, exe_time)); + + transform_probability_pub_->publish(makeFloat32Stamped(sensor_ros_time, transform_probability)); + + iteration_num_pub_->publish(makeInt32Stamped(sensor_ros_time, iteration_num)); + + const float initial_to_result_distance = + norm(initial_pose_cov_msg.pose.pose.position, result_pose_with_cov_msg.pose.pose.position); + initial_to_result_distance_pub_->publish( + makeFloat32Stamped(sensor_ros_time, initial_to_result_distance)); + + const float initial_to_result_distance_old = + norm(initial_pose_old_msg_ptr->pose.pose.position, result_pose_with_cov_msg.pose.pose.position); + initial_to_result_distance_old_pub_->publish( + makeFloat32Stamped(sensor_ros_time, initial_to_result_distance_old)); + + const float initial_to_result_distance_new = + norm(initial_pose_new_msg_ptr->pose.pose.position, result_pose_with_cov_msg.pose.pose.position); + initial_to_result_distance_new_pub_->publish( + makeFloat32Stamped(sensor_ros_time, initial_to_result_distance_new)); + + key_value_stdmap_["transform_probability"] = std::to_string(transform_probability); + key_value_stdmap_["iteration_num"] = std::to_string(iteration_num); + key_value_stdmap_["skipping_publish_num"] = std::to_string(skipping_publish_num); + if (is_local_optimal_solution_oscillation) { + key_value_stdmap_["is_local_optimal_solution_oscillation"] = "1"; + } else { + key_value_stdmap_["is_local_optimal_solution_oscillation"] = "0"; + } +} + +geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::alignUsingMonteCarlo( + const std::shared_ptr> & ndt_ptr, + const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov) +{ + if (ndt_ptr->getInputTarget() == nullptr || ndt_ptr->getInputSource() == nullptr) { + RCLCPP_WARN(get_logger(), "No Map or Sensor PointCloud"); + return geometry_msgs::msg::PoseWithCovarianceStamped(); + } + + // generateParticle + const auto initial_poses = createRandomPoseArray(initial_pose_with_cov, 100); + + std::vector particle_array; + auto output_cloud = std::make_shared>(); + + for (unsigned int i = 0; i < initial_poses.size(); i++) { + const auto & initial_pose = initial_poses[i]; + + const Eigen::Affine3d initial_pose_affine = fromRosPoseToEigen(initial_pose); + const Eigen::Matrix4f initial_pose_matrix = initial_pose_affine.matrix().cast(); + + ndt_ptr->align(*output_cloud, initial_pose_matrix); + + const Eigen::Matrix4f result_pose_matrix = ndt_ptr->getFinalTransformation(); + Eigen::Affine3d result_pose_affine; + result_pose_affine.matrix() = result_pose_matrix.cast(); + const geometry_msgs::msg::Pose result_pose = tf2::toMsg(result_pose_affine); + + const auto transform_probability = ndt_ptr->getTransformationProbability(); + const auto num_iteration = ndt_ptr->getFinalNumIteration(); + + Particle particle(initial_pose, result_pose, transform_probability, num_iteration); + particle_array.push_back(particle); + const auto marker_array = makeDebugMarkers( + this->now(), map_frame_, autoware_utils::createMarkerScale(0.3, 0.1, 0.1), particle, i); + ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array); + + auto sensor_points_mapTF_ptr = std::make_shared>(); + const auto sensor_points_baselinkTF_ptr = ndt_ptr->getInputSource(); + pcl::transformPointCloud( + *sensor_points_baselinkTF_ptr, *sensor_points_mapTF_ptr, result_pose_matrix); + sensor_msgs::msg::PointCloud2 sensor_points_mapTF_msg; + pcl::toROSMsg(*sensor_points_mapTF_ptr, sensor_points_mapTF_msg); + sensor_points_mapTF_msg.header.stamp = initial_pose_with_cov.header.stamp; + sensor_points_mapTF_msg.header.frame_id = map_frame_; + sensor_aligned_pose_pub_->publish(sensor_points_mapTF_msg); + } + + auto best_particle_ptr = std::max_element( + std::begin(particle_array), std::end(particle_array), + [](const Particle & lhs, const Particle & rhs) { return lhs.score < rhs.score; }); + + geometry_msgs::msg::PoseWithCovarianceStamped result_pose_with_cov_msg; + result_pose_with_cov_msg.header.frame_id = map_frame_; + result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose; + // ndt_pose_with_covariance_pub_->publish(result_pose_with_cov_msg); + + return result_pose_with_cov_msg; +} + +void NDTScanMatcher::publishTF( + const std::string & child_frame_id, const geometry_msgs::msg::PoseStamped & pose_msg) +{ + tf2_broadcaster_.sendTransform(autoware_utils::pose2transform(pose_msg, child_frame_id)); +} + +bool NDTScanMatcher::getTransform( + const std::string & target_frame, const std::string & source_frame, + const geometry_msgs::msg::TransformStamped::SharedPtr & transform_stamped_ptr) +{ + const geometry_msgs::msg::TransformStamped identity = + identityTransformStamped(this->now(), target_frame, source_frame); + + if (target_frame == source_frame) { + *transform_stamped_ptr = identity; + return true; + } + + try { + *transform_stamped_ptr = + tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN(get_logger(), "%s", ex.what()); + RCLCPP_ERROR( + get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); + + *transform_stamped_ptr = identity; + return false; + } + return true; +} diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp new file mode 100644 index 0000000000000..97d41cead59c1 --- /dev/null +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp @@ -0,0 +1,25 @@ +// 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_scan_matcher/ndt_scan_matcher_core.hpp" + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/localization/ndt_scan_matcher/src/util_func.cpp b/localization/ndt_scan_matcher/src/util_func.cpp new file mode 100644 index 0000000000000..e6b76a312d40d --- /dev/null +++ b/localization/ndt_scan_matcher/src/util_func.cpp @@ -0,0 +1,257 @@ +// 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_scan_matcher/util_func.hpp" + +#include "ndt_scan_matcher/matrix_type.hpp" + +static std::random_device seed_gen; + +// ref by http://takacity.blog.fc2.com/blog-entry-69.html +std_msgs::msg::ColorRGBA ExchangeColorCrc(double x) +{ + std_msgs::msg::ColorRGBA color; + + x = std::max(x, 0.0); + x = std::min(x, 0.9999); + + if (x <= 0.25) { + color.b = 1.0; + color.g = std::sin(x * 2.0 * M_PI); + color.r = 0; + } else if (x > 0.25 && x <= 0.5) { + color.b = std::sin(x * 2 * M_PI); + color.g = 1.0; + color.r = 0; + } else if (x > 0.5 && x <= 0.75) { + color.b = 0; + color.g = 1.0; + color.r = -std::sin(x * 2.0 * M_PI); + } else { + color.b = 0; + color.g = -std::sin(x * 2.0 * M_PI); + color.r = 1.0; + } + color.a = 0.999; + return color; +} + +double calcDiffForRadian(const double lhs_rad, const double rhs_rad) +{ + double diff_rad = lhs_rad - rhs_rad; + if (diff_rad > M_PI) { + diff_rad = diff_rad - 2 * M_PI; + } else if (diff_rad < -M_PI) { + diff_rad = diff_rad + 2 * M_PI; + } + return diff_rad; +} + +Eigen::Map makeEigenCovariance(const std::array & covariance) +{ + return Eigen::Map(covariance.data(), 6, 6); +} + +// x: roll, y: pitch, z: yaw +geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Pose & pose) +{ + geometry_msgs::msg::Vector3 rpy; + tf2::Quaternion q(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); + tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z); + return rpy; +} + +geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::PoseStamped & pose) +{ + return getRPY(pose.pose); +} + +geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +{ + return getRPY(pose.pose.pose); +} + +geometry_msgs::msg::Twist calcTwist( + const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b) +{ + const rclcpp::Duration dt = rclcpp::Time(pose_b.header.stamp) - rclcpp::Time(pose_a.header.stamp); + const double dt_s = dt.seconds(); + + if (dt_s == 0) { + return geometry_msgs::msg::Twist(); + } + + const auto pose_a_rpy = getRPY(pose_a); + const auto pose_b_rpy = getRPY(pose_b); + + geometry_msgs::msg::Vector3 diff_xyz; + geometry_msgs::msg::Vector3 diff_rpy; + + diff_xyz.x = pose_b.pose.position.x - pose_a.pose.position.x; + diff_xyz.y = pose_b.pose.position.y - pose_a.pose.position.y; + diff_xyz.z = pose_b.pose.position.z - pose_a.pose.position.z; + diff_rpy.x = calcDiffForRadian(pose_b_rpy.x, pose_a_rpy.x); + diff_rpy.y = calcDiffForRadian(pose_b_rpy.y, pose_a_rpy.y); + diff_rpy.z = calcDiffForRadian(pose_b_rpy.z, pose_a_rpy.z); + + geometry_msgs::msg::Twist twist; + twist.linear.x = diff_xyz.x / dt_s; + twist.linear.y = diff_xyz.y / dt_s; + twist.linear.z = diff_xyz.z / dt_s; + twist.angular.x = diff_rpy.x / dt_s; + twist.angular.y = diff_rpy.y / dt_s; + twist.angular.z = diff_rpy.z / dt_s; + + return twist; +} + +void getNearestTimeStampPose( + const std::deque & + pose_cov_msg_ptr_array, + const rclcpp::Time & time_stamp, + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & output_old_pose_cov_msg_ptr, + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & output_new_pose_cov_msg_ptr) +{ + for (const auto & pose_cov_msg_ptr : pose_cov_msg_ptr_array) { + output_new_pose_cov_msg_ptr = + std::const_pointer_cast(pose_cov_msg_ptr); + const rclcpp::Time pose_time_stamp = output_new_pose_cov_msg_ptr->header.stamp; + if (pose_time_stamp > time_stamp) { + // TODO(Tier IV): refactor + if (pose_time_stamp.seconds() == 0.0) { + output_old_pose_cov_msg_ptr = output_new_pose_cov_msg_ptr; + } + break; + } + output_old_pose_cov_msg_ptr = output_new_pose_cov_msg_ptr; + } +} + +geometry_msgs::msg::PoseStamped interpolatePose( + const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b, + const rclcpp::Time & time_stamp) +{ + const rclcpp::Time pose_a_time_stamp = pose_a.header.stamp; + const rclcpp::Time pose_b_time_stamp = pose_b.header.stamp; + if ( + (pose_a_time_stamp.seconds() == 0.0) || (pose_b_time_stamp.seconds() == 0.0) || + (time_stamp.seconds() == 0.0)) { + return geometry_msgs::msg::PoseStamped(); + } + + const auto twist = calcTwist(pose_a, pose_b); + const double dt = (time_stamp - pose_a_time_stamp).seconds(); + + const auto pose_a_rpy = getRPY(pose_a); + + geometry_msgs::msg::Vector3 xyz; + geometry_msgs::msg::Vector3 rpy; + xyz.x = pose_a.pose.position.x + twist.linear.x * dt; + xyz.y = pose_a.pose.position.y + twist.linear.y * dt; + xyz.z = pose_a.pose.position.z + twist.linear.z * dt; + rpy.x = pose_a_rpy.x + twist.angular.x * dt; + rpy.y = pose_a_rpy.y + twist.angular.y * dt; + rpy.z = pose_a_rpy.z + twist.angular.z * dt; + + tf2::Quaternion tf_quaternion; + tf_quaternion.setRPY(rpy.x, rpy.y, rpy.z); + + geometry_msgs::msg::PoseStamped pose; + pose.header = pose_a.header; + pose.header.stamp = time_stamp; + pose.pose.position.x = xyz.x; + pose.pose.position.y = xyz.y; + pose.pose.position.z = xyz.z; + pose.pose.orientation = tf2::toMsg(tf_quaternion); + return pose; +} + +geometry_msgs::msg::PoseStamped interpolatePose( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp) +{ + geometry_msgs::msg::PoseStamped tmp_pose_a; + tmp_pose_a.header = pose_a.header; + tmp_pose_a.pose = pose_a.pose.pose; + + geometry_msgs::msg::PoseStamped tmp_pose_b; + tmp_pose_b.header = pose_b.header; + tmp_pose_b.pose = pose_b.pose.pose; + + return interpolatePose(tmp_pose_a, tmp_pose_b, time_stamp); +} + +void popOldPose( + std::deque & + pose_cov_msg_ptr_array, + const rclcpp::Time & time_stamp) +{ + while (!pose_cov_msg_ptr_array.empty()) { + if (rclcpp::Time(pose_cov_msg_ptr_array.front()->header.stamp) >= time_stamp) { + break; + } + pose_cov_msg_ptr_array.pop_front(); + } +} + +Eigen::Affine3d fromRosPoseToEigen(const geometry_msgs::msg::Pose & ros_pose) +{ + Eigen::Affine3d eigen_pose; + tf2::fromMsg(ros_pose, eigen_pose); + return eigen_pose; +} + +std::vector createRandomPoseArray( + const geometry_msgs::msg::PoseWithCovarianceStamped & base_pose_with_cov, + const size_t particle_num) +{ + std::default_random_engine engine(seed_gen()); + const Eigen::Map covariance = + makeEigenCovariance(base_pose_with_cov.pose.covariance); + + std::normal_distribution<> x_distribution(0.0, covariance(0, 0)); + std::normal_distribution<> y_distribution(0.0, covariance(1, 1)); + std::normal_distribution<> z_distribution(0.0, covariance(2, 2)); + std::normal_distribution<> roll_distribution(0.0, covariance(3, 3)); + std::normal_distribution<> pitch_distribution(0.0, covariance(4, 4)); + std::normal_distribution<> yaw_distribution(0.0, covariance(5, 5)); + + const auto base_rpy = getRPY(base_pose_with_cov); + + std::vector poses; + for (size_t i = 0; i < particle_num; ++i) { + geometry_msgs::msg::Vector3 xyz; + geometry_msgs::msg::Vector3 rpy; + + xyz.x = base_pose_with_cov.pose.pose.position.x + x_distribution(engine); + xyz.y = base_pose_with_cov.pose.pose.position.y + y_distribution(engine); + xyz.z = base_pose_with_cov.pose.pose.position.z + z_distribution(engine); + rpy.x = base_rpy.x + roll_distribution(engine); + rpy.y = base_rpy.y + pitch_distribution(engine); + rpy.z = base_rpy.z + yaw_distribution(engine); + + tf2::Quaternion tf_quaternion; + tf_quaternion.setRPY(rpy.x, rpy.y, rpy.z); + + geometry_msgs::msg::Pose pose; + pose.position.x = xyz.x; + pose.position.y = xyz.y; + pose.position.z = xyz.z; + pose.orientation = tf2::toMsg(tf_quaternion); + + poses.push_back(pose); + } + + return poses; +}