fix(ndt_scan_matcher): fix compiler flags and Eigen aligned allocator…

… to support general SIMD architecture (autowarefoundation#154) * using eigen's aligned allocator for matrix vectors Signed-off-by: Daichi Murakami <harihitode@gmail.com> * fix compiler flag to be architecture independent Signed-off-by: Daichi Murakami <harihitode@gmail.com> * ci(pre-commit): autofix * add const Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com>
badai-nguyen · Jan 17, 2022 · e192d76 · e192d76
1 parent c9e2431
commit e192d76
Show file tree

Hide file tree

Showing 12 changed files with 30 additions and 16 deletions.
diff --git a/localization/ndt/CMakeLists.txt b/localization/ndt/CMakeLists.txt
@@ -1,6 +1,8 @@
 cmake_minimum_required(VERSION 3.5)
 project(ndt)
 
+add_compile_options(-march=native)
+
 if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 14)
   set(CMAKE_CXX_STANDARD_REQUIRED ON)

diff --git a/localization/ndt/include/ndt/base.hpp b/localization/ndt/include/ndt/base.hpp
@@ -48,7 +48,8 @@ class NormalDistributionsTransformBase
   virtual boost::shared_ptr<const pcl::PointCloud<PointTarget>> getInputTarget() const = 0;
   virtual boost::shared_ptr<const pcl::PointCloud<PointSource>> getInputSource() const = 0;
   virtual Eigen::Matrix4f getFinalTransformation() const = 0;
-  virtual std::vector<Eigen::Matrix4f> getFinalTransformationArray() const = 0;
+  virtual std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
+  getFinalTransformationArray() const = 0;
 
   virtual Eigen::Matrix<double, 6, 6> getHessian() const = 0;
 

diff --git a/localization/ndt/include/ndt/impl/omp.hpp b/localization/ndt/include/ndt/impl/omp.hpp
@@ -136,7 +136,7 @@ Eigen::Matrix4f NormalDistributionsTransformOMP<PointSource, PointTarget>::getFi
 }
 
 template <class PointSource, class PointTarget>
-std::vector<Eigen::Matrix4f>
+std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
 NormalDistributionsTransformOMP<PointSource, PointTarget>::getFinalTransformationArray() const
 {
   return ndt_ptr_->getFinalTransformationArray();

diff --git a/localization/ndt/include/ndt/impl/pcl_generic.hpp b/localization/ndt/include/ndt/impl/pcl_generic.hpp
@@ -138,10 +138,11 @@ NormalDistributionsTransformPCLGeneric<PointSource, PointTarget>::getFinalTransf
 }
 
 template <class PointSource, class PointTarget>
-std::vector<Eigen::Matrix4f> NormalDistributionsTransformPCLGeneric<
-  PointSource, PointTarget>::getFinalTransformationArray() const
+std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
+NormalDistributionsTransformPCLGeneric<PointSource, PointTarget>::getFinalTransformationArray()
+  const
 {
-  return std::vector<Eigen::Matrix4f>();
+  return std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>();
 }
 
 template <class PointSource, class PointTarget>

diff --git a/localization/ndt/include/ndt/impl/pcl_modified.hpp b/localization/ndt/include/ndt/impl/pcl_modified.hpp
@@ -139,8 +139,9 @@ NormalDistributionsTransformPCLModified<PointSource, PointTarget>::getFinalTrans
 }
 
 template <class PointSource, class PointTarget>
-std::vector<Eigen::Matrix4f> NormalDistributionsTransformPCLModified<
-  PointSource, PointTarget>::getFinalTransformationArray() const
+std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
+NormalDistributionsTransformPCLModified<PointSource, PointTarget>::getFinalTransformationArray()
+  const
 {
   return ndt_ptr_->getFinalTransformationArray();
 }

diff --git a/localization/ndt/include/ndt/omp.hpp b/localization/ndt/include/ndt/omp.hpp
@@ -51,7 +51,8 @@ class NormalDistributionsTransformOMP
   boost::shared_ptr<const pcl::PointCloud<PointTarget>> getInputTarget() const override;
   boost::shared_ptr<const pcl::PointCloud<PointSource>> getInputSource() const override;
   Eigen::Matrix4f getFinalTransformation() const override;
