From a235363d3c52e3df8fbb2f8f490f311cc7deaa82 Mon Sep 17 00:00:00 2001
From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Date: Mon, 25 Apr 2022 14:47:43 +0900
Subject: [PATCH] fix: modify build error of ndt related packages in rolling
 (#793)

* fix(ndt_pcl_modified): modify build error in rolling

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* fix(ndt): modify build error in rolling

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* fix(ndt_scan_matcher): modify build error in rolling

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* ci(pre-commit): autofix

* remove unused find_package

* fix(ndt): remove memory.h

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Signed-off-by: guiping meng <alan.meng@autocore.ai>
---
 localization/ndt/CMakeLists.txt                    |  5 +++++
 localization/ndt/include/ndt/base.hpp              | 12 ++++++------
 localization/ndt/include/ndt/impl/omp.hpp          | 10 +++++-----
 localization/ndt/include/ndt/impl/pcl_generic.hpp  | 10 +++++-----
 localization/ndt/include/ndt/impl/pcl_modified.hpp | 10 +++++-----
 localization/ndt/include/ndt/omp.hpp               | 14 +++++++-------
 localization/ndt/include/ndt/pcl_generic.hpp       | 14 +++++++-------
 localization/ndt/include/ndt/pcl_modified.hpp      | 14 +++++++-------
 localization/ndt_pcl_modified/CMakeLists.txt       |  5 +++++
 .../include/ndt_pcl_modified/impl/ndt.hpp          | 10 ++++++++++
 .../include/ndt_pcl_modified/ndt.hpp               |  4 ++++
 .../ndt_scan_matcher/ndt_scan_matcher_core.hpp     | 11 +++++++++++
 .../include/ndt_scan_matcher/util_func.hpp         |  6 ++++++
 localization/ndt_scan_matcher/package.xml          |  1 +
 .../ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 11 +++++++----
 15 files changed, 91 insertions(+), 46 deletions(-)

diff --git a/localization/ndt/CMakeLists.txt b/localization/ndt/CMakeLists.txt
index ceaa9f6e9c0c1..e7542aee07cfb 100644
--- a/localization/ndt/CMakeLists.txt
+++ b/localization/ndt/CMakeLists.txt
@@ -46,6 +46,11 @@ target_include_directories(ndt
     $<INSTALL_INTERFACE:include>
 )
 
+target_include_directories(ndt
+  SYSTEM PUBLIC
+    ${PCL_INCLUDE_DIRS}
+)
+
 ament_target_dependencies(ndt PUBLIC ndt_omp ndt_pcl_modified)
 # Can't use ament_target_dependencies() here because it doesn't link PCL
 # properly, see ndt_omp for more information.
diff --git a/localization/ndt/include/ndt/base.hpp b/localization/ndt/include/ndt/base.hpp
index f06e5d057f273..b81e7f1c57f6f 100644
--- a/localization/ndt/include/ndt/base.hpp
+++ b/localization/ndt/include/ndt/base.hpp
@@ -15,7 +15,7 @@
 #ifndef NDT__BASE_HPP_
 #define NDT__BASE_HPP_
 
-#include <pcl/io/io.h>
+#include <pcl/common/io.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/point_types.h>
 #include <pcl/search/kdtree.h>
@@ -30,8 +30,8 @@ class NormalDistributionsTransformBase
   virtual ~NormalDistributionsTransformBase() = default;
 
   virtual void align(pcl::PointCloud<PointSource> & output, const Eigen::Matrix4f & guess) = 0;
-  virtual void setInputTarget(const boost::shared_ptr<pcl::PointCloud<PointTarget>> & map_ptr) = 0;
-  virtual void setInputSource(const boost::shared_ptr<pcl::PointCloud<PointSource>> & scan_ptr) = 0;
+  virtual void setInputTarget(const pcl::shared_ptr<pcl::PointCloud<PointTarget>> & map_ptr) = 0;
+  virtual void setInputSource(const pcl::shared_ptr<pcl::PointCloud<PointSource>> & scan_ptr) = 0;
 
   virtual void setMaximumIterations(int max_iter) = 0;
   virtual void setResolution(float res) = 0;
@@ -46,15 +46,15 @@ class NormalDistributionsTransformBase
   virtual double getTransformationProbability() const = 0;
   virtual double getNearestVoxelTransformationLikelihood() const = 0;
   virtual double getFitnessScore() = 0;
-  virtual boost::shared_ptr<const pcl::PointCloud<PointTarget>> getInputTarget() const = 0;
-  virtual boost::shared_ptr<const pcl::PointCloud<PointSource>> getInputSource() const = 0;
+  virtual pcl::shared_ptr<const pcl::PointCloud<PointTarget>> getInputTarget() const = 0;
+  virtual pcl::shared_ptr<const pcl::PointCloud<PointSource>> getInputSource() const = 0;
   virtual Eigen::Matrix4f getFinalTransformation() const = 0;
   virtual std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
   getFinalTransformationArray() const = 0;
 
   virtual Eigen::Matrix<double, 6, 6> getHessian() const = 0;
 
-  virtual boost::shared_ptr<pcl::search::KdTree<PointTarget>> getSearchMethodTarget() const = 0;
+  virtual pcl::shared_ptr<pcl::search::KdTree<PointTarget>> getSearchMethodTarget() const = 0;
 
   virtual double calculateTransformationProbability(
     const pcl::PointCloud<PointSource> & trans_cloud) const = 0;
diff --git a/localization/ndt/include/ndt/impl/omp.hpp b/localization/ndt/include/ndt/impl/omp.hpp
index 1308ec97d479f..353439332c76f 100644
--- a/localization/ndt/include/ndt/impl/omp.hpp
+++ b/localization/ndt/include/ndt/impl/omp.hpp
@@ -34,14 +34,14 @@ void NormalDistributionsTransformOMP<PointSource, PointTarget>::align(
 
 template <class PointSource, class PointTarget>
 void NormalDistributionsTransformOMP<PointSource, PointTarget>::setInputTarget(
-  const boost::shared_ptr<pcl::PointCloud<PointTarget>> & map_ptr)
+  const pcl::shared_ptr<pcl::PointCloud<PointTarget>> & map_ptr)
 {
   ndt_ptr_->setInputTarget(map_ptr);
 }
 
 template <class PointSource, class PointTarget>
 void NormalDistributionsTransformOMP<PointSource, PointTarget>::setInputSource(
-  const boost::shared_ptr<pcl::PointCloud<PointSource>> & scan_ptr)
+  const pcl::shared_ptr<pcl::PointCloud<PointSource>> & scan_ptr)
 {
   ndt_ptr_->setInputSource(scan_ptr);
 }
@@ -122,14 +122,14 @@ double NormalDistributionsTransformOMP<PointSource, PointTarget>::getFitnessScor
 }
 
 template <class PointSource, class PointTarget>
-boost::shared_ptr<const pcl::PointCloud<PointTarget>>
+pcl::shared_ptr<const pcl::PointCloud<PointTarget>>
 NormalDistributionsTransformOMP<PointSource, PointTarget>::getInputTarget() const
 {
   return ndt_ptr_->getInputTarget();
 }
 
 template <class PointSource, class PointTarget>
-boost::shared_ptr<const pcl::PointCloud<PointSource>>
+pcl::shared_ptr<const pcl::PointCloud<PointSource>>
 NormalDistributionsTransformOMP<PointSource, PointTarget>::getInputSource() const
 {
   return ndt_ptr_->getInputSource();
@@ -158,7 +158,7 @@ Eigen::Matrix<double, 6, 6> NormalDistributionsTransformOMP<PointSource, PointTa
 }
 
 template <class PointSource, class PointTarget>
-boost::shared_ptr<pcl::search::KdTree<PointTarget>>
+pcl::shared_ptr<pcl::search::KdTree<PointTarget>>
 NormalDistributionsTransformOMP<PointSource, PointTarget>::getSearchMethodTarget() const
 {
   return ndt_ptr_->getSearchMethodTarget();
diff --git a/localization/ndt/include/ndt/impl/pcl_generic.hpp b/localization/ndt/include/ndt/impl/pcl_generic.hpp
index d05c946a459a4..329093cd28589 100644
--- a/localization/ndt/include/ndt/impl/pcl_generic.hpp
+++ b/localization/ndt/include/ndt/impl/pcl_generic.hpp
@@ -35,14 +35,14 @@ void NormalDistributionsTransformPCLGeneric<PointSource, PointTarget>::align(
 
 template <class PointSource, class PointTarget>
 void NormalDistributionsTransformPCLGeneric<PointSource, PointTarget>::setInputTarget(
-  const boost::shared_ptr<pcl::PointCloud<PointTarget>> & map_ptr)
+  const pcl::shared_ptr<pcl::PointCloud<PointTarget>> & map_ptr)
 {
   ndt_ptr_->setInputTarget(map_ptr);
 }
 
 template <class PointSource, class PointTarget>
 void NormalDistributionsTransformPCLGeneric<PointSource, PointTarget>::setInputSource(
-  const boost::shared_ptr<pcl::PointCloud<PointSource>> & scan_ptr)
+  const pcl::shared_ptr<pcl::PointCloud<PointSource>> & scan_ptr)
 {
   ndt_ptr_->setInputSource(scan_ptr);
 }
@@ -125,14 +125,14 @@ double NormalDistributionsTransformPCLGeneric<PointSource, PointTarget>::getFitn
 }
 
 template <class PointSource, class PointTarget>
-boost::shared_ptr<const pcl::PointCloud<PointTarget>>
+pcl::shared_ptr<const pcl::PointCloud<PointTarget>>
 NormalDistributionsTransformPCLGeneric<PointSource, PointTarget>::getInputTarget() const
 {
   return ndt_ptr_->getInputTarget();
 }
 
 template <class PointSource, class PointTarget>
-boost::shared_ptr<const pcl::PointCloud<PointSource>>
+pcl::shared_ptr<const pcl::PointCloud<PointSource>>
 NormalDistributionsTransformPCLGeneric<PointSource, PointTarget>::getInputSource() const
 {
   return ndt_ptr_->getInputSource();
@@ -162,7 +162,7 @@ NormalDistributionsTransformPCLGeneric<PointSource, PointTarget>::getHessian() c
 }
 
 template <class PointSource, class PointTarget>
-boost::shared_ptr<pcl::search::KdTree<PointTarget>>
+pcl::shared_ptr<pcl::search::KdTree<PointTarget>>
 NormalDistributionsTransformPCLGeneric<PointSource, PointTarget>::getSearchMethodTarget() const
 {
   return ndt_ptr_->getSearchMethodTarget();
diff --git a/localization/ndt/include/ndt/impl/pcl_modified.hpp b/localization/ndt/include/ndt/impl/pcl_modified.hpp
index 4715b85ffb4d4..f6c359fcb7158 100644
--- a/localization/ndt/include/ndt/impl/pcl_modified.hpp
+++ b/localization/ndt/include/ndt/impl/pcl_modified.hpp
@@ -35,14 +35,14 @@ void NormalDistributionsTransformPCLModified<PointSource, PointTarget>::align(
 
 template <class PointSource, class PointTarget>
 void NormalDistributionsTransformPCLModified<PointSource, PointTarget>::setInputTarget(
-  const boost::shared_ptr<pcl::PointCloud<PointTarget>> & map_ptr)
+  const pcl::shared_ptr<pcl::PointCloud<PointTarget>> & map_ptr)
 {
   ndt_ptr_->setInputTarget(map_ptr);
 }
 
 template <class PointSource, class PointTarget>
 void NormalDistributionsTransformPCLModified<PointSource, PointTarget>::setInputSource(
-  const boost::shared_ptr<pcl::PointCloud<PointSource>> & scan_ptr)
+  const pcl::shared_ptr<pcl::PointCloud<PointSource>> & scan_ptr)
 {
   ndt_ptr_->setInputSource(scan_ptr);
 }
@@ -126,14 +126,14 @@ double NormalDistributionsTransformPCLModified<PointSource, PointTarget>::getFit
 }
 
 template <class PointSource, class PointTarget>
-boost::shared_ptr<const pcl::PointCloud<PointTarget>>
+pcl::shared_ptr<const pcl::PointCloud<PointTarget>>
 NormalDistributionsTransformPCLModified<PointSource, PointTarget>::getInputTarget() const
 {
   return ndt_ptr_->getInputTarget();
 }
 
 template <class PointSource, class PointTarget>
-boost::shared_ptr<const pcl::PointCloud<PointSource>>
+pcl::shared_ptr<const pcl::PointCloud<PointSource>>
 NormalDistributionsTransformPCLModified<PointSource, PointTarget>::getInputSource() const
 {
   return ndt_ptr_->getInputSource();
@@ -162,7 +162,7 @@ NormalDistributionsTransformPCLModified<PointSource, PointTarget>::getHessian()
 }
 
 template <class PointSource, class PointTarget>
-boost::shared_ptr<pcl::search::KdTree<PointTarget>>
+pcl::shared_ptr<pcl::search::KdTree<PointTarget>>
 NormalDistributionsTransformPCLModified<PointSource, PointTarget>::getSearchMethodTarget() const
 {
   return ndt_ptr_->getSearchMethodTarget();
diff --git a/localization/ndt/include/ndt/omp.hpp b/localization/ndt/include/ndt/omp.hpp
index 95c5f1bd4b2dc..9c323097cc8b6 100644
--- a/localization/ndt/include/ndt/omp.hpp
+++ b/localization/ndt/include/ndt/omp.hpp
@@ -17,7 +17,7 @@
 
 #include "ndt/base.hpp"
 
-#include <pcl/io/io.h>
+#include <pcl/common/io.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/point_types.h>
 #include <pclomp/ndt_omp.h>
@@ -33,8 +33,8 @@ class NormalDistributionsTransformOMP
   ~NormalDistributionsTransformOMP() = default;
 
   void align(pcl::PointCloud<PointSource> & output, const Eigen::Matrix4f & guess) override;
-  void setInputTarget(const boost::shared_ptr<pcl::PointCloud<PointTarget>> & map_ptr) override;
-  void setInputSource(const boost::shared_ptr<pcl::PointCloud<PointSource>> & scan_ptr) override;
+  void setInputTarget(const pcl::shared_ptr<pcl::PointCloud<PointTarget>> & map_ptr) override;
+  void setInputSource(const pcl::shared_ptr<pcl::PointCloud<PointSource>> & scan_ptr) override;
 
   void setMaximumIterations(int max_iter) override;
   void setResolution(float res) override;
@@ -49,15 +49,15 @@ class NormalDistributionsTransformOMP
   double getTransformationProbability() const override;
   double getNearestVoxelTransformationLikelihood() const override;
   double getFitnessScore() override;
-  boost::shared_ptr<const pcl::PointCloud<PointTarget>> getInputTarget() const override;
-  boost::shared_ptr<const pcl::PointCloud<PointSource>> getInputSource() const override;
+  pcl::shared_ptr<const pcl::PointCloud<PointTarget>> getInputTarget() const override;
+  pcl::shared_ptr<const pcl::PointCloud<PointSource>> getInputSource() const override;
   Eigen::Matrix4f getFinalTransformation() const override;
   std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
   getFinalTransformationArray() const override;
 
   Eigen::Matrix<double, 6, 6> getHessian() const override;
 
-  boost::shared_ptr<pcl::search::KdTree<PointTarget>> getSearchMethodTarget() const override;
+  pcl::shared_ptr<pcl::search::KdTree<PointTarget>> getSearchMethodTarget() const override;
 
   double calculateTransformationProbability(
     const pcl::PointCloud<PointSource> & trans_cloud) const override;
@@ -72,7 +72,7 @@ class NormalDistributionsTransformOMP
   pclomp::NeighborSearchMethod getNeighborhoodSearchMethod() const;
 
 private:
-  boost::shared_ptr<pclomp::NormalDistributionsTransform<PointSource, PointTarget>> ndt_ptr_;
+  pcl::shared_ptr<pclomp::NormalDistributionsTransform<PointSource, PointTarget>> ndt_ptr_;
 };
 
 #include "ndt/impl/omp.hpp"
