From bb74bf4611829dfd7f2198e6ea0bf6e3958e9377 Mon Sep 17 00:00:00 2001 From: a-maumau Date: Mon, 17 Jun 2024 19:00:29 +0900 Subject: [PATCH] removed unused Signed-off-by: a-maumau --- .../camera_particle_corrector.hpp | 2 +- .../camera_corrector/logit.hpp | 9 --- .../yabloc_particle_filter/common/mean.hpp | 9 +-- .../common/prediction_util.hpp | 15 ++-- .../common/visualize.hpp | 8 +- .../correction/abstract_corrector.hpp | 7 +- .../correction/correction_util.hpp | 7 +- .../gnss_corrector/weight_manager.hpp | 44 ++++++----- .../ll2_cost_map/direct_cost_map.hpp | 3 - .../ll2_cost_map/hierarchical_cost_map.hpp | 28 +++---- .../prediction/predictor.hpp | 2 +- .../prediction/resampler.hpp | 9 ++- .../prediction/resampling_history.hpp | 2 +- .../camera_particle_corrector_core.cpp | 40 ++++++---- .../camera_corrector/filter_line_segments.cpp | 29 +++---- .../src/camera_corrector/logit.cpp | 9 +-- .../src/common/mean.cpp | 37 +++------ .../src/common/particle_visualize_node.cpp | 30 +------ .../src/common/visualize.cpp | 4 +- .../src/correction/abstract_corrector.cpp | 2 +- .../src/correction/correction_util.cpp | 19 ----- .../gnss_corrector/gnss_corrector_core.cpp | 29 +++---- .../src/ll2_cost_map/direct_cost_map.cpp | 28 ++----- .../ll2_cost_map/hierarchical_cost_map.cpp | 78 +++++++++++-------- .../src/prediction/predictor.cpp | 43 +++++----- .../src/prediction/resampler.cpp | 6 +- .../test/src/test_resampler.cpp | 75 +++++++++--------- 27 files changed, 252 insertions(+), 322 deletions(-) diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp index 582abb03901a1..41f10f12c4ef3 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp @@ -33,7 +33,7 @@ namespace yabloc::modularized_particle_filter { -cv::Point2f cv2pt(const Eigen::Vector3f v); +cv::Point2f cv2pt(const Eigen::Vector3f& v); float abs_cos(const Eigen::Vector3f & t, float deg); class CameraParticleCorrector : public modularized_particle_filter::AbstractCorrector diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/logit.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/logit.hpp index caf93abd0028f..0b2954dd46ba5 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/logit.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/logit.hpp @@ -18,15 +18,6 @@ namespace yabloc { float logit_to_prob(float logit, float gain = 1.0f); - -/** - * Convert probability to logit - * This function is much faster than logit_to_prob() because it refers to pre-computed table - * - * @param[in] prob - * @return logit - */ -float prob_to_logit(float prob); } // namespace yabloc #endif // YABLOC_PARTICLE_FILTER__CAMERA_CORRECTOR__LOGIT_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/mean.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/mean.hpp index e6af4f0d76d49..4e2c6cd8a842c 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/mean.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/mean.hpp @@ -20,18 +20,13 @@ #include -namespace yabloc -{ -namespace modularized_particle_filter +namespace yabloc::modularized_particle_filter { geometry_msgs::msg::Pose get_mean_pose( const yabloc_particle_filter::msg::ParticleArray & particle_array); Eigen::Matrix3f std_of_distribution( const yabloc_particle_filter::msg::ParticleArray & particle_array); - -float std_of_weight(const yabloc_particle_filter::msg::ParticleArray & particle_array); -} // namespace modularized_particle_filter -} // namespace yabloc +} // namespace yabloc::modularized_particle_filter #endif // YABLOC_PARTICLE_FILTER__COMMON__MEAN_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/prediction_util.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/prediction_util.hpp index 6059f16826c98..ab001d58eddf3 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/prediction_util.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/prediction_util.hpp @@ -22,14 +22,12 @@ #include #include -namespace yabloc +namespace yabloc::modularized_particle_filter::util { -namespace modularized_particle_filter::util -{ -std::random_device seed_gen; -std::default_random_engine engine(seed_gen()); +inline std::random_device seed_gen; +inline std::default_random_engine engine(seed_gen()); -Eigen::Vector2d nrand_2d(const Eigen::Matrix2d cov) +inline Eigen::Vector2d nrand_2d(const Eigen::Matrix2d& cov) { Eigen::JacobiSVD svd; svd.compute(cov, Eigen::ComputeFullU | Eigen::ComputeFullV); @@ -50,7 +48,7 @@ T nrand(T std) return dist(engine); } -double normalize_radian(const double rad, const double min_rad = -M_PI) +inline double normalize_radian(const double rad, const double min_rad = -M_PI) { const auto max_rad = min_rad + 2 * M_PI; @@ -63,6 +61,5 @@ double normalize_radian(const double rad, const double min_rad = -M_PI) return value - std::copysign(2 * M_PI, value); } -} // namespace modularized_particle_filter::util -} // namespace yabloc +} // namespace yabloc::modularized_particle_filter::util #endif // YABLOC_PARTICLE_FILTER__COMMON__PREDICTION_UTIL_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/visualize.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/visualize.hpp index f1ecf35e81761..3b35d1a9a0da6 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/visualize.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/visualize.hpp @@ -21,9 +21,7 @@ #include #include -namespace yabloc -{ -namespace modularized_particle_filter +namespace yabloc::modularized_particle_filter { class ParticleVisualizer { @@ -37,9 +35,7 @@ class ParticleVisualizer private: rclcpp::Publisher::SharedPtr pub_marker_array_; - std_msgs::msg::ColorRGBA compute_color(float value); }; -} // namespace modularized_particle_filter -} // namespace yabloc +} // namespace yabloc::modularized_particle_filter #endif // YABLOC_PARTICLE_FILTER__COMMON__VISUALIZE_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/abstract_corrector.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/abstract_corrector.hpp index 4deaeec0810ae..33802bd666440 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/abstract_corrector.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/abstract_corrector.hpp @@ -26,9 +26,7 @@ #include #include -namespace yabloc -{ -namespace modularized_particle_filter +namespace yabloc::modularized_particle_filter { class AbstractCorrector : public rclcpp::Node { @@ -55,7 +53,6 @@ class AbstractCorrector : public rclcpp::Node private: void on_particle_array(const ParticleArray & particle_array); }; -} // namespace modularized_particle_filter -} // namespace yabloc +} // namespace yabloc::modularized_particle_filter #endif // YABLOC_PARTICLE_FILTER__CORRECTION__ABSTRACT_CORRECTOR_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/correction_util.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/correction_util.hpp index 2c2f10bf7ad2e..29f4791c56dc6 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/correction_util.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/correction_util.hpp @@ -22,14 +22,11 @@ #include -namespace yabloc -{ -namespace modularized_particle_filter +namespace yabloc::modularized_particle_filter { std::optional find_synced_particles( boost::circular_buffer circular_buffer, rclcpp::Time time); -} -} // namespace yabloc +} // namespace yabloc::modularized_particle_filter #endif // YABLOC_PARTICLE_FILTER__CORRECTION__CORRECTION_UTIL_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector/weight_manager.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector/weight_manager.hpp index 791f65eb9b835..3a857fd52f76b 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector/weight_manager.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector/weight_manager.hpp @@ -35,41 +35,47 @@ struct WeightManager } }; - Parameter for_fixed_; - Parameter for_not_fixed_; + Parameter for_fixed_{}; + Parameter for_not_fixed_{}; explicit WeightManager(rclcpp::Node * node) { - for_fixed_.flat_radius_ = node->declare_parameter("for_fixed/flat_radius"); - for_fixed_.max_radius_ = node->declare_parameter("for_fixed/max_radius"); - for_fixed_.min_weight_ = node->declare_parameter("for_fixed/min_weight"); - for_fixed_.max_weight_ = node->declare_parameter("for_fixed/max_weight"); + for_fixed_.flat_radius_ = + static_cast(node->declare_parameter("for_fixed/flat_radius")); + for_fixed_.max_radius_ = + static_cast(node->declare_parameter("for_fixed/max_radius")); + for_fixed_.min_weight_ = + static_cast(node->declare_parameter("for_fixed/min_weight")); + for_fixed_.max_weight_ = + static_cast(node->declare_parameter("for_fixed/max_weight")); for_fixed_.compute_coeff(); - for_not_fixed_.flat_radius_ = node->declare_parameter("for_not_fixed/flat_radius"); - for_not_fixed_.max_radius_ = node->declare_parameter("for_not_fixed/max_radius"); - for_not_fixed_.min_weight_ = node->declare_parameter("for_not_fixed/min_weight"); - for_not_fixed_.max_weight_ = node->declare_parameter("for_not_fixed/max_weight"); + for_not_fixed_.flat_radius_ = + static_cast(node->declare_parameter("for_not_fixed/flat_radius")); + for_not_fixed_.max_radius_ = + static_cast(node->declare_parameter("for_not_fixed/max_radius")); + for_not_fixed_.min_weight_ = + static_cast(node->declare_parameter("for_not_fixed/min_weight")); + for_not_fixed_.max_weight_ = + static_cast(node->declare_parameter("for_not_fixed/max_weight")); for_not_fixed_.compute_coeff(); } - float normal_pdf(float distance, const Parameter & param) const + [[nodiscard]] static float normal_pdf(float distance, const Parameter & param) { // NOTE: This is not exact normal distribution because of no scale factor depending on sigma float d = std::clamp(std::abs(distance) - param.flat_radius_, 0.f, param.max_radius_); return param.max_weight_ * std::exp(-param.coeff_ * d * d); } - float normal_pdf(float distance, bool is_rtk_fixed) const + [[nodiscard]] float normal_pdf(float distance, bool is_rtk_fixed) const { if (is_rtk_fixed) { return normal_pdf(distance, for_fixed_); - } else { - return normal_pdf(distance, for_not_fixed_); - } + } return normal_pdf(distance, for_not_fixed_); } - float inverse_normal_pdf(float prob, const Parameter & param) const + [[nodiscard]] static float inverse_normal_pdf(float prob, const Parameter & param) { prob = (param.max_weight_ - param.min_weight_) * prob + param.min_weight_; @@ -78,13 +84,11 @@ struct WeightManager return param.flat_radius_ + std::sqrt(-std::log(prob / param.max_weight_) / param.coeff_); } - float inverse_normal_pdf(float prob, bool is_rtk_fixed) const + [[nodiscard]] float inverse_normal_pdf(float prob, bool is_rtk_fixed) const { if (is_rtk_fixed) { return inverse_normal_pdf(prob, for_fixed_); - } else { - return inverse_normal_pdf(prob, for_not_fixed_); - } + } return inverse_normal_pdf(prob, for_not_fixed_); } }; } // namespace yabloc::modularized_particle_filter diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/direct_cost_map.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/direct_cost_map.hpp index 5f56b8f079cd1..8169719a21f3b 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/direct_cost_map.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/direct_cost_map.hpp @@ -20,9 +20,6 @@ namespace yabloc { cv::Mat direct_cost_map(const cv::Mat & cost_map, const cv::Mat & intensity); - -cv::Mat visualize_direction_map(const cv::Mat & cost_map); - } // namespace yabloc #endif // YABLOC_PARTICLE_FILTER__LL2_COST_MAP__DIRECT_COST_MAP_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/hierarchical_cost_map.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/hierarchical_cost_map.hpp index 32d579c4ab595..6c3a753f8d7f7 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/hierarchical_cost_map.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/hierarchical_cost_map.hpp @@ -37,34 +37,36 @@ namespace yabloc { struct Area { - Area() {} + Area() = default; explicit Area(const Eigen::Vector2f & v) { - if (unit_length_ < 0) { + if (unit_length < 0) { throw_error(); } - x = static_cast(std::floor(v.x() / unit_length_)); - y = static_cast(std::floor(v.y() / unit_length_)); + x = static_cast(std::floor(v.x() / unit_length)); + y = static_cast(std::floor(v.y() / unit_length)); } - Eigen::Vector2f real_scale() const { return {x * unit_length_, y * unit_length_}; } + [[nodiscard]] Eigen::Vector2f real_scale() const { return { + static_cast(x) * unit_length, static_cast(y) * unit_length}; + } - std::array real_scale_boundary() const + [[nodiscard]] std::array real_scale_boundary() const { std::array boundary; boundary.at(0) = real_scale(); - boundary.at(1) = real_scale() + Eigen::Vector2f(unit_length_, unit_length_); + boundary.at(1) = real_scale() + Eigen::Vector2f(unit_length, unit_length); return boundary; }; - void throw_error() const + static void throw_error() { std::cerr << "Area::unit_length_ is not initialized" << std::endl; throw std::runtime_error("invalid Area::unit_length"); } - int x, y; - static float unit_length_; - static float image_size_; + int x{}, y{}; + static float unit_length; + static float image_size; friend bool operator==(const Area & one, const Area & other) { @@ -129,7 +131,7 @@ class HierarchicalCostMap rclcpp::Logger logger_; std::optional height_{std::nullopt}; - common::GammaConverter gamma_converter{4.0f}; + common::GammaConverter gamma_converter_{4.0f}; std::unordered_map map_accessed_; @@ -138,7 +140,7 @@ class HierarchicalCostMap std::vector bounding_boxes_; std::unordered_map cost_maps_; - cv::Point to_cv_point(const Area & are, const Eigen::Vector2f) const; + cv::Point to_cv_point(const Area & area, const Eigen::Vector2f & p) const; void build_map(const Area & area); cv::Mat create_available_area_image(const Area & area) const; diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp index e4fb58f22e17f..b619ed524affd 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp @@ -106,7 +106,7 @@ class Predictor : public rclcpp::Node void initialize_particles(const PoseCovStamped & initialpose); // void update_with_dynamic_noise( - ParticleArray & particle_array, const TwistCovStamped & twist, double dt); + ParticleArray & particle_array, const TwistCovStamped & twist, double dt) const; // void publish_mean_pose(const geometry_msgs::msg::Pose & mean_pose, const rclcpp::Time & stamp); void publish_range_marker(const Eigen::Vector3f & pos, const Eigen::Vector3f & tangent); diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampler.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampler.hpp index f2a517216020a..f35b226054dac 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampler.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampler.hpp @@ -22,10 +22,10 @@ namespace yabloc::modularized_particle_filter { -class resampling_skip_exception : public std::runtime_error +class ResamplingSkipException : public std::runtime_error { public: - explicit resampling_skip_exception(const char * message) : runtime_error(message) {} + explicit ResamplingSkipException(const char * message) : runtime_error(message) {} }; class RetroactiveResampler @@ -56,9 +56,10 @@ class RetroactiveResampler int latest_resampling_generation_; // Random generator from 0 to 1 - double random_from_01_uniformly() const; + [[nodiscard]] static double random_from_01_uniformly() ; // Check the sanity of the particles obtained from the particle corrector. - bool check_weighted_particles_validity(const ParticleArray & weighted_particles) const; + [[nodiscard]] bool check_weighted_particles_validity( + const ParticleArray & weighted_particles) const; }; } // namespace yabloc::modularized_particle_filter diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampling_history.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampling_history.hpp index 5b24ee96d6df5..6c1e9737c3f0d 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampling_history.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampling_history.hpp @@ -35,7 +35,7 @@ class ResamplingHistory } } - bool check_history_validity() const + [[nodiscard]] bool check_history_validity() const { for (auto & generation : resampling_history_) { bool result = std::any_of(generation.begin(), generation.end(), [this](int x) { diff --git a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp index 6efc65cd01f60..d5ada69dc4ad8 100644 --- a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp @@ -15,6 +15,7 @@ #include "yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp" #include "yabloc_particle_filter/camera_corrector/logit.hpp" +#include #include #include #include @@ -30,8 +31,8 @@ namespace yabloc::modularized_particle_filter CameraParticleCorrector::CameraParticleCorrector(const rclcpp::NodeOptions & options) : AbstractCorrector("camera_particle_corrector", options), - min_prob_(declare_parameter("min_prob")), - far_weight_gain_(declare_parameter("far_weight_gain")), + min_prob_(static_cast(declare_parameter("min_prob"))), + far_weight_gain_(static_cast(declare_parameter("far_weight_gain"))), cost_map_(this) { using std::placeholders::_1; @@ -98,7 +99,8 @@ CameraParticleCorrector::split_line_segments(const PointCloud2 & msg) { LineSegments all_line_segments_cloud; pcl::fromROSMsg(msg, all_line_segments_cloud); - LineSegments reliable_cloud, iffy_cloud; + LineSegments reliable_cloud; + LineSegments iffy_cloud; { for (const auto & p : all_line_segments_cloud) { if (p.label == 0) @@ -111,7 +113,7 @@ CameraParticleCorrector::split_line_segments(const PointCloud2 & msg) auto [good_cloud, bad_cloud] = filt(iffy_cloud); { cv::Mat debug_image = cv::Mat::zeros(800, 800, CV_8UC3); - auto draw = [&debug_image](LineSegments & cloud, cv::Scalar color) -> void { + auto draw = [&debug_image](const LineSegments & cloud, const cv::Scalar& color) -> void { for (const auto & line : cloud) { const Eigen::Vector3f p1 = line.getVector3fMap(); const Eigen::Vector3f p2 = line.getNormalVector3fMap(); @@ -162,7 +164,7 @@ void CameraParticleCorrector::on_line_segments(const PointCloud2 & line_segments } } - cost_map_.set_height(mean_pose.position.z); + cost_map_.set_height(static_cast(mean_pose.position.z)); if (publish_weighted_particles) { for (auto & particle : weighted_particles.particles) { @@ -187,8 +189,8 @@ void CameraParticleCorrector::on_line_segments(const PointCloud2 & line_segments // DEBUG: just visualization { - Pose mean_pose = get_mean_pose(weighted_particles); - Sophus::SE3f transform = common::pose_to_se3(mean_pose); + Pose tmp_mean_pose = get_mean_pose(weighted_particles); + Sophus::SE3f transform = common::pose_to_se3(tmp_mean_pose); pcl::PointCloud cloud = evaluate_cloud( common::transform_line_segments(line_segments_cloud, transform), transform.translation()); @@ -199,10 +201,9 @@ void CameraParticleCorrector::on_line_segments(const PointCloud2 & line_segments pcl::PointCloud rgb_cloud; pcl::PointCloud rgb_iffy_cloud; - float max_score = 0; - for (const auto p : cloud) { - max_score = std::max(max_score, std::abs(p.intensity)); - } + float max_score = std::accumulate(cloud.begin(), cloud.end(), 0.0f, + [](float max_score, const auto& p) { return std::max(max_score, std::abs(p.intensity)); } + ); for (const auto p : cloud) { pcl::PointXYZRGB rgb; rgb.getVector3fMap() = p.getVector3fMap(); @@ -256,7 +257,7 @@ void CameraParticleCorrector::on_ll2(const PointCloud2 & ll2_msg) float abs_cos(const Eigen::Vector3f & t, float deg) { - const float radian = deg * M_PI / 180.0; + const auto radian = static_cast(deg * M_PI / 180.0); Eigen::Vector2f x(t.x(), t.y()); Eigen::Vector2f y(tier4_autoware_utils::cos(radian), tier4_autoware_utils::sin(radian)); x.normalize(); @@ -276,7 +277,7 @@ float CameraParticleCorrector::compute_logit( // NOTE: Close points are prioritized float squared_norm = (p - self_position).topRows(2).squaredNorm(); - float gain = exp(-far_weight_gain_ * squared_norm); // 0 < gain < 1 + float gain = std::exp(-far_weight_gain_ * squared_norm); // 0 < gain < 1 const CostMapValue v3 = cost_map_.at(p.topRows(2)); @@ -285,9 +286,10 @@ float CameraParticleCorrector::compute_logit( continue; } if (pn.label == 0) { // posteriori - logit += 0.2f * gain * (abs_cos(tangent, v3.angle) * v3.intensity - 0.5f); + logit += + 0.2f * gain * (abs_cos(tangent, static_cast(v3.angle)) * v3.intensity - 0.5f); } else { // apriori - logit += gain * (abs_cos(tangent, v3.angle) * v3.intensity - 0.5f); + logit += gain * (abs_cos(tangent, static_cast(v3.angle)) * v3.intensity - 0.5f); } } } @@ -307,11 +309,15 @@ pcl::PointCloud CameraParticleCorrector::evaluate_cloud( // NOTE: Close points are prioritized float squared_norm = (p - self_position).topRows(2).squaredNorm(); - float gain = std::exp(-far_weight_gain_ * squared_norm); CostMapValue v3 = cost_map_.at(p.topRows(2)); float logit = 0; - if (!v3.unmapped) logit = gain * (abs_cos(tangent, v3.angle) * v3.intensity - 0.5f); + if (!v3.unmapped){ + float gain = std::exp(-far_weight_gain_ * squared_norm); + + logit = + gain * (abs_cos(tangent, static_cast(v3.angle)) * v3.intensity - 0.5f); + } pcl::PointXYZI xyzi(logit_to_prob(logit, 10.f)); xyzi.getVector3fMap() = p; diff --git a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/filter_line_segments.cpp b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/filter_line_segments.cpp index 3f782d85aec43..64cc020c1e9ca 100644 --- a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/filter_line_segments.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/filter_line_segments.cpp @@ -22,33 +22,34 @@ namespace yabloc::modularized_particle_filter { -cv::Point2f cv2pt(const Eigen::Vector3f v) +cv::Point2f cv2pt(const Eigen::Vector3f& v) { - const float METRIC_PER_PIXEL = 0.05; - const float IMAGE_RADIUS = 400; - return {-v.y() / METRIC_PER_PIXEL + IMAGE_RADIUS, -v.x() / METRIC_PER_PIXEL + 2 * IMAGE_RADIUS}; + const float metric_per_pixel = 0.05; + const float image_radius = 400; + return {-v.y() / metric_per_pixel + image_radius, -v.x() / metric_per_pixel + 2 * image_radius}; } float normalized_atan2(const Eigen::Vector3f & t, float deg) { - float diff = std::atan2(t.y(), t.x()) - deg * M_PI / 180; - diff = std::fmod(diff, M_PI); + auto diff = static_cast(std::atan2(t.y(), t.x()) - deg * M_PI / 180); + diff = static_cast(std::fmod(diff, M_PI)); if (diff < 0) diff = -diff; if (diff < M_PI_2) { - return 1 - diff / M_PI_2; - } else if (diff < M_PI) { - return diff / M_PI_2 - 1; - } else { - throw std::runtime_error("invalid cos"); + return static_cast(1.0 - diff / M_PI_2); } + + if (diff < M_PI) { + return static_cast(diff / M_PI_2 - 1.0); + } throw std::runtime_error("invalid cos"); } std::pair CameraParticleCorrector::filt(const LineSegments & iffy_lines) { - LineSegments good, bad; + LineSegments good; + LineSegments bad; if (!latest_pose_.has_value()) { throw std::runtime_error("latest_pose_ is nullopt"); } @@ -67,7 +68,7 @@ CameraParticleCorrector::filt(const LineSegments & iffy_lines) for (float distance = 0; distance < length; distance += 0.1f) { Eigen::Vector3f px = pose * (p2 + tangent * distance); CostMapValue v3 = cost_map_.at(px.topRows(2)); - float cos2 = normalized_atan2(pose.so3() * tangent, v3.angle); + float cos2 = normalized_atan2(pose.so3() * tangent, static_cast(v3.angle)); score += (cos2 * v3.intensity); count++; @@ -77,7 +78,7 @@ CameraParticleCorrector::filt(const LineSegments & iffy_lines) // rgb_cloud.push_back(rgb); } - if (score / count > 0.5f) { + if (score / static_cast(count) > 0.5f) { good.push_back(line); } else { bad.push_back(line); diff --git a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/logit.cpp b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/logit.cpp index d8bb5690b6fc4..65b7039e25051 100644 --- a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/logit.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/logit.cpp @@ -27,7 +27,7 @@ struct ProbToLogitTable ProbToLogitTable() { for (int i = 0; i < 100; ++i) { - float p = i / 100.0f; + float p = static_cast(i) / 100.0f; table_.at(i) = std::log(p / std::max(1 - p, 1e-6f)); } } @@ -37,7 +37,7 @@ struct ProbToLogitTable return table_.at(index); } - std::array table_; + std::array table_{}; } prob_to_logit_table; } // namespace @@ -47,9 +47,4 @@ float logit_to_prob(float logit, float gain) return 1.f / (1 + std::exp(-gain * logit)); } -float prob_to_logit(float prob) -{ - return prob_to_logit_table(prob); -} - } // namespace yabloc diff --git a/localization/yabloc/yabloc_particle_filter/src/common/mean.cpp b/localization/yabloc/yabloc_particle_filter/src/common/mean.cpp index 0710c8c4dca64..b40d134a5c37e 100644 --- a/localization/yabloc/yabloc_particle_filter/src/common/mean.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/common/mean.cpp @@ -70,7 +70,9 @@ geometry_msgs::msg::Pose get_mean_pose( mean_pose.position.y += particle.pose.position.y * normalized_weight; mean_pose.position.z += particle.pose.position.z * normalized_weight; - double yaw{0.0}, pitch{0.0}, roll{0.0}; + double yaw{0.0}; + double pitch{0.0}; + double roll{0.0}; tf2::getEulerYPR(particle.pose.orientation, yaw, pitch, roll); rolls.push_back(roll); @@ -93,43 +95,28 @@ Eigen::Matrix3f std_of_distribution(const yabloc_particle_filter::msg::ParticleA { using Particle = yabloc_particle_filter::msg::Particle; auto ori = get_mean_pose(array).orientation; - Eigen::Quaternionf orientation(ori.w, ori.x, ori.y, ori.z); - float invN = 1.f / array.particles.size(); + Eigen::Quaternionf orientation( + static_cast(ori.w), + static_cast(ori.x), + static_cast(ori.y), + static_cast(ori.z) + ); + float inv_n = 1.f / static_cast(array.particles.size()); Eigen::Vector3f mean = Eigen::Vector3f::Zero(); for (const Particle & p : array.particles) { Eigen::Affine3f affine = common::pose_to_affine(p.pose); mean += affine.translation(); } - mean *= invN; + mean *= inv_n; Eigen::Matrix3f sigma = Eigen::Matrix3f::Zero(); for (const Particle & p : array.particles) { Eigen::Affine3f affine = common::pose_to_affine(p.pose); Eigen::Vector3f d = affine.translation() - mean; d = orientation.conjugate() * d; - sigma += (d * d.transpose()) * invN; + sigma += (d * d.transpose()) * inv_n; } return sigma; } - -float std_of_weight(const yabloc_particle_filter::msg::ParticleArray & particle_array) -{ - using Particle = yabloc_particle_filter::msg::Particle; - - const float invN = 1.f / particle_array.particles.size(); - float mean = 0; - for (const Particle & p : particle_array.particles) { - mean += p.weight; - } - mean *= invN; - - float sigma = 0.0; - for (const Particle & p : particle_array.particles) { - sigma += (p.weight - mean) * (p.weight - mean); - } - sigma *= invN; - - return std::sqrt(sigma); -} } // namespace yabloc::modularized_particle_filter diff --git a/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp b/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp index b63e87b9462f1..48f2041595a77 100644 --- a/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp @@ -39,36 +39,12 @@ class ParticleVisualize : public rclcpp::Node "/particle_array", 10, std::bind(&ParticleVisualize::on_particles, this, _1)); // Publisher - pub_marker_array = this->create_publisher("/marker_array", 10); + pub_marker_array_ = this->create_publisher("/marker_array", 10); } private: rclcpp::Subscription::SharedPtr sub_particles_; - rclcpp::Publisher::SharedPtr pub_marker_array; - - std_msgs::msg::ColorRGBA compute_color(float value) const - { - float r = 1.0f, g = 1.0f, b = 1.0f; - // clang-format off - value = std::clamp(value, 0.0f, 1.0f); - if (value < 0.25f) { - r = 0; g = 4 * (value); - } else if (value < 0.5f) { - r = 0; b = 1 + 4 * (0.25f - value); - } else if (value < 0.75f) { - r = 4 * (value - 0.5f); b = 0; - } else { - g = 1 + 4 * (0.75f - value); b = 0; - } - // clang-format on - - std_msgs::msg::ColorRGBA rgba; - rgba.r = r; - rgba.g = g; - rgba.b = b; - rgba.a = 1.0f; - return rgba; - } + rclcpp::Publisher::SharedPtr pub_marker_array_; void on_particles(const ParticleArray & msg) { @@ -102,7 +78,7 @@ class ParticleVisualize : public rclcpp::Node marker.pose.position.z = p.pose.position.z; marker_array.markers.push_back(marker); } - pub_marker_array->publish(marker_array); + pub_marker_array_->publish(marker_array); } }; } // namespace yabloc::modularized_particle_filter diff --git a/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp b/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp index 888baf0bea0ac..17b22757b4bc5 100644 --- a/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp @@ -33,7 +33,7 @@ void ParticleVisualizer::publish(const ParticleArray & msg) float min = minmax_weight.first->weight; float max = minmax_weight.second->weight; max = std::max(max, min + 1e-7f); - auto boundWeight = [min, max](float raw) -> float { return (raw - min) / (max - min); }; + auto bound_weight = [min, max](float raw) -> float { return (raw - min) / (max - min); }; int id = 0; for (const Particle & p : msg.particles) { @@ -46,7 +46,7 @@ void ParticleVisualizer::publish(const ParticleArray & msg) marker.scale.y = 0.1; marker.scale.z = 0.1; marker.color = - static_cast(common::color_scale::rainbow(boundWeight(p.weight))); + static_cast(common::color_scale::rainbow(bound_weight(p.weight))); marker.pose.orientation = p.pose.orientation; marker.pose.position.x = p.pose.position.x; marker.pose.position.y = p.pose.position.y; diff --git a/localization/yabloc/yabloc_particle_filter/src/correction/abstract_corrector.cpp b/localization/yabloc/yabloc_particle_filter/src/correction/abstract_corrector.cpp index 5221256c9e392..1c8e2f5976a1d 100644 --- a/localization/yabloc/yabloc_particle_filter/src/correction/abstract_corrector.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/correction/abstract_corrector.cpp @@ -19,7 +19,7 @@ namespace yabloc::modularized_particle_filter AbstractCorrector::AbstractCorrector( const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options), - acceptable_max_delay_(declare_parameter("acceptable_max_delay")), + acceptable_max_delay_(static_cast(declare_parameter("acceptable_max_delay"))), visualize_(declare_parameter("visualize")), logger_(rclcpp::get_logger("abstract_corrector")) { diff --git a/localization/yabloc/yabloc_particle_filter/src/correction/correction_util.cpp b/localization/yabloc/yabloc_particle_filter/src/correction/correction_util.cpp index 67ca4f5add947..4e3b3167a9123 100644 --- a/localization/yabloc/yabloc_particle_filter/src/correction/correction_util.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/correction/correction_util.cpp @@ -16,23 +16,4 @@ namespace yabloc::modularized_particle_filter { -std::optional find_synced_particles( - boost::circular_buffer circular_buffer, - rclcpp::Time time) -{ - for (int i{1}; i < static_cast(circular_buffer.size()); i++) { - if (rclcpp::Time(circular_buffer[i].header.stamp) < time) { - return circular_buffer[i]; - } - } - if (0 < static_cast(circular_buffer.size())) { - RCLCPP_WARN_STREAM( - rclcpp::get_logger("modularized_particle_filter.correction_util"), - "the sensor data is too old: " - << rclcpp::Time(circular_buffer[static_cast(circular_buffer.size()) - 1].header.stamp) - .seconds() - - time.seconds()); - } - return std::nullopt; -} } // namespace yabloc::modularized_particle_filter diff --git a/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp b/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp index f1dd22426a07d..3e63dbf2385ee 100644 --- a/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp @@ -21,7 +21,8 @@ namespace yabloc::modularized_particle_filter { GnssParticleCorrector::GnssParticleCorrector(const rclcpp::NodeOptions & options) : AbstractCorrector("gnss_particle_corrector", options), - mahalanobis_distance_threshold_(declare_parameter("mahalanobis_distance_threshold")), + mahalanobis_distance_threshold_( + static_cast(declare_parameter("mahalanobis_distance_threshold"))), weight_manager_(this) { using std::placeholders::_1; @@ -61,7 +62,9 @@ void GnssParticleCorrector::on_pose(const PoseCovStamped::ConstSharedPtr pose_ms const rclcpp::Time stamp = pose_msg->header.stamp; const auto & position = pose_msg->pose.pose.position; Eigen::Vector3f gnss_position; - gnss_position << position.x, position.y, position.z; + gnss_position << static_cast(position.x), + static_cast(position.y), + static_cast(position.z); constexpr bool is_rtk_fixed = false; process(gnss_position, stamp, is_rtk_fixed); @@ -96,10 +99,10 @@ void GnssParticleCorrector::process( // Compute travel distance from last update position // If the distance is too short, skip weighting { - Eigen::Vector3f mean_position = common::pose_to_affine(mean_pose).translation(); - if ((mean_position - last_mean_position_).squaredNorm() > 1) { + Eigen::Vector3f tmp_mean_position = common::pose_to_affine(mean_pose).translation(); + if ((tmp_mean_position - last_mean_position_).squaredNorm() > 1) { this->set_weighted_particle_array(weighted_particles); - last_mean_position_ = mean_position; + last_mean_position_ = tmp_mean_position; } else { RCLCPP_WARN_STREAM_THROTTLE( get_logger(), *get_clock(), 2000, @@ -113,12 +116,12 @@ void GnssParticleCorrector::publish_marker(const Eigen::Vector3f & position, boo using namespace std::literals::chrono_literals; using Point = geometry_msgs::msg::Point; - auto drawCircle = [](std::vector & points, float radius) -> void { - const int N = 10; - for (int theta = 0; theta < 2 * N + 1; theta++) { + auto draw_circle = [](std::vector & points, float radius) -> void { + const int n = 10; + for (int theta = 0; theta < 2 * n + 1; theta++) { geometry_msgs::msg::Point pt; - pt.x = radius * std::cos(theta * M_PI / N); - pt.y = radius * std::sin(theta * M_PI / N); + pt.x = radius * std::cos(theta * M_PI / n); + pt.y = radius * std::sin(theta * M_PI / n); points.push_back(pt); } }; @@ -135,11 +138,11 @@ void GnssParticleCorrector::publish_marker(const Eigen::Vector3f & position, boo marker.pose.position.y = position.y(); marker.pose.position.z = latest_height_.data; - float prob = i / 4.0f; + float prob = static_cast(i) / 4.0f; marker.color = static_cast(common::color_scale::rainbow(prob)); marker.color.a = 0.5f; marker.scale.x = 0.1; - drawCircle(marker.points, weight_manager_.inverse_normal_pdf(prob, is_rtk_fixed)); + draw_circle(marker.points, weight_manager_.inverse_normal_pdf(prob, is_rtk_fixed)); array_msg.markers.push_back(marker); } marker_pub_->publish(array_msg); @@ -151,7 +154,7 @@ GnssParticleCorrector::ParticleArray GnssParticleCorrector::weight_particles( ParticleArray weighted_particles{predicted_particles}; for (auto & particle : weighted_particles.particles) { - float distance = static_cast( + auto distance = static_cast( std::hypot(particle.pose.position.x - pose.x(), particle.pose.position.y - pose.y())); particle.weight = weight_manager_.normal_pdf(distance, is_rtk_fixed); } diff --git a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/direct_cost_map.cpp b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/direct_cost_map.cpp index 7da87e8050ebe..c6359a8c8f5fc 100644 --- a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/direct_cost_map.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/direct_cost_map.cpp @@ -18,14 +18,14 @@ namespace yabloc { cv::Mat direct_cost_map(const cv::Mat & cost_map, const cv::Mat & intensity) { - constexpr int MAX_INT = std::numeric_limits::max(); + constexpr int max_int = std::numeric_limits::max(); std::vector> distances; distances.resize(cost_map.rows); for (int i = 0; i < cost_map.rows; i++) { distances.at(i).resize(cost_map.cols); - std::fill(distances.at(i).begin(), distances.at(i).end(), MAX_INT); - const uchar * intensity_ptr = intensity.ptr(i); + std::fill(distances.at(i).begin(), distances.at(i).end(), max_int); + const auto * intensity_ptr = intensity.ptr(i); for (int j = 0; j < cost_map.cols; j++) { if (intensity_ptr[j] == 0) distances.at(i).at(j) = 0; } @@ -36,7 +36,7 @@ cv::Mat direct_cost_map(const cv::Mat & cost_map, const cv::Mat & intensity) // Forward for (int r = 1; r < cost_map.rows; r++) { const uchar * upper_ptr = dst.ptr(r - 1); - uchar * current_ptr = dst.ptr(r); + auto * current_ptr = dst.ptr(r); for (int c = 1; c < cost_map.cols; c++) { int up = distances.at(r - 1).at(c); @@ -56,7 +56,7 @@ cv::Mat direct_cost_map(const cv::Mat & cost_map, const cv::Mat & intensity) // Backward for (int r = cost_map.rows - 2; r >= 0; r--) { const uchar * downer_ptr = dst.ptr(r + 1); - uchar * current_ptr = dst.ptr(r); + auto * current_ptr = dst.ptr(r); for (int c = cost_map.cols - 2; c >= 0; c--) { int down = distances.at(r + 1).at(c); @@ -76,24 +76,6 @@ cv::Mat direct_cost_map(const cv::Mat & cost_map, const cv::Mat & intensity) return dst; } -cv::Mat visualize_direction_map(const cv::Mat & cost_map) -{ - cv::Mat s = 255 * cv::Mat::ones(cost_map.size(), CV_8UC1); - cv::Mat v = 255 * cv::Mat::ones(cost_map.size(), CV_8UC1); - cv::Mat hsv, rgb; - cv::merge(std::vector{cost_map, s, v}, hsv); - cv::cvtColor(hsv, rgb, cv::COLOR_HSV2BGR); - - for (int r = 0; r < cost_map.rows; r++) { - const uchar * src_ptr = cost_map.ptr(r); - cv::Vec3b * dst_ptr = rgb.ptr(r); - for (int c = 0; c < cost_map.cols; c++) { - if (src_ptr[c] == 0) dst_ptr[c] = cv::Vec3b(0, 0, 0); - } - } - return rgb; -} - } // namespace yabloc // #include diff --git a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp index 62d8ed826b373..93e4f77144fd0 100644 --- a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp @@ -24,20 +24,20 @@ namespace yabloc { -float Area::unit_length_ = -1; +float Area::unit_length = -1; HierarchicalCostMap::HierarchicalCostMap(rclcpp::Node * node) -: max_range_(node->declare_parameter("max_range")), - image_size_(node->declare_parameter("image_size")), +: max_range_(static_cast(node->declare_parameter("max_range"))), + image_size_(static_cast(node->declare_parameter("image_size"))), max_map_count_(10), logger_(node->get_logger()) { - Area::unit_length_ = max_range_; - float gamma = node->declare_parameter("gamma"); - gamma_converter.reset(gamma); + Area::unit_length = max_range_; + float gamma = static_cast(node->declare_parameter("gamma")); + gamma_converter_.reset(gamma); } -cv::Point2i HierarchicalCostMap::to_cv_point(const Area & area, const Eigen::Vector2f p) const +cv::Point2i HierarchicalCostMap::to_cv_point(const Area & area, const Eigen::Vector2f & p) const { Eigen::Vector2f relative = p - area.real_scale(); float px = relative.x() / max_range_ * image_size_; @@ -59,7 +59,7 @@ CostMapValue HierarchicalCostMap::at(const Eigen::Vector2f & position) cv::Point2i tmp = to_cv_point(key, position); cv::Vec3b b3 = cost_maps_.at(key).ptr(tmp.y)[tmp.x]; - return {b3[0] / 255.f, b3[1], b3[2] == 1}; + return {static_cast(b3[0]) / 255.f, b3[1], b3[2] == 1}; } void HierarchicalCostMap::set_height(float height) @@ -103,10 +103,12 @@ void HierarchicalCostMap::build_map(const Area & area) { if (!cloud_.has_value()) return; - cv::Mat image = 255 * cv::Mat::ones(cv::Size(image_size_, image_size_), CV_8UC1); - cv::Mat orientation = cv::Mat::zeros(cv::Size(image_size_, image_size_), CV_8UC1); + cv::Mat image = 255 * cv::Mat::ones( + cv::Size(static_cast(image_size_), static_cast(image_size_)), CV_8UC1); + cv::Mat orientation = cv::Mat::zeros(cv::Size( + static_cast(image_size_), static_cast(image_size_)), CV_8UC1); - auto cvPoint = [this, area](const Eigen::Vector3f & p) -> cv::Point { + auto cv_point = [this, area](const Eigen::Vector3f & p) -> cv::Point { return this->to_cv_point(area, p.topRows(2)); }; @@ -117,12 +119,12 @@ void HierarchicalCostMap::build_map(const Area & area) if (std::abs(pn.normal_z - *height_) > 4) continue; } - cv::Point2i from = cvPoint(pn.getVector3fMap()); - cv::Point2i to = cvPoint(pn.getNormalVector3fMap()); + cv::Point2i from = cv_point(pn.getVector3fMap()); + cv::Point2i to = cv_point(pn.getNormalVector3fMap()); - float radian = std::atan2(from.y - to.y, from.x - to.x); + auto radian = static_cast(std::atan2(from.y - to.y, from.x - to.x)); if (radian < 0) radian += M_PI; - float degree = radian * 180 / M_PI; + auto degree = static_cast(radian * 180 / M_PI); cv::line(image, from, to, cv::Scalar::all(0), 1); cv::line(orientation, from, to, cv::Scalar::all(degree), 1); @@ -142,7 +144,7 @@ void HierarchicalCostMap::build_map(const Area & area) cv::Mat directed_cost_map; cv::merge( - std::vector{gamma_converter(distance), whole_orientation, available_area}, + std::vector{gamma_converter_(distance), whole_orientation, available_area}, directed_cost_map); cost_maps_[area] = directed_cost_map; @@ -173,9 +175,11 @@ HierarchicalCostMap::MarkerArray HierarchicalCostMap::show_map_range() const marker.scale.x = 0.1; Eigen::Vector2f xy = area.real_scale(); marker.points.push_back(point_msg(xy.x(), xy.y())); - marker.points.push_back(point_msg(xy.x() + area.unit_length_, xy.y())); - marker.points.push_back(point_msg(xy.x() + area.unit_length_, xy.y() + area.unit_length_)); - marker.points.push_back(point_msg(xy.x(), xy.y() + area.unit_length_)); + marker.points.push_back(point_msg(xy.x() + yabloc::Area::unit_length, xy.y())); + marker.points.push_back( + point_msg(xy.x() + yabloc::Area::unit_length, xy.y() + yabloc::Area::unit_length) + ); + marker.points.push_back(point_msg(xy.x(), xy.y() + yabloc::Area::unit_length)); marker.points.push_back(point_msg(xy.x(), xy.y())); array_msg.markers.push_back(marker); } @@ -188,27 +192,36 @@ cv::Mat HierarchicalCostMap::get_map_image(const Pose & pose) // return cv::Mat::zeros(cv::Size(image_size_, image_size_), CV_8UC3); Eigen::Vector2f center; - center << pose.position.x, pose.position.y; + center << static_cast(pose.position.x), static_cast(pose.position.y); - float w = pose.orientation.w; - float z = pose.orientation.z; - Eigen::Matrix2f R = Eigen::Rotation2Df(2.f * std::atan2(z, w) - M_PI_2).toRotationMatrix(); + auto w = static_cast(pose.orientation.w); + auto z = static_cast(pose.orientation.z); + Eigen::Matrix2f r = + Eigen::Rotation2Df(static_cast(2.f * std::atan2(z, w) - M_PI_2)).toRotationMatrix(); - auto toVector2f = [this, center, R](float h, float w) -> Eigen::Vector2f { + auto to_vector2f = [this, center, r](float h, float w) -> Eigen::Vector2f { Eigen::Vector2f offset; offset.x() = (w / this->image_size_ - 0.5f) * this->max_range_ * 1.5f; offset.y() = -(h / this->image_size_ - 0.5f) * this->max_range_ * 1.5f; - return center + R * offset; + return center + r * offset; }; - cv::Mat image = cv::Mat::zeros(cv::Size(image_size_, image_size_), CV_8UC3); - for (int w = 0; w < image_size_; w++) { - for (int h = 0; h < image_size_; h++) { - CostMapValue v3 = this->at(toVector2f(h, w)); + cv::Mat image = cv::Mat::zeros( + cv::Size(static_cast(image_size_), static_cast(image_size_)), CV_8UC3); + for (int w_index = 0; static_cast(w_index) < image_size_; w_index++) { + for (int h_index = 0; static_cast(h_index) < image_size_; h_index++) { + CostMapValue v3 = this->at( + to_vector2f(static_cast(h_index), static_cast(w_index)) + ); if (v3.unmapped) - image.at(h, w) = cv::Vec3b(v3.angle, 255 * v3.intensity, 50); + image.at(h_index, w_index) = + cv::Vec3b(v3.angle, static_cast(255 * v3.intensity), 50); else - image.at(h, w) = cv::Vec3b(v3.angle, 255 * v3.intensity, 255 * v3.intensity); + image.at(h_index, w_index) = cv::Vec3b( + v3.angle, + static_cast(255 * v3.intensity), + static_cast(255 * v3.intensity) + ); } } @@ -235,7 +248,8 @@ void HierarchicalCostMap::erase_obsolete() cv::Mat HierarchicalCostMap::create_available_area_image(const Area & area) const { - cv::Mat available_area = cv::Mat::zeros(cv::Size(image_size_, image_size_), CV_8UC1); + cv::Mat available_area = + cv::Mat::zeros(cv::Size(static_cast(image_size_), static_cast(image_size_)), CV_8UC1); if (bounding_boxes_.empty()) return available_area; // Define current area diff --git a/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp index 8046d43dc34a3..317adf757979f 100644 --- a/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp @@ -33,10 +33,13 @@ namespace yabloc::modularized_particle_filter Predictor::Predictor(const rclcpp::NodeOptions & options) : Node("predictor", options), - number_of_particles_(declare_parameter("num_of_particles")), - resampling_interval_seconds_(declare_parameter("resampling_interval_seconds")), - static_linear_covariance_(declare_parameter("static_linear_covariance")), - static_angular_covariance_(declare_parameter("static_angular_covariance")), + number_of_particles_(static_cast(declare_parameter("num_of_particles"))), + resampling_interval_seconds_( + static_cast(declare_parameter("resampling_interval_seconds"))), + static_linear_covariance_( + static_cast(declare_parameter("static_linear_covariance"))), + static_angular_covariance_( + static_cast(declare_parameter("static_angular_covariance"))), cov_xx_yy_{this->template declare_parameter>("cov_xx_yy")} { tf2_broadcaster_ = std::make_unique(*this); @@ -93,11 +96,11 @@ void Predictor::on_trigger_service( RCLCPP_INFO_STREAM(get_logger(), "yabloc particle filter is deactivated"); } - const bool before_activated_ = yabloc_activated_; + const bool before_activated = yabloc_activated_; yabloc_activated_ = request->data; response->success = true; - if (yabloc_activated_ && (!before_activated_)) { + if (yabloc_activated_ && (!before_activated)) { RCLCPP_INFO_STREAM(get_logger(), "restart particle filter"); if (latest_ekf_pose_ptr_) { on_initial_pose(latest_ekf_pose_ptr_); @@ -113,10 +116,12 @@ void Predictor::on_initial_pose(const PoseCovStamped::ConstSharedPtr initialpose // Publish initial pose marker auto position = initialpose->pose.pose.position; Eigen::Vector3f pos_vec3f; - pos_vec3f << position.x, position.y, position.z; + pos_vec3f << static_cast(position.x), + static_cast(position.y), + static_cast(position.z); auto orientation = initialpose->pose.pose.orientation; - float theta = 2 * std::atan2(orientation.z, orientation.w); + auto theta = static_cast(2 * std::atan2(orientation.z, orientation.w)); Eigen::Vector3f tangent; tangent << std::cos(theta), std::sin(theta), 0; @@ -152,7 +157,7 @@ void Predictor::initialize_particles(const PoseCovStamped & initialpose) pose.position.x += noise.x(); pose.position.y += noise.y(); - float noised_yaw = util::normalize_radian(yaw + util::nrand(yaw_std)); + auto noised_yaw = static_cast(util::normalize_radian(yaw + util::nrand(yaw_std))); pose.orientation.w = std::cos(noised_yaw / 2.0); pose.orientation.x = 0.0; pose.orientation.y = 0.0; @@ -184,14 +189,14 @@ void Predictor::on_twist_cov(const TwistCovStamped::ConstSharedPtr twist_cov) } void Predictor::update_with_dynamic_noise( - ParticleArray & particle_array, const TwistCovStamped & twist, double dt) + ParticleArray & particle_array, const TwistCovStamped & twist, double dt) const { // linear & angular velocity - const float linear_x = twist.twist.twist.linear.x; - const float angular_z = twist.twist.twist.angular.z; + const auto linear_x = static_cast(twist.twist.twist.linear.x); + const auto angular_z = static_cast(twist.twist.twist.angular.z); // standard deviation of linear & angular velocity - const float std_linear_x = std::sqrt(twist.twist.covariance[6 * 0 + 0]); - const float std_angular_z = std::sqrt(twist.twist.covariance[6 * 5 + 5]); + const auto std_linear_x = static_cast(std::sqrt(twist.twist.covariance[6 * 0 + 0])); + const auto std_angular_z = static_cast(std::sqrt(twist.twist.covariance[6 * 5 + 5])); // 1[rad/s] = 60[deg/s] // 1[m/s] = 3.6[km/h] const float truncated_angular_std = @@ -272,7 +277,7 @@ void Predictor::on_weighted_particles(const ParticleArray::ConstSharedPtr weight try { particle_array = resampler_ptr_->add_weight_retroactively(particle_array, *weighted_particles_ptr); - } catch (const resampling_skip_exception & e) { + } catch (const ResamplingSkipException & e) { // Do nothing (just skipping the resample()) RCLCPP_INFO_STREAM(this->get_logger(), "skipped resampling"); } @@ -284,16 +289,16 @@ void Predictor::on_weighted_particles(const ParticleArray::ConstSharedPtr weight // Exit if previous resampling time is not valid. if (!previous_resampling_time_opt_.has_value()) { previous_resampling_time_opt_ = current_time; - throw resampling_skip_exception("previous resampling time is not valid"); + throw ResamplingSkipException("previous resampling time is not valid"); } if (current_time - previous_resampling_time_opt_.value() <= resampling_interval_seconds_) { - throw resampling_skip_exception("it is not time to resample"); + throw ResamplingSkipException("it is not time to resample"); } particle_array = resampler_ptr_->resample(particle_array); previous_resampling_time_opt_ = current_time; - } catch (const resampling_skip_exception & e) { + } catch (const ResamplingSkipException & e) { void(); // Do nothing (just skipping the resample()) } @@ -397,7 +402,7 @@ Predictor::PoseCovStamped Predictor::rectify_initial_pose( msg.pose.pose.orientation.z = std::sin(theta / 2); Eigen::Matrix2f cov; - cov << cov_xx_yy_.at(0), 0, 0, cov_xx_yy_.at(1); + cov << static_cast(cov_xx_yy_.at(0)), 0, 0, static_cast(cov_xx_yy_.at(1)); Eigen::Rotation2D r(theta); cov = r * cov * r.inverse(); diff --git a/localization/yabloc/yabloc_particle_filter/src/prediction/resampler.cpp b/localization/yabloc/yabloc_particle_filter/src/prediction/resampler.cpp index cc4cbc3730e0c..c8c3971ba85b2 100644 --- a/localization/yabloc/yabloc_particle_filter/src/prediction/resampler.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/prediction/resampler.cpp @@ -65,7 +65,7 @@ RetroactiveResampler::ParticleArray RetroactiveResampler::add_weight_retroactive { if (!check_weighted_particles_validity(weighted_particles)) { RCLCPP_ERROR_STREAM(logger_, "weighted_particles has invalid data"); - throw resampling_skip_exception("weighted_particles has invalid data"); + throw ResamplingSkipException("weighted_particles has invalid data"); } // Initialize corresponding index lookup table @@ -143,7 +143,7 @@ RetroactiveResampler::ParticleArray RetroactiveResampler::resample( // Copy particle to resampled variable resampled_particles.particles[m] = predicted_particles.particles[predicted_particle_index]; // Reset weight uniformly - resampled_particles.particles[m].weight = num_of_particles_inv; + resampled_particles.particles[m].weight = static_cast(num_of_particles_inv); // Make history resampling_history_[latest_resampling_generation_][m] = predicted_particle_index; } @@ -157,7 +157,7 @@ RetroactiveResampler::ParticleArray RetroactiveResampler::resample( return resampled_particles; } -double RetroactiveResampler::random_from_01_uniformly() const +double RetroactiveResampler::random_from_01_uniformly() { static std::default_random_engine engine(0); std::uniform_real_distribution dist(0.0, 1.0); diff --git a/localization/yabloc/yabloc_particle_filter/test/src/test_resampler.cpp b/localization/yabloc/yabloc_particle_filter/test/src/test_resampler.cpp index 124b0500e010e..d7793d510edd6 100644 --- a/localization/yabloc/yabloc_particle_filter/test/src/test_resampler.cpp +++ b/localization/yabloc/yabloc_particle_filter/test/src/test_resampler.cpp @@ -22,20 +22,20 @@ namespace mpf = yabloc::modularized_particle_filter; using Particle = yabloc_particle_filter::msg::Particle; using ParticleArray = yabloc_particle_filter::msg::ParticleArray; -constexpr int PARTICLE_COUNT = 10; -constexpr int HISTORY_SIZE = 10; +constexpr int particle_count = 10; +constexpr int history_size = 10; TEST(ResamplerTestSuite, outOfHistory) { - mpf::RetroactiveResampler resampler(PARTICLE_COUNT, HISTORY_SIZE); + mpf::RetroactiveResampler resampler(particle_count, history_size); ParticleArray predicted; ParticleArray weighted; predicted.header.stamp = rclcpp::Time(0); predicted.id = 0; weighted.id = 0; - predicted.particles.resize(PARTICLE_COUNT); - weighted.particles.resize(PARTICLE_COUNT); + predicted.particles.resize(particle_count); + weighted.particles.resize(particle_count); for (auto & p : predicted.particles) p.weight = 1; for (auto & p : weighted.particles) p.weight = 1; @@ -64,7 +64,7 @@ TEST(ResamplerTestSuite, outOfHistory) } // Iterate resampling to fill all history - for (int t = 0; t < HISTORY_SIZE; ++t) { + for (int t = 0; t < history_size; ++t) { auto resampled = resampler.resample(predicted); EXPECT_EQ(resampled.id, t + 1); predicted = resampled; @@ -85,26 +85,26 @@ TEST(ResamplerTestSuite, outOfHistory) TEST(ResamplerTestSuite, simpleResampling) { - mpf::RetroactiveResampler resampler(PARTICLE_COUNT, HISTORY_SIZE); + mpf::RetroactiveResampler resampler(particle_count, history_size); ParticleArray predicted; predicted.header.stamp = rclcpp::Time(0); - predicted.particles.resize(PARTICLE_COUNT); + predicted.particles.resize(particle_count); predicted.id = 0; - for (int i = 0; i < PARTICLE_COUNT; ++i) predicted.particles.at(i).weight = 1; + for (int i = 0; i < particle_count; ++i) predicted.particles.at(i).weight = 1; ParticleArray weighted; - weighted.particles.resize(PARTICLE_COUNT); + weighted.particles.resize(particle_count); // Update by uniform distribution { // Weight weighted.id = 0; - for (int i = 0; i < PARTICLE_COUNT; ++i) weighted.particles.at(i).weight = 0.5; + for (int i = 0; i < particle_count; ++i) weighted.particles.at(i).weight = 0.5; ParticleArray array1 = resampler.add_weight_retroactively(predicted, weighted); // All weights must be equal - for (const auto & p : array1.particles) EXPECT_NEAR(p.weight, 1.0 / PARTICLE_COUNT, 1e-3); + for (const auto & p : array1.particles) EXPECT_NEAR(p.weight, 1.0 / particle_count, 1e-3); // Resample predicted = array1; @@ -117,10 +117,10 @@ TEST(ResamplerTestSuite, simpleResampling) { // Weight weighted.id = 0; - for (int i = 0; i < PARTICLE_COUNT; ++i) { + for (int i = 0; i < particle_count; ++i) { auto & p = predicted.particles.at(i); auto & q = weighted.particles.at(i); - if (i < PARTICLE_COUNT / 2) { + if (i < particle_count / 2) { p.pose.position.x = 1; q.weight = 2.0; } else { @@ -131,12 +131,12 @@ TEST(ResamplerTestSuite, simpleResampling) ParticleArray array1 = resampler.add_weight_retroactively(predicted, weighted); // All weight must match with following expectation - for (int i = 0; i < PARTICLE_COUNT; ++i) { + for (int i = 0; i < particle_count; ++i) { const auto & p = array1.particles.at(i); - if (i < PARTICLE_COUNT / 2) { - EXPECT_NEAR(p.weight, 2.0 / 1.5 / PARTICLE_COUNT, 1e-3f); + if (i < particle_count / 2) { + EXPECT_NEAR(p.weight, 2.0 / 1.5 / particle_count, 1e-3f); } else { - EXPECT_NEAR(p.weight, 1.0 / 1.5 / PARTICLE_COUNT, 1e-3f); + EXPECT_NEAR(p.weight, 1.0 / 1.5 / particle_count, 1e-3f); } } @@ -146,32 +146,33 @@ TEST(ResamplerTestSuite, simpleResampling) predicted = resampled; EXPECT_EQ(predicted.id, 2); - int centroid = 0; - for (const auto & p : predicted.particles) centroid += p.pose.position.x; + int centroid = std::accumulate(predicted.particles.begin(), predicted.particles.end(), 0, + [](int sum, const auto& p) { return sum + static_cast(p.pose.position.x); } + ); EXPECT_GT(centroid, 0); } } TEST(ResamplerTestSuite, resamplingWithRetrogression) { - mpf::RetroactiveResampler resampler(PARTICLE_COUNT, HISTORY_SIZE); + mpf::RetroactiveResampler resampler(particle_count, history_size); ParticleArray predicted; predicted.header.stamp = rclcpp::Time(0); - predicted.particles.resize(PARTICLE_COUNT); + predicted.particles.resize(particle_count); predicted.id = 0; - for (int i = 0; i < PARTICLE_COUNT; ++i) { + for (int i = 0; i < particle_count; ++i) { auto & p = predicted.particles.at(i); p.weight = 1.0; - if (i < PARTICLE_COUNT / 2) + if (i < particle_count / 2) p.pose.position.x = 1; else p.pose.position.x = -1; } // Fill all history with biased weighted particles - for (int p = 0; p < HISTORY_SIZE; ++p) { + for (int p = 0; p < history_size; ++p) { auto resampled = resampler.resample(predicted); predicted = resampled; EXPECT_EQ(predicted.id, p + 1); @@ -179,18 +180,19 @@ TEST(ResamplerTestSuite, resamplingWithRetrogression) // Update by ancient measurement { - double before_centroid = 0; - for (const auto & p : predicted.particles) { - before_centroid += p.pose.position.x * p.weight; - } + double before_centroid = std::accumulate(predicted.particles.begin(), + predicted.particles.end(), 0.0, [](double sum, const auto& p) + { return sum + p.pose.position.x * p.weight; } + ); + // Weight ParticleArray weighted; - weighted.particles.resize(PARTICLE_COUNT); + weighted.particles.resize(particle_count); weighted.id = 1; // ancient generation id - for (int i = 0; i < PARTICLE_COUNT; ++i) { + for (int i = 0; i < particle_count; ++i) { auto & q = weighted.particles.at(i); - if (i < PARTICLE_COUNT / 2) { + if (i < particle_count / 2) { q.weight = 2.0; } else { q.weight = 1.0; @@ -199,10 +201,11 @@ TEST(ResamplerTestSuite, resamplingWithRetrogression) predicted = resampler.add_weight_retroactively(predicted, weighted); - double after_centroid = 0; - for (const auto & p : predicted.particles) { - after_centroid += p.pose.position.x * p.weight; - } + double after_centroid = std::accumulate(predicted.particles.begin(), + predicted.particles.end(), 0.0, [](double sum, const auto& p) + { return sum + p.pose.position.x * p.weight; } + ); + EXPECT_TRUE(after_centroid > before_centroid); } }