11# autoware_localization_util
22
33## Overview
4+
45` autoware_localization_util ` is a collection of localization utility packages. It contains 5 individual libiary that used by autoware localization nodes.
56
67- ` covariance_ellipse ` 2d covariance visualization wrapper.
910- ` util_func ` A tool library which contains several function for localization nodes.
1011
1112## Design
13+
1214- ` covariance_ellipse ` Translate ` geometry_msgs::msg::PoseWithCovariance ` message into ellipse visual marker to demonstrate covariance distribution.
1315- ` smart_pose_buffer ` A buffer library which implements pose message buffering, pose interpolate and pose validation.
1416- ` tree_structured_parzen_estimator ` A Probability Estimator (AKA TSPE) library that includes estimator and log likelihood ratio calculation.
1517- ` util_func ` Tool function collection.
1618
1719## Usage
18- ### covariance_ellipse
20+
21+ ### covariance_ellipse
22+
1923Include header file to use:
2024
2125``` cpp
2226#include " autoware/localization_util/covariance_ellipse.hpp"
2327```
2428
2529calculate ellipse and visualize
30+
2631``` cpp
2732autoware::localization_util::Ellipse ellipse_ = autoware::localization_util::calculate_xy_ellipse(input_msg->pose, scale_);
2833
@@ -33,6 +38,7 @@ autoware::localization_util::Ellipse ellipse_ = autoware::localization_util::cal
3338### smart_pose_buffer
3439
3540buffer init
41+
3642``` cpp
3743#include " autoware/localization_util/smart_pose_buffer.hpp"
3844
@@ -45,6 +51,7 @@ initial_pose_buffer_ = std::make_unique<SmartPoseBuffer>(
4551```
4652
4753interpolate and pop out old pose message
54+
4855``` cpp
4956std::optional<SmartPoseBuffer::InterpolateResult> interpolation_result_opt =
5057initial_pose_buffer_->interpolate (sensor_ros_time);
@@ -57,6 +64,7 @@ interpolation_result_opt.value();
5764```
5865
5966clear buffer
67+
6068```cpp
6169initial_pose_buffer_->clear();
6270```
@@ -77,11 +85,13 @@ param_.initial_pose_estimation.n_startup_trials, sample_mean, sample_stddev);
7785```
7886
7987get estimation result
88+
8089```cpp
8190const TreeStructuredParzenEstimator::Input input = tpe.get_next_input();
8291```
8392
8493add new data to the estimator
94+
8595``` cpp
8696TreeStructuredParzenEstimator::Input result (6);
8797 result[ 0] = pose.position.x;
@@ -96,6 +106,7 @@ tpe.add_trial(TreeStructuredParzenEstimator::Trial{result, ndt_result.transform_
96106### util_func
97107
98108include header file to use
109+
99110```cpp
100111#include "autoware/localization_util/util_func.hpp"
101112
@@ -106,6 +117,7 @@ using autoware::localization_util::pose_to_matrix4f;
106117```
107118
108119list of usefull function
120+
109121``` cpp
110122std_msgs::msg::ColorRGBA exchange_color_crc (double x);
111123double calc_diff_for_radian(const double lhs_rad, const double rhs_rad);
@@ -134,4 +146,4 @@ T transform(const T & input, const geometry_msgs::msg::TransformStamped & transf
134146void output_pose_with_cov_to_log(
135147 const rclcpp::Logger & logger, const std::string & prefix,
136148 const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov);
137- ```
149+ ```
0 commit comments