fix: modify build error of ndt related packages in rolling (#793)

* fix(ndt_pcl_modified): modify build error in rolling Signed-off-by: wep21 <[email protected]> * fix(ndt): modify build error in rolling Signed-off-by: wep21 <[email protected]> * fix(ndt_scan_matcher): modify build error in rolling Signed-off-by: wep21 <[email protected]> * ci(pre-commit): autofix * remove unused find_package * fix(ndt): remove memory.h Signed-off-by: wep21 <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Signed-off-by: guiping meng <[email protected]>
tier4 · Sep 21, 2022 · a235363 · a235363
1 parent 7075fca
commit a235363
Show file tree

Hide file tree

Showing 15 changed files with 91 additions and 46 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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,23 +49,23 @@ 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;
   double calculateNearestVoxelTransformationLikelihood(
     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
@@ -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,23 +50,23 @@ 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;
   double calculateNearestVoxelTransformationLikelihood(
     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
@@ -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
@@ -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
@@ -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
@@ -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>