Skip to content

Commit

Permalink
fix(ndt_scan_matcher): improved tpe (autowarefoundation#6990)
Browse files Browse the repository at this point in the history 
* Improved tpe

Signed-off-by: Shintaro Sakoda <[email protected]>

* Added name in TODO

Signed-off-by: Shintaro Sakoda <[email protected]>

* Fixed tpe test

Signed-off-by: Shintaro Sakoda <[email protected]>

* style(pre-commit): autofix

---------

Signed-off-by: Shintaro Sakoda <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Signed-off-by: vividf <[email protected]>
  • Loading branch information
@SakodaShintaro @pre-commit-ci @vividf
2 people authored and vividf committed May 16, 2024
1 parent bc93a8e commit f5b1b94
Show file tree
Hide file tree
Showing 6 changed files with 117 additions and 174 deletions.
2 changes: 1 addition & 1 deletion localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@
# 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.
n_startup_trials: 20
n_startup_trials: 100


validation:
Expand Down
2 changes: 1 addition & 1 deletion localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@
"n_startup_trials": {
"type": "number",
"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.",
"default": 20,
"default": 100,
"minimum": 1
}
},
Expand Down
72 changes: 22 additions & 50 deletions localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,8 +22,6 @@
#include <tier4_autoware_utils/geometry/geometry.hpp>
#include <tier4_autoware_utils/transform/transforms.hpp>

#include <boost/math/special_functions/erf.hpp>

#include <pcl_conversions/pcl_conversions.h>

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

// Let phi be the cumulative distribution function of the standard normal distribution.
// It has the following relationship with the error function (erf).
// phi(x) = 1/2 (1 + erf(x / sqrt(2)))
// so, 2 * phi(x) - 1 = erf(x / sqrt(2)).
// The range taken by 2 * phi(x) - 1 is [-1, 1], so it can be used as a uniform distribution in
// TPE. Let u = 2 * phi(x) - 1, then x = sqrt(2) * erf_inv(u). Computationally, it is not a good
// to give erf_inv -1 and 1, so it is rounded off at (-1 + eps, 1 - eps).
const double sqrt2 = std::sqrt(2);
auto uniform_to_normal = [&sqrt2](const double uniform) {
assert(-1.0 <= uniform && uniform <= 1.0);
constexpr double epsilon = 1.0e-6;
const double clamped = std::clamp(uniform, -1.0 + epsilon, 1.0 - epsilon);
return boost::math::erf_inv(clamped) * sqrt2;
};

auto normal_to_uniform = [&sqrt2](const double normal) {
return boost::math::erf(normal / sqrt2);
// Since only yaw is uniformly sampled, we define the mean and standard deviation for the others.
const std::vector<double> sample_mean{
initial_pose_with_cov.pose.pose.position.x, // trans_x
initial_pose_with_cov.pose.pose.position.y, // trans_y
initial_pose_with_cov.pose.pose.position.z, // trans_z
base_rpy.x, // angle_x
base_rpy.y // angle_y
};
const std::vector<double> sample_stddev{stddev_x, stddev_y, stddev_z, stddev_roll, stddev_pitch};

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

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

geometry_msgs::msg::Pose initial_pose;
initial_pose.position.x =
initial_pose_with_cov.pose.pose.position.x + uniform_to_normal(input[0]) * stddev_x;
initial_pose.position.y =
initial_pose_with_cov.pose.pose.position.y + uniform_to_normal(input[1]) * stddev_y;
initial_pose.position.z =
initial_pose_with_cov.pose.pose.position.z + uniform_to_normal(input[2]) * stddev_z;
initial_pose.position.x = input[0];
initial_pose.position.y = input[1];
initial_pose.position.z = input[2];
geometry_msgs::msg::Vector3 init_rpy;
init_rpy.x = base_rpy.x + uniform_to_normal(input[3]) * stddev_roll;
init_rpy.y = base_rpy.y + uniform_to_normal(input[4]) * stddev_pitch;
init_rpy.z = base_rpy.z + input[5] * M_PI;
init_rpy.x = input[3];
init_rpy.y = input[4];
init_rpy.z = input[5];
tf2::Quaternion tf_quaternion;
tf_quaternion.setRPY(init_rpy.x, init_rpy.y, init_rpy.z);
initial_pose.orientation = tf2::toMsg(tf_quaternion);
Expand All @@ -1061,22 +1042,13 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose(
const geometry_msgs::msg::Pose pose = matrix4f_to_pose(ndt_result.pose);
const geometry_msgs::msg::Vector3 rpy = get_rpy(pose);

const double diff_x = pose.position.x - initial_pose_with_cov.pose.pose.position.x;
const double diff_y = pose.position.y - initial_pose_with_cov.pose.pose.position.y;
const double diff_z = pose.position.z - initial_pose_with_cov.pose.pose.position.z;
const double diff_roll = rpy.x - base_rpy.x;
const double diff_pitch = rpy.y - base_rpy.y;
const double diff_yaw = rpy.z - base_rpy.z;

// Only yaw is a loop_variable, so only simple normalization is performed.
// All other variables are converted from normal distribution to uniform distribution.
TreeStructuredParzenEstimator::Input result(is_loop_variable.size());
result[0] = normal_to_uniform(diff_x / stddev_x);
result[1] = normal_to_uniform(diff_y / stddev_y);
result[2] = normal_to_uniform(diff_z / stddev_z);
result[3] = normal_to_uniform(diff_roll / stddev_roll);
result[4] = normal_to_uniform(diff_pitch / stddev_pitch);
result[5] = diff_yaw / M_PI;
TreeStructuredParzenEstimator::Input result(6);
result[0] = pose.position.x;
result[1] = pose.position.y;
result[2] = pose.position.z;
result[3] = rpy.x;
result[4] = rpy.y;
result[5] = rpy.z;
tpe.add_trial(TreeStructuredParzenEstimator::Trial{result, ndt_result.transform_probability});

auto sensor_points_in_map_ptr = std::make_shared<pcl::PointCloud<PointSource>>();
Expand Down
27 changes: 15 additions & 12 deletions ...n_estimator/include/tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,37 +44,40 @@ class TreeStructuredParzenEstimator
MAXIMIZE = 1,
};

enum Index {
TRANS_X = 0,
TRANS_Y = 1,
TRANS_Z = 2,
ANGLE_X = 3,
ANGLE_Y = 4,
ANGLE_Z = 5,
INDEX_NUM = 6,
};

TreeStructuredParzenEstimator() = delete;
TreeStructuredParzenEstimator(
const Direction direction, const int64_t n_startup_trials, std::vector<bool> is_loop_variable);
const Direction direction, const int64_t n_startup_trials,
const std::vector<double> & sample_mean, const std::vector<double> & sample_stddev);
void add_trial(const Trial & trial);
Input get_next_input() const;

private:
static constexpr double BASE_STDDEV_COEFF = 0.2;
static constexpr double MAX_GOOD_RATE = 0.10;
static constexpr double MAX_VALUE = 1.0;
static constexpr double MIN_VALUE = -1.0;
static constexpr double VALUE_WIDTH = MAX_VALUE - MIN_VALUE;
static constexpr int64_t N_EI_CANDIDATES = 100;
static constexpr double PRIOR_WEIGHT = 0.0;

static std::mt19937_64 engine;
static std::uniform_real_distribution<double> dist_uniform;
static std::normal_distribution<double> dist_normal;

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

std::vector<Trial> trials_;
int64_t above_num_;
const Direction direction_;
const int64_t n_startup_trials_;
const int64_t input_dimension_;
const std::vector<bool> is_loop_variable_;
const Input base_stddev_;
const std::vector<double> sample_mean_;
const std::vector<double> sample_stddev_;
Input base_stddev_;
};

#endif // TREE_STRUCTURED_PARZEN_ESTIMATOR__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_
Loading

0 comments on commit f5b1b94

Please sign in to comment.