-  std::vector<Eigen::Matrix4f> getFinalTransformationArray() const override;
+  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
+  getFinalTransformationArray() const override;
 
   Eigen::Matrix<double, 6, 6> getHessian() const override;
 

diff --git a/localization/ndt/include/ndt/pcl_generic.hpp b/localization/ndt/include/ndt/pcl_generic.hpp
@@ -51,7 +51,8 @@ class NormalDistributionsTransformPCLGeneric
   boost::shared_ptr<const pcl::PointCloud<PointTarget>> getInputTarget() const override;
   boost::shared_ptr<const pcl::PointCloud<PointSource>> getInputSource() const override;
   Eigen::Matrix4f getFinalTransformation() const override;
-  std::vector<Eigen::Matrix4f> getFinalTransformationArray() const override;
+  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
+  getFinalTransformationArray() const override;
 
   Eigen::Matrix<double, 6, 6> getHessian() const override;
 

diff --git a/localization/ndt/include/ndt/pcl_modified.hpp b/localization/ndt/include/ndt/pcl_modified.hpp
@@ -52,7 +52,8 @@ class NormalDistributionsTransformPCLModified
   boost::shared_ptr<const pcl::PointCloud<PointTarget>> getInputTarget() const override;
   boost::shared_ptr<const pcl::PointCloud<PointSource>> getInputSource() const override;
   Eigen::Matrix4f getFinalTransformation() const override;
-  std::vector<Eigen::Matrix4f> getFinalTransformationArray() const override;
+  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
+  getFinalTransformationArray() const override;
 
   Eigen::Matrix<double, 6, 6> getHessian() const override;
 

diff --git a/localization/ndt_pcl_modified/CMakeLists.txt b/localization/ndt_pcl_modified/CMakeLists.txt
@@ -1,6 +1,8 @@
 cmake_minimum_required(VERSION 3.5)
 project(ndt_pcl_modified)
 
+add_compile_options(-march=native)
+
 if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 14)
   set(CMAKE_CXX_STANDARD_REQUIRED ON)

diff --git a/localization/ndt_pcl_modified/include/ndt_pcl_modified/ndt.hpp b/localization/ndt_pcl_modified/include/ndt_pcl_modified/ndt.hpp
@@ -74,7 +74,8 @@ class NormalDistributionsTransformModified
 
   inline const Eigen::Matrix<double, 6, 6> getHessian() const { return hessian_; }
 
-  inline const std::vector<Eigen::Matrix4f> getFinalTransformationArray() const
+  inline const std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
+  getFinalTransformationArray() const
   {
     return transformation_array_;
   }
@@ -106,7 +107,7 @@ class NormalDistributionsTransformModified
   using NormalDistributionsTransform<PointSource, PointTarget>::trans_probability_;
 
   Eigen::Matrix<double, 6, 6> hessian_;
-  std::vector<Eigen::Matrix4f> transformation_array_;
+  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> transformation_array_;
 
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW

diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt
@@ -1,6 +1,8 @@
 cmake_minimum_required(VERSION 3.5)
 project(ndt_scan_matcher)
 
+add_compile_options(-march=native)
+
 if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 14)
   set(CMAKE_CXX_STANDARD_REQUIRED ON)

diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
@@ -67,8 +67,9 @@ double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Poin
 }
 
 bool isLocalOptimalSolutionOscillation(
-  const std::vector<Eigen::Matrix4f> & result_pose_matrix_array, const float oscillation_threshold,
-  const float inversion_vector_threshold)
+  const std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> &
+    result_pose_matrix_array,
+  const float oscillation_threshold, const float inversion_vector_threshold)
 {
   bool prev_oscillation = false;
   int oscillation_cnt = 0;
@@ -450,8 +451,8 @@ void NDTScanMatcher::callbackSensorPoints(
   result_pose_affine.matrix() = result_pose_matrix.cast<double>();
   const geometry_msgs::msg::Pose result_pose_msg = tf2::toMsg(result_pose_affine);
 
-  const std::vector<Eigen::Matrix4f> result_pose_matrix_array =
-    ndt_ptr_->getFinalTransformationArray();
+  const std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
+    result_pose_matrix_array = ndt_ptr_->getFinalTransformationArray();
   std::vector<geometry_msgs::msg::Pose> result_pose_msg_array;
   for (const auto & pose_matrix : result_pose_matrix_array) {
     Eigen::Affine3d pose_affine;