#include "autoware/localization_util/covariance_ellipse.hpp"

autoware::localization_util::Ellipse ellipse_ = autoware::localization_util::calculate_xy_ellipse(input_msg->pose, scale_);

  const auto ellipse_marker = autoware::localization_util::create_ellipse_marker(
    ellipse_, input_msg->header, input_msg->pose);

#include "autoware/localization_util/smart_pose_buffer.hpp"

using autoware::localization_util::SmartPoseBuffer;

std::unique_ptr<autoware::localization_util::SmartPoseBuffer> initial_pose_buffer_;
initial_pose_buffer_ = std::make_unique<SmartPoseBuffer>(
    this->get_logger(), param_.validation.initial_pose_timeout_sec,
    param_.validation.initial_pose_distance_tolerance_m);

std::optional<SmartPoseBuffer::InterpolateResult> interpolation_result_opt =
initial_pose_buffer_->interpolate(sensor_ros_time);

...

initial_pose_buffer_->pop_old(sensor_ros_time);
const SmartPoseBuffer::InterpolateResult & interpolation_result =
interpolation_result_opt.value();

initial_pose_buffer_->clear();

#include "autoware/localization_util/tree_structured_parzen_estimator.hpp"

using autoware::localization_util::TreeStructuredParzenEstimator;

TreeStructuredParzenEstimator tpe(
TreeStructuredParzenEstimator::Direction::MAXIMIZE,
param_.initial_pose_estimation.n_startup_trials, sample_mean, sample_stddev);

const TreeStructuredParzenEstimator::Input input = tpe.get_next_input();

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});

#include "autoware/localization_util/util_func.hpp"

using autoware::localization_util::exchange_color_crc;
using autoware::localization_util::matrix4f_to_pose;
using autoware::localization_util::point_to_vector3d;
using autoware::localization_util::pose_to_matrix4f;

std_msgs::msg::ColorRGBA exchange_color_crc(double x);
double calc_diff_for_radian(const double lhs_rad, const double rhs_rad);
geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose);
geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseStamped & pose);
geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovarianceStamped & pose);
geometry_msgs::msg::Quaternion rpy_rad_to_quaternion(
  const double r_rad, const double p_rad, const double y_rad);
geometry_msgs::msg::Quaternion rpy_deg_to_quaternion(
  const double r_deg, const double p_deg, const double y_deg);
geometry_msgs::msg::Twist calc_twist(
  const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b);
geometry_msgs::msg::PoseStamped interpolate_pose(
  const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b,
  const rclcpp::Time & time_stamp);
geometry_msgs::msg::PoseStamped interpolate_pose(
  const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a,
  const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp);
Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose);
Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose);
geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_matrix);
Eigen::Vector3d point_to_vector3d(const geometry_msgs::msg::Point & ros_pos);
template <class T>
T transform(const T & input, const geometry_msgs::msg::TransformStamped & transform);double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2);

void output_pose_with_cov_to_log(
  const rclcpp::Logger & logger, const std::string & prefix,
  const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov);