Skip to content

Commit 6a06241

Browse files
SakodaShintaropre-commit-ci[bot]
SakodaShintaro
and
pre-commit-ci[bot]
authored
refactor(ndt_scan_matcher, ndt_omp): move ndt_omp into ndt_scan_matcher (#8912)
* Moved ndt_omp into ndt_scan_matcher Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Added Copyright Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * style(pre-commit): autofix * Fixed include Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed cast style Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed include Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed honorific title Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed honorific title Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * style(pre-commit): autofix * Fixed include hierarchy Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * style(pre-commit): autofix * Fixed include hierarchy Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * style(pre-commit): autofix * Fixed hierarchy Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed NVTP to NVTL Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Added cspell:ignore Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed miss spell Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * style(pre-commit): autofix * Fixed include Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Renamed applyFilter Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Moved ***_impl.hpp from include/ to src/ Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * style(pre-commit): autofix * Fixed variable scope Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed to pass by reference Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> --------- Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent a9763cb commit 6a06241

17 files changed

+3232
-11
lines changed

‎localization/autoware_ndt_scan_matcher/CMakeLists.txt

+18-1
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,8 @@
11
cmake_minimum_required(VERSION 3.14)
22
project(autoware_ndt_scan_matcher)
33

4+
# cspell:ignore multigrid
5+
46
find_package(autoware_cmake REQUIRED)
57
autoware_package()
68

@@ -22,17 +24,32 @@ else()
2224
endif()
2325
endif()
2426

27+
find_package(OpenMP)
28+
2529
find_package(PCL REQUIRED COMPONENTS common io registration)
2630
include_directories(${PCL_INCLUDE_DIRS})
2731

32+
ament_auto_add_library(multigrid_ndt_omp SHARED
33+
src/ndt_omp/multi_voxel_grid_covariance_omp.cpp
34+
src/ndt_omp/multigrid_ndt_omp.cpp
35+
src/ndt_omp/estimate_covariance.cpp
36+
)
37+
target_link_libraries(multigrid_ndt_omp ${PCL_LIBRARIES})
38+
39+
if(OpenMP_CXX_FOUND)
40+
target_link_libraries(multigrid_ndt_omp OpenMP::OpenMP_CXX)
41+
else()
42+
message(WARNING "OpenMP not found")
43+
endif()
44+
2845
ament_auto_add_library(${PROJECT_NAME} SHARED
2946
src/map_update_module.cpp
3047
src/ndt_scan_matcher_core.cpp
3148
src/particle.cpp
3249
)
3350

3451
link_directories(${PCL_LIBRARY_DIRS})
35-
target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES})
52+
target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES} multigrid_ndt_omp)
3653

