Skip to content

Commit f5b1b94

Browse files
SakodaShintaropre-commit-ci[bot]
authored andcommitted
fix(ndt_scan_matcher): improved tpe (autowarefoundation#6990)
* Improved tpe Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Added name in TODO Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed tpe test Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * style(pre-commit): autofix --------- 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> Signed-off-by: vividf <yihsiang.fang@tier4.jp>
1 parent bc93a8e commit f5b1b94

File tree

6 files changed

+117
-174
lines changed

6 files changed

+117
-174
lines changed

localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml

+1-1
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,7 @@
4848
# The number of initial random trials in the TPE (Tree-Structured Parzen Estimator).
4949
# This value should be equal to or less than 'initial_estimate_particles_num' and more than 0.
5050
# If it is equal to 'initial_estimate_particles_num', the search will be the same as a full random search.
51-
n_startup_trials: 20
51+
n_startup_trials: 100
5252

5353

5454
validation:

localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json

+1-1
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@
1414
"n_startup_trials": {
1515
"type": "number",
1616
"description": "The number of initial random trials in the TPE (Tree-Structured Parzen Estimator). This value should be equal to or less than 'initial_estimate_particles_num' and more than 0. If it is equal to 'initial_estimate_particles_num', the search will be the same as a full random search.",
17-
"default": 20,
17+
"default": 100,
1818
"minimum": 1
1919
}
2020
},

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

+22-50
Original file line numberDiff line numberDiff line change
@@ -22,8 +22,6 @@
2222
#include <tier4_autoware_utils/geometry/geometry.hpp>
2323
#include <tier4_autoware_utils/transform/transforms.hpp>
2424

25-
#include <boost/math/special_functions/erf.hpp>
26-
2725
#include <pcl_conversions/pcl_conversions.h>
2826