diff --git a/localization/ndt/include/ndt/pcl_generic.hpp b/localization/ndt/include/ndt/pcl_generic.hpp
index 8ddb7dcc34006..63928510d54b2 100644
--- a/localization/ndt/include/ndt/pcl_generic.hpp
+++ b/localization/ndt/include/ndt/pcl_generic.hpp
@@ -17,7 +17,7 @@
 
 #include "ndt/base.hpp"
 
-#include <pcl/io/io.h>
+#include <pcl/common/io.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/point_types.h>
 #include <pcl/registration/ndt.h>
@@ -33,8 +33,8 @@ class NormalDistributionsTransformPCLGeneric
   ~NormalDistributionsTransformPCLGeneric() = default;
 
   void align(pcl::PointCloud<PointSource> & output, const Eigen::Matrix4f & guess) override;
-  void setInputTarget(const boost::shared_ptr<pcl::PointCloud<PointTarget>> & map_ptr) override;
-  void setInputSource(const boost::shared_ptr<pcl::PointCloud<PointSource>> & scan_ptr) override;
+  void setInputTarget(const pcl::shared_ptr<pcl::PointCloud<PointTarget>> & map_ptr) override;
+  void setInputSource(const pcl::shared_ptr<pcl::PointCloud<PointSource>> & scan_ptr) override;
 
   void setMaximumIterations(int max_iter) override;
   void setResolution(float res) override;
@@ -49,15 +49,15 @@ class NormalDistributionsTransformPCLGeneric
   double getTransformationProbability() const override;
   double getNearestVoxelTransformationLikelihood() const override;
   double getFitnessScore() override;
-  boost::shared_ptr<const pcl::PointCloud<PointTarget>> getInputTarget() const override;
-  boost::shared_ptr<const pcl::PointCloud<PointSource>> getInputSource() const override;
+  pcl::shared_ptr<const pcl::PointCloud<PointTarget>> getInputTarget() const override;
+  pcl::shared_ptr<const pcl::PointCloud<PointSource>> getInputSource() const override;
   Eigen::Matrix4f getFinalTransformation() const override;
   std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
   getFinalTransformationArray() const override;
 
   Eigen::Matrix<double, 6, 6> getHessian() const override;
 
-  boost::shared_ptr<pcl::search::KdTree<PointTarget>> getSearchMethodTarget() const override;
+  pcl::shared_ptr<pcl::search::KdTree<PointTarget>> getSearchMethodTarget() const override;
 
   double calculateTransformationProbability(
     const pcl::PointCloud<PointSource> & trans_cloud) const override;
@@ -65,7 +65,7 @@ class NormalDistributionsTransformPCLGeneric
     const pcl::PointCloud<PointSource> & trans_cloud) const override;
 
 private:
-  boost::shared_ptr<pcl::NormalDistributionsTransform<PointSource, PointTarget>> ndt_ptr_;
+  pcl::shared_ptr<pcl::NormalDistributionsTransform<PointSource, PointTarget>> ndt_ptr_;
 };
 
 #include "ndt/impl/pcl_generic.hpp"
diff --git a/localization/ndt/include/ndt/pcl_modified.hpp b/localization/ndt/include/ndt/pcl_modified.hpp
index 34f50cd7553ba..df993566481e5 100644
--- a/localization/ndt/include/ndt/pcl_modified.hpp
+++ b/localization/ndt/include/ndt/pcl_modified.hpp
@@ -19,7 +19,7 @@
 
 #include <ndt_pcl_modified/ndt.hpp>
 
-#include <pcl/io/io.h>
+#include <pcl/common/io.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/point_types.h>
 
@@ -34,8 +34,8 @@ class NormalDistributionsTransformPCLModified
   ~NormalDistributionsTransformPCLModified() = default;
 
   void align(pcl::PointCloud<PointSource> & output, const Eigen::Matrix4f & guess) override;
-  void setInputTarget(const boost::shared_ptr<pcl::PointCloud<PointTarget>> & map_ptr) override;
-  void setInputSource(const boost::shared_ptr<pcl::PointCloud<PointSource>> & scan_ptr) override;
+  void setInputTarget(const pcl::shared_ptr<pcl::PointCloud<PointTarget>> & map_ptr) override;
+  void setInputSource(const pcl::shared_ptr<pcl::PointCloud<PointSource>> & scan_ptr) override;
 
   void setMaximumIterations(int max_iter) override;
   void setResolution(float res) override;
