refactor(ndt_scan_matcher, ndt_omp): move ndt_omp into ndt_scan_match…

…er (#8912) * Moved ndt_omp into ndt_scan_matcher Signed-off-by: Shintaro Sakoda <[email protected]> * Added Copyright Signed-off-by: Shintaro Sakoda <[email protected]> * style(pre-commit): autofix * Fixed include Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed cast style Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed include Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed honorific title Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed honorific title Signed-off-by: Shintaro Sakoda <[email protected]> * style(pre-commit): autofix * Fixed include hierarchy Signed-off-by: Shintaro Sakoda <[email protected]> * style(pre-commit): autofix * Fixed include hierarchy Signed-off-by: Shintaro Sakoda <[email protected]> * style(pre-commit): autofix * Fixed hierarchy Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed NVTP to NVTL Signed-off-by: Shintaro Sakoda <[email protected]> * Added cspell:ignore Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed miss spell Signed-off-by: Shintaro Sakoda <[email protected]> * style(pre-commit): autofix * Fixed include Signed-off-by: Shintaro Sakoda <[email protected]> * Renamed applyFilter Signed-off-by: Shintaro Sakoda <[email protected]> * Moved ***_impl.hpp from include/ to src/ Signed-off-by: Shintaro Sakoda <[email protected]> * style(pre-commit): autofix * Fixed variable scope Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed to pass by reference Signed-off-by: Shintaro Sakoda <[email protected]> --------- Signed-off-by: Shintaro Sakoda <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Oct 31, 2024 · 6a06241 · 6a06241
1 parent a9763cb
commit 6a06241
Show file tree

Hide file tree

Showing 17 changed files with 3,232 additions and 11 deletions.
diff --git a/localization/autoware_ndt_scan_matcher/CMakeLists.txt b/localization/autoware_ndt_scan_matcher/CMakeLists.txt
@@ -1,6 +1,8 @@
 cmake_minimum_required(VERSION 3.14)
 project(autoware_ndt_scan_matcher)
 
+# cspell:ignore multigrid
+
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
@@ -22,17 +24,32 @@ else()
   endif()
 endif()
 
+find_package(OpenMP)
+
 find_package(PCL REQUIRED COMPONENTS common io registration)
 include_directories(${PCL_INCLUDE_DIRS})
 
+ament_auto_add_library(multigrid_ndt_omp SHARED
+  src/ndt_omp/multi_voxel_grid_covariance_omp.cpp
+  src/ndt_omp/multigrid_ndt_omp.cpp
+  src/ndt_omp/estimate_covariance.cpp
+)
+target_link_libraries(multigrid_ndt_omp ${PCL_LIBRARIES})
+
+if(OpenMP_CXX_FOUND)
+  target_link_libraries(multigrid_ndt_omp OpenMP::OpenMP_CXX)
+else()
+  message(WARNING "OpenMP not found")
+endif()
+
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/map_update_module.cpp
   src/ndt_scan_matcher_core.cpp
   src/particle.cpp
 )
 
 link_directories(${PCL_LIBRARY_DIRS})
-target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES} multigrid_ndt_omp)
 
 rclcpp_components_register_node(${PROJECT_NAME}
   PLUGIN "autoware::ndt_scan_matcher::NDTScanMatcher"

diff --git a/...lization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/hyper_parameters.hpp b/...lization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/hyper_parameters.hpp
@@ -15,9 +15,9 @@
 #ifndef AUTOWARE__NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_
 #define AUTOWARE__NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_
 
-#include <rclcpp/rclcpp.hpp>
+#include "ndt_omp/multigrid_ndt_omp.h"
 
-#include <multigrid_pclomp/multigrid_ndt_omp.h>
+#include <rclcpp/rclcpp.hpp>
 
 #include <algorithm>
 #include <sstream>

diff --git a/...ization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp b/...ization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp
@@ -17,8 +17,9 @@
 
 #include "autoware/localization_util/diagnostics_module.hpp"
 #include "autoware/localization_util/util_func.hpp"
-#include "autoware/ndt_scan_matcher/hyper_parameters.hpp"
-#include "autoware/ndt_scan_matcher/particle.hpp"
+#include "hyper_parameters.hpp"
+#include "ndt_omp/multigrid_ndt_omp.h"
+#include "particle.hpp"
 
 #include <autoware/universe_utils/ros/marker_helper.hpp>
 #include <autoware/universe_utils/transform/transforms.hpp>
@@ -30,7 +31,6 @@
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include <fmt/format.h>
-#include <multigrid_pclomp/multigrid_ndt_omp.h>
 #include <pcl_conversions/pcl_conversions.h>
 
 #include <map>

diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/LICENSE b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/LICENSE
@@ -0,0 +1,25 @@
+BSD 2-Clause License
+
+Copyright (c) 2019, k.koide
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. 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.
+
+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 HOLDER 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.
diff --git a/...n/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/README.md b/...n/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/README.md
@@ -0,0 +1,12 @@
+# ndt_omp
+
+<!-->
+
+cspell:ignore Kenji, Koide
+<-->
+
+The codes in this directory are derived from <https://github.com/tier4/ndt_omp>, which is a fork of <https://github.com/koide3/ndt_omp>.
+
+We use this code in accordance with the LICENSE, and we have directly confirmed this with Dr. Kenji Koide.
+
+We sincerely appreciate Dr. Koide’s support and contributions.
diff --git a/...toware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/estimate_covariance.hpp b/...toware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/estimate_covariance.hpp
@@ -0,0 +1,90 @@
+// Copyright 2023 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.
+
+#ifndef AUTOWARE__NDT_SCAN_MATCHER__NDT_OMP__ESTIMATE_COVARIANCE_HPP_
+#define AUTOWARE__NDT_SCAN_MATCHER__NDT_OMP__ESTIMATE_COVARIANCE_HPP_
+
+#include "multigrid_ndt_omp.h"
+
+#include <Eigen/Core>
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+namespace pclomp
+{
+
+struct ResultOfMultiNdtCovarianceEstimation
+{
+  Eigen::Vector2d mean;
+  Eigen::Matrix2d covariance;
+  std::vector<Eigen::Matrix4f> ndt_initial_poses;
+  std::vector<NdtResult> ndt_results;
+};
+
+/** \brief Estimate functions */
+Eigen::Matrix2d estimate_xy_covariance_by_laplace_approximation(
+  const Eigen::Matrix<double, 6, 6> & hessian);
+ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt(
+  const NdtResult & ndt_result,
+  const std::shared_ptr<
+    pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>> & ndt_ptr,
+  const std::vector<Eigen::Matrix4f> & poses_to_search);
+ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt_score(
+  const NdtResult & ndt_result,
+  const std::shared_ptr<
+    pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>> & ndt_ptr,
+  const std::vector<Eigen::Matrix4f> & poses_to_search, const double temperature);
+
+/** \brief Find rotation matrix aligning covariance to principal axes
+ * (1) Compute eigenvalues and eigenvectors
+ * (2) Compute angle for first eigenvector
+ * (3) Return rotation matrix
+ */
+Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes(
+  const Eigen::Matrix2d & matrix);
+
+/** \brief Propose poses to search.
+ * (1) Compute covariance by Laplace approximation
+ * (2) Find rotation matrix aligning covariance to principal axes
+ * (3) Propose search points by adding offset_x and offset_y to the center_pose
+ */
+std::vector<Eigen::Matrix4f> propose_poses_to_search(
+  const NdtResult & ndt_result, const std::vector<double> & offset_x,
+  const std::vector<double> & offset_y);
+
+/** \brief Calculate weights by exponential */
+std::vector<double> calc_weight_vec(const std::vector<double> & score_vec, double temperature);
+
+/** \brief Calculate weighted mean and covariance */
+std::pair<Eigen::Vector2d, Eigen::Matrix2d> calculate_weighted_mean_and_cov(
+  const std::vector<Eigen::Vector2d> & pose_2d_vec, const std::vector<double> & weight_vec);
+
+/** \brief Rotate covariance to base_link coordinate */
+Eigen::Matrix2d rotate_covariance_to_base_link(
+  const Eigen::Matrix2d & covariance, const Eigen::Matrix4f & pose);
+
+/** \brief Rotate covariance to map coordinate */
+Eigen::Matrix2d rotate_covariance_to_map(
+  const Eigen::Matrix2d & covariance, const Eigen::Matrix4f & pose);
+
+/** \brief  Adjust diagonal covariance */
+Eigen::Matrix2d adjust_diagonal_covariance(
+  const Eigen::Matrix2d & covariance, const Eigen::Matrix4f & pose, const double fixed_cov00,
+  const double fixed_cov11);
+
+}  // namespace pclomp
+
+#endif  // AUTOWARE__NDT_SCAN_MATCHER__NDT_OMP__ESTIMATE_COVARIANCE_HPP_