2927
#ifdef ROS_DISTRO_GALACTIC
@@ -988,34 +986,20 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose(
988986
const double stddev_roll = std::sqrt(covariance(3, 3));
989987
const double stddev_pitch = std::sqrt(covariance(4, 4));
990988

991-
// Let phi be the cumulative distribution function of the standard normal distribution.
992-
// It has the following relationship with the error function (erf).
993-
// phi(x) = 1/2 (1 + erf(x / sqrt(2)))
994-
// so, 2 * phi(x) - 1 = erf(x / sqrt(2)).
995-
// The range taken by 2 * phi(x) - 1 is [-1, 1], so it can be used as a uniform distribution in
996-
// TPE. Let u = 2 * phi(x) - 1, then x = sqrt(2) * erf_inv(u). Computationally, it is not a good
997-
// to give erf_inv -1 and 1, so it is rounded off at (-1 + eps, 1 - eps).
998-
const double sqrt2 = std::sqrt(2);
999-
auto uniform_to_normal = [&sqrt2](const double uniform) {
1000-
assert(-1.0 <= uniform && uniform <= 1.0);
1001-
constexpr double epsilon = 1.0e-6;
1002-
const double clamped = std::clamp(uniform, -1.0 + epsilon, 1.0 - epsilon);
1003-
return boost::math::erf_inv(clamped) * sqrt2;
1004-
};
1005-
1006-
auto normal_to_uniform = [&sqrt2](const double normal) {
1007-
return boost::math::erf(normal / sqrt2);
989+
// Since only yaw is uniformly sampled, we define the mean and standard deviation for the others.
990+
const std::vector<double> sample_mean{
991+
initial_pose_with_cov.pose.pose.position.x, // trans_x
992+
initial_pose_with_cov.pose.pose.position.y, // trans_y
993+
initial_pose_with_cov.pose.pose.position.z, // trans_z
994+
base_rpy.x, // angle_x
995+
base_rpy.y // angle_y
1008996
};
997+
const std::vector<double> sample_stddev{stddev_x, stddev_y, stddev_z, stddev_roll, stddev_pitch};
1009998

1010999
// Optimizing (x, y, z, roll, pitch, yaw) 6 dimensions.
1011-
// The last dimension (yaw) is a loop variable.
1012-
// Although roll and pitch are also angles, they are considered non-looping variables that follow
1013-
// a normal distribution with a small standard deviation. This assumes that the initial pose of
1014-
// the ego vehicle is aligned with the ground to some extent about roll and pitch.
1015-
const std::vector<bool> is_loop_variable = {false, false, false, false, false, true};
10161000
TreeStructuredParzenEstimator tpe(
10171001
TreeStructuredParzenEstimator::Direction::MAXIMIZE,
1018-
param_.initial_pose_estimation.n_startup_trials, is_loop_variable);
1002+
param_.initial_pose_estimation.n_startup_trials, sample_mean, sample_stddev);
10191003

10201004
std::vector<Particle> particle_array;
10211005
auto output_cloud = std::make_shared<pcl::PointCloud<PointSource>>();
@@ -1029,16 +1013,13 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose(
10291013
const TreeStructuredParzenEstimator::Input input = tpe.get_next_input();
10301014

10311015
geometry_msgs::msg::Pose initial_pose;
1032-
initial_pose.position.x =
1033-
initial_pose_with_cov.pose.pose.position.x + uniform_to_normal(input[0]) * stddev_x;
1034-
initial_pose.position.y =
1035-
initial_pose_with_cov.pose.pose.position.y + uniform_to_normal(input[1]) * stddev_y;
1036-
initial_pose.position.z =
1037-
initial_pose_with_cov.pose.pose.position.z + uniform_to_normal(input[2]) * stddev_z;
1016+
initial_pose.position.x = input[0];
1017+
initial_pose.position.y = input[1];
1018+
initial_pose.position.z = input[2];
10381019
geometry_msgs::msg::Vector3 init_rpy;
1039-
init_rpy.x = base_rpy.x + uniform_to_normal(input[3]) * stddev_roll;
1040-
init_rpy.y = base_rpy.y + uniform_to_normal(input[4]) * stddev_pitch;
1041-
init_rpy.z = base_rpy.z + input[5] * M_PI;
1020+
init_rpy.x = input[3];
1021+
init_rpy.y = input[4];
1022+
init_rpy.z = input[5];
10421023
tf2::Quaternion tf_quaternion;
10431024
tf_quaternion.setRPY(init_rpy.x, init_rpy.y, init_rpy.z);
10441025
initial_pose.orientation = tf2::toMsg(tf_quaternion);
@@ -1061,22 +1042,13 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose(
10611042
const geometry_msgs::msg::Pose pose = matrix4f_to_pose(ndt_result.pose);
10621043
const geometry_msgs::msg::Vector3 rpy = get_rpy(pose);
10631044

1064-
const double diff_x = pose.position.x - initial_pose_with_cov.pose.pose.position.x;
1065-
const double diff_y = pose.position.y - initial_pose_with_cov.pose.pose.position.y;
1066-
const double diff_z = pose.position.z - initial_pose_with_cov.pose.pose.position.z;
1067-
const double diff_roll = rpy.x - base_rpy.x;
1068-
const double diff_pitch = rpy.y - base_rpy.y;
1069-
const double diff_yaw = rpy.z - base_rpy.z;
1070-
1071-
// Only yaw is a loop_variable, so only simple normalization is performed.
1072-
// All other variables are converted from normal distribution to uniform distribution.
1073-
TreeStructuredParzenEstimator::Input result(is_loop_variable.size());
1074-
result[0] = normal_to_uniform(diff_x / stddev_x);
1075-
result[1] = normal_to_uniform(diff_y / stddev_y);
1076-
result[2] = normal_to_uniform(diff_z / stddev_z);
1077-
result[3] = normal_to_uniform(diff_roll / stddev_roll);
1078-
result[4] = normal_to_uniform(diff_pitch / stddev_pitch);
1079-
result[5] = diff_yaw / M_PI;
1045+
TreeStructuredParzenEstimator::Input result(6);
1046+
result[0] = pose.position.x;
1047+
result[1] = pose.position.y;
1048+
result[2] = pose.position.z;
1049+
result[3] = rpy.x;
1050+
result[4] = rpy.y;
1051+
result[5] = rpy.z;
10801052
tpe.add_trial(TreeStructuredParzenEstimator::Trial{result, ndt_result.transform_probability});
10811053

10821054
auto sensor_points_in_map_ptr = std::make_shared<pcl::PointCloud<PointSource>>();

localization/tree_structured_parzen_estimator/include/tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp

+15-12
Original file line numberDiff line numberDiff line change
@@ -44,37 +44,40 @@ class TreeStructuredParzenEstimator
4444
MAXIMIZE = 1,
4545
};
4646

47+
enum Index {
48+
TRANS_X = 0,
49+
TRANS_Y = 1,
50+
TRANS_Z = 2,
51+
ANGLE_X = 3,
52+
ANGLE_Y = 4,
53+
ANGLE_Z = 5,
54+
INDEX_NUM = 6,
55+
};
56+
4757
TreeStructuredParzenEstimator() = delete;
4858
TreeStructuredParzenEstimator(
49-
const Direction direction, const int64_t n_startup_trials, std::vector<bool> is_loop_variable);
59+
const Direction direction, const int64_t n_startup_trials,
60+
const std::vector<double> & sample_mean, const std::vector<double> & sample_stddev);
5061
void add_trial(const Trial & trial);
5162
Input get_next_input() const;
5263

5364
private:
54-
static constexpr double BASE_STDDEV_COEFF = 0.2;
5565
static constexpr double MAX_GOOD_RATE = 0.10;
56-
static constexpr double MAX_VALUE = 1.0;
57-
static constexpr double MIN_VALUE = -1.0;
58-
static constexpr double VALUE_WIDTH = MAX_VALUE - MIN_VALUE;
5966
static constexpr int64_t N_EI_CANDIDATES = 100;
60-
static constexpr double PRIOR_WEIGHT = 0.0;
6167

6268
static std::mt19937_64 engine;
63-
static std::uniform_real_distribution<double> dist_uniform;
64-
static std::normal_distribution<double> dist_normal;
6569

6670
double compute_log_likelihood_ratio(const Input & input) const;
6771
double log_gaussian_pdf(const Input & input, const Input & mu, const Input & sigma) const;
68-
static std::vector<double> get_weights(const int64_t n);
69-
static double normalize_loop_variable(const double value);
7072

7173
std::vector<Trial> trials_;
7274
int64_t above_num_;
7375
const Direction direction_;
7476
const int64_t n_startup_trials_;
7577
const int64_t input_dimension_;
76-
const std::vector<bool> is_loop_variable_;
77-
const Input base_stddev_;
78+
const std::vector<double> sample_mean_;
79+
const std::vector<double> sample_stddev_;
80+
Input base_stddev_;
7881
};
7982

8083
#endif // TREE_STRUCTURED_PARZEN_ESTIMATOR__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_

0 commit comments

Comments
 (0)