@@ -50,15 +50,15 @@ class NormalDistributionsTransformPCLModified
   double getTransformationProbability() const override;
   double getNearestVoxelTransformationLikelihood() const override;
   double getFitnessScore() override;
-  boost::shared_ptr<const pcl::PointCloud<PointTarget>> getInputTarget() const override;
-  boost::shared_ptr<const pcl::PointCloud<PointSource>> getInputSource() const override;
+  pcl::shared_ptr<const pcl::PointCloud<PointTarget>> getInputTarget() const override;
+  pcl::shared_ptr<const pcl::PointCloud<PointSource>> getInputSource() const override;
   Eigen::Matrix4f getFinalTransformation() const override;
   std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
   getFinalTransformationArray() const override;
 
   Eigen::Matrix<double, 6, 6> getHessian() const override;
 
-  boost::shared_ptr<pcl::search::KdTree<PointTarget>> getSearchMethodTarget() const override;
+  pcl::shared_ptr<pcl::search::KdTree<PointTarget>> getSearchMethodTarget() const override;
 
   double calculateTransformationProbability(
     const pcl::PointCloud<PointSource> & trans_cloud) const override;
@@ -66,7 +66,7 @@ class NormalDistributionsTransformPCLModified
     const pcl::PointCloud<PointSource> & trans_cloud) const override;
 
 private:
-  boost::shared_ptr<pcl::NormalDistributionsTransformModified<PointSource, PointTarget>> ndt_ptr_;
+  pcl::shared_ptr<pcl::NormalDistributionsTransformModified<PointSource, PointTarget>> ndt_ptr_;
 };
 
 #include "ndt/impl/pcl_modified.hpp"
diff --git a/localization/ndt_pcl_modified/CMakeLists.txt b/localization/ndt_pcl_modified/CMakeLists.txt
index b7faa1eda40c0..47605e0cace66 100644
--- a/localization/ndt_pcl_modified/CMakeLists.txt
+++ b/localization/ndt_pcl_modified/CMakeLists.txt
@@ -41,6 +41,11 @@ target_include_directories(ndt_pcl_modified
     $<INSTALL_INTERFACE:include>
 )
 
+target_include_directories(ndt_pcl_modified
+  SYSTEM PUBLIC
+    ${PCL_INCLUDE_DIRS}
+)
+
 ament_target_dependencies(ndt_pcl_modified PCL)
 ament_export_targets(export_ndt_pcl_modified HAS_LIBRARY_TARGET)
 ament_export_dependencies(PCL)
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
index 0feb97a19d839..dc4da99e6222b 100644
--- a/localization/ndt_pcl_modified/include/ndt_pcl_modified/impl/ndt.hpp
+++ b/localization/ndt_pcl_modified/include/ndt_pcl_modified/impl/ndt.hpp
@@ -81,8 +81,13 @@ void pcl::NormalDistributionsTransformModified<PointSource, PointTarget>::comput
   }
 
   // Initialize Point Gradient and Hessian
+#if PCL_VERSION >= PCL_VERSION_CALC(1, 12, 1)
+  point_jacobian_.setZero();
+  point_jacobian_.block(0, 0, 3, 3).setIdentity();
+#else
   point_gradient_.setZero();
   point_gradient_.block(0, 0, 3, 3).setIdentity();
+#endif
   point_hessian_.setZero();
 
   Eigen::Transform<float, 3, Eigen::Affine, Eigen::ColMajor> eig_transformation;
@@ -366,8 +371,13 @@ double pcl::NormalDistributionsTransformModified<PointSource, PointTarget>::comp
   // Hessian is unnecessary for step length determination but gradients are required
   // so derivative and transform data is stored for the next iteration.
   if (step_iterations) {
+#if PCL_VERSION >= PCL_VERSION_CALC(1, 12, 1)
+    NormalDistributionsTransformModified<PointSource, PointTarget>::computeHessian(
+      hessian, trans_cloud);
+#else
     NormalDistributionsTransformModified<PointSource, PointTarget>::computeHessian(
       hessian, trans_cloud, x_t);
+#endif
   }
 
   return a_t;