3754
rclcpp_components_register_node(${PROJECT_NAME}
3855
PLUGIN "autoware::ndt_scan_matcher::NDTScanMatcher"

‎localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/hyper_parameters.hpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -15,9 +15,9 @@
1515
#ifndef AUTOWARE__NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_
1616
#define AUTOWARE__NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_
1717

18-
#include <rclcpp/rclcpp.hpp>
18+
#include "ndt_omp/multigrid_ndt_omp.h"
1919

20-
#include <multigrid_pclomp/multigrid_ndt_omp.h>
20+
#include <rclcpp/rclcpp.hpp>
2121

2222
#include <algorithm>
2323
#include <sstream>

‎localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -17,8 +17,9 @@
1717

1818
#include "autoware/localization_util/diagnostics_module.hpp"
1919
#include "autoware/localization_util/util_func.hpp"
20-
#include "autoware/ndt_scan_matcher/hyper_parameters.hpp"
21-
#include "autoware/ndt_scan_matcher/particle.hpp"
20+
#include "hyper_parameters.hpp"
21+
#include "ndt_omp/multigrid_ndt_omp.h"
22+
#include "particle.hpp"
2223

2324
#include <autoware/universe_utils/ros/marker_helper.hpp>
2425
#include <autoware/universe_utils/transform/transforms.hpp>
@@ -30,7 +31,6 @@
3031
#include <visualization_msgs/msg/marker_array.hpp>
3132

3233
#include <fmt/format.h>
33-
#include <multigrid_pclomp/multigrid_ndt_omp.h>
3434
#include <pcl_conversions/pcl_conversions.h>
3535

3636
#include <map>

‎localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/LICENSE

+25
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
BSD 2-Clause License
2+
3+
Copyright (c) 2019, k.koide
4+
All rights reserved.
5+
6+
Redistribution and use in source and binary forms, with or without
7+
modification, are permitted provided that the following conditions are met:
8+
9+
1. Redistributions of source code must retain the above copyright notice, this
10+
list of conditions and the following disclaimer.
11+
12+
2. Redistributions in binary form must reproduce the above copyright notice,
13+
this list of conditions and the following disclaimer in the documentation
14+
and/or other materials provided with the distribution.
15+
16+
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17+
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18+
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19+
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
20+
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
21+
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
22+
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
23+
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
24+
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
25+
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

‎localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/README.md

+12
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
# ndt_omp
2+
3+
<!-->
4+
5+
cspell:ignore Kenji, Koide
6+
<-->
7+
8+
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>.
9+
10+
We use this code in accordance with the LICENSE, and we have directly confirmed this with Dr. Kenji Koide.
11+
12+
We sincerely appreciate Dr. Koide’s support and contributions.

‎localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/estimate_covariance.hpp

+90
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,90 @@
1+
// Copyright 2023 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef AUTOWARE__NDT_SCAN_MATCHER__NDT_OMP__ESTIMATE_COVARIANCE_HPP_
16+
#define AUTOWARE__NDT_SCAN_MATCHER__NDT_OMP__ESTIMATE_COVARIANCE_HPP_
17+
18+
#include "multigrid_ndt_omp.h"
19+
20+
#include <Eigen/Core>
21+
22+
#include <memory>
23+
#include <utility>
24+
#include <vector>
25+
26+
namespace pclomp
27+
{
28+
29+
struct ResultOfMultiNdtCovarianceEstimation
30+
{
31+
Eigen::Vector2d mean;
32+
Eigen::Matrix2d covariance;
33+
std::vector<Eigen::Matrix4f> ndt_initial_poses;
34+
std::vector<NdtResult> ndt_results;
35+
};
36+
37+
/** \brief Estimate functions */
38+
Eigen::Matrix2d estimate_xy_covariance_by_laplace_approximation(
39+
const Eigen::Matrix<double, 6, 6> & hessian);
40+
ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt(
41+
const NdtResult & ndt_result,
42+
const std::shared_ptr<
43+
pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>> & ndt_ptr,
44+
const std::vector<Eigen::Matrix4f> & poses_to_search);
45+
ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt_score(
46+
const NdtResult & ndt_result,
47+
const std::shared_ptr<
48+
pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>> & ndt_ptr,
49+
const std::vector<Eigen::Matrix4f> & poses_to_search, const double temperature);
50+
51+
/** \brief Find rotation matrix aligning covariance to principal axes
52+
* (1) Compute eigenvalues and eigenvectors
53+
* (2) Compute angle for first eigenvector
54+
* (3) Return rotation matrix
55+
*/
56+
Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes(
57+
const Eigen::Matrix2d & matrix);
58+
59+
/** \brief Propose poses to search.
60+
* (1) Compute covariance by Laplace approximation
61+
* (2) Find rotation matrix aligning covariance to principal axes
62+
* (3) Propose search points by adding offset_x and offset_y to the center_pose
63+
*/
64+
std::vector<Eigen::Matrix4f> propose_poses_to_search(
65+
const NdtResult & ndt_result, const std::vector<double> & offset_x,
66+
const std::vector<double> & offset_y);
67+
68+
/** \brief Calculate weights by exponential */
69+
std::vector<double> calc_weight_vec(const std::vector<double> & score_vec, double temperature);
70+
71+
/** \brief Calculate weighted mean and covariance */
72+
std::pair<Eigen::Vector2d, Eigen::Matrix2d> calculate_weighted_mean_and_cov(
73+
const std::vector<Eigen::Vector2d> & pose_2d_vec, const std::vector<double> & weight_vec);
74+
75+
/** \brief Rotate covariance to base_link coordinate */
76+
Eigen::Matrix2d rotate_covariance_to_base_link(
77+
const Eigen::Matrix2d & covariance, const Eigen::Matrix4f & pose);
78+
79+
/** \brief Rotate covariance to map coordinate */
80+
Eigen::Matrix2d rotate_covariance_to_map(
81+
const Eigen::Matrix2d & covariance, const Eigen::Matrix4f & pose);
82+
83+
/** \brief Adjust diagonal covariance */
84+
Eigen::Matrix2d adjust_diagonal_covariance(
85+
const Eigen::Matrix2d & covariance, const Eigen::Matrix4f & pose, const double fixed_cov00,
86+
const double fixed_cov11);
87+
88+
} // namespace pclomp
89+
90+
#endif // AUTOWARE__NDT_SCAN_MATCHER__NDT_OMP__ESTIMATE_COVARIANCE_HPP_

0 commit comments

Comments
 (0)