diff --git a/localization/ndt_pcl_modified/include/ndt_pcl_modified/ndt.hpp b/localization/ndt_pcl_modified/include/ndt_pcl_modified/ndt.hpp
index 81a2e219f2b81..7a9958bfdd441 100644
--- a/localization/ndt_pcl_modified/include/ndt_pcl_modified/ndt.hpp
+++ b/localization/ndt_pcl_modified/include/ndt_pcl_modified/ndt.hpp
@@ -102,7 +102,11 @@ class NormalDistributionsTransformModified
   using NormalDistributionsTransform<PointSource, PointTarget>::step_size_;
   using NormalDistributionsTransform<PointSource, PointTarget>::gauss_d1_;
   using NormalDistributionsTransform<PointSource, PointTarget>::gauss_d2_;
+#if PCL_VERSION >= PCL_VERSION_CALC(1, 12, 1)
+  using NormalDistributionsTransform<PointSource, PointTarget>::point_jacobian_;
+#else
   using NormalDistributionsTransform<PointSource, PointTarget>::point_gradient_;
+#endif
   using NormalDistributionsTransform<PointSource, PointTarget>::point_hessian_;
   using NormalDistributionsTransform<PointSource, PointTarget>::trans_probability_;
 
diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp
index e0dd7f4022b15..86ebc4a132ce2 100644
--- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp
+++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp
@@ -36,11 +36,22 @@
 
 #include <fmt/format.h>
 #include <tf2/transform_datatypes.h>
+
+#ifdef USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
+
+#ifdef USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER
 #include <tf2_sensor_msgs/tf2_sensor_msgs.h>
+#else
+#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
+#endif
 
 #include <array>
 #include <deque>
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
index 85fecf11f060c..251b4ceb3c45c 100644
--- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp
+++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp
@@ -20,8 +20,14 @@
 #include <std_msgs/msg/color_rgba.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
+#ifdef USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER
 #include <tf2_eigen/tf2_eigen.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_eigen/tf2_eigen.hpp>
+
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
 
 #include <algorithm>
 #include <cmath>
diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml
index 9b6bb71d60426..b42a938089931 100644
--- a/localization/ndt_scan_matcher/package.xml
+++ b/localization/ndt_scan_matcher/package.xml
@@ -19,6 +19,7 @@
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>tf2</depend>
+  <depend>tf2_eigen</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>tf2_sensor_msgs</depend>
diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
index ce0c0b804d1ca..01b8c3c509ff5 100644
--- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
+++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
@@ -22,10 +22,13 @@
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
 
-#include <boost/shared_ptr.hpp>
-
 #include <pcl_conversions/pcl_conversions.h>
+
+#ifdef USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER
 #include <tf2_eigen/tf2_eigen.h>
+#else
+#include <tf2_eigen/tf2_eigen.hpp>
+#endif
 
 #include <algorithm>
 #include <cmath>
@@ -410,7 +413,7 @@ void NDTScanMatcher::callbackMapPoints(
   new_ndt_ptr_->setResolution(resolution);
   new_ndt_ptr_->setMaximumIterations(max_iterations);
 
-  boost::shared_ptr<pcl::PointCloud<PointTarget>> map_points_ptr(new pcl::PointCloud<PointTarget>);
+  pcl::shared_ptr<pcl::PointCloud<PointTarget>> map_points_ptr(new pcl::PointCloud<PointTarget>);
   pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr);
   new_ndt_ptr_->setInputTarget(map_points_ptr);
   // create Thread
@@ -442,7 +445,7 @@ void NDTScanMatcher::callbackSensorPoints(
   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<float>();
-  boost::shared_ptr<pcl::PointCloud<PointSource>> sensor_points_baselinkTF_ptr(
+  pcl::shared_ptr<pcl::PointCloud<PointSource>> sensor_points_baselinkTF_ptr(
     new pcl::PointCloud<PointSource>);
   pcl::transformPointCloud(
     *sensor_points_sensorTF_ptr, *sensor_points_baselinkTF_ptr, base_to_sensor_matrix);