removed unused

Signed-off-by: a-maumau <[email protected]>

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

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_

localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/mean.hpp

@@ -20,18 +20,13 @@
 
 #include <geometry_msgs/msg/pose.hpp>
 
-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_

localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/prediction_util.hpp

@@ -22,14 +22,12 @@
 #include <random>
 #include <vector>
 
-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<Eigen::Matrix2d> 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_

localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/visualize.hpp

@@ -21,9 +21,7 @@
 #include <std_msgs/msg/color_rgba.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
-namespace yabloc
-{
-namespace modularized_particle_filter
+namespace yabloc::modularized_particle_filter
 {
 class ParticleVisualizer
 {
@@ -37,9 +35,7 @@ class ParticleVisualizer
 
 private:
   rclcpp::Publisher<MarkerArray>::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_

localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/abstract_corrector.hpp

@@ -26,9 +26,7 @@
 #include <optional>
 #include <string>
 
-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_

localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/correction_util.hpp

@@ -22,14 +22,11 @@
 
 #include <optional>
 
-namespace yabloc
-{
-namespace modularized_particle_filter
+namespace yabloc::modularized_particle_filter
 {
 std::optional<yabloc_particle_filter::msg::ParticleArray> find_synced_particles(
   boost::circular_buffer<yabloc_particle_filter::msg::ParticleArray> circular_buffer,
   rclcpp::Time time);
-}
-}  // namespace yabloc
+}  // namespace yabloc::modularized_particle_filter
 
 #endif  // YABLOC_PARTICLE_FILTER__CORRECTION__CORRECTION_UTIL_HPP_

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<float>("for_fixed/flat_radius");
-    for_fixed_.max_radius_ = node->declare_parameter<float>("for_fixed/max_radius");
-    for_fixed_.min_weight_ = node->declare_parameter<float>("for_fixed/min_weight");
-    for_fixed_.max_weight_ = node->declare_parameter<float>("for_fixed/max_weight");
+    for_fixed_.flat_radius_ =
+      static_cast<float>(node->declare_parameter<float>("for_fixed/flat_radius"));
+    for_fixed_.max_radius_ =
+      static_cast<float>(node->declare_parameter<float>("for_fixed/max_radius"));
+    for_fixed_.min_weight_ =
+      static_cast<float>(node->declare_parameter<float>("for_fixed/min_weight"));
+    for_fixed_.max_weight_ =
+      static_cast<float>(node->declare_parameter<float>("for_fixed/max_weight"));
     for_fixed_.compute_coeff();
 
-    for_not_fixed_.flat_radius_ = node->declare_parameter<float>("for_not_fixed/flat_radius");
-    for_not_fixed_.max_radius_ = node->declare_parameter<float>("for_not_fixed/max_radius");
-    for_not_fixed_.min_weight_ = node->declare_parameter<float>("for_not_fixed/min_weight");
-    for_not_fixed_.max_weight_ = node->declare_parameter<float>("for_not_fixed/max_weight");
+    for_not_fixed_.flat_radius_ =
+      static_cast<float>(node->declare_parameter<float>("for_not_fixed/flat_radius"));
+    for_not_fixed_.max_radius_ =
+      static_cast<float>(node->declare_parameter<float>("for_not_fixed/max_radius"));
+    for_not_fixed_.min_weight_ =
+      static_cast<float>(node->declare_parameter<float>("for_not_fixed/min_weight"));
+    for_not_fixed_.max_weight_ =
+      static_cast<float>(node->declare_parameter<float>("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

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_

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<int>(std::floor(v.x() / unit_length_));
-    y = static_cast<int>(std::floor(v.y() / unit_length_));
+    x = static_cast<int>(std::floor(v.x() / unit_length));
+    y = static_cast<int>(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<float>(x) * unit_length, static_cast<float>(y) * unit_length};
+  }
 
-  std::array<Eigen::Vector2f, 2> real_scale_boundary() const
+  [[nodiscard]] std::array<Eigen::Vector2f, 2> real_scale_boundary() const
   {
     std::array<Eigen::Vector2f, 2> 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<float> height_{std::nullopt};
 
-  common::GammaConverter gamma_converter{4.0f};
+  common::GammaConverter gamma_converter_{4.0f};
 
   std::unordered_map<Area, bool, Area> map_accessed_;
 
@@ -138,7 +140,7 @@ class HierarchicalCostMap
   std::vector<BgPolygon> bounding_boxes_;
   std::unordered_map<Area, cv::Mat, Area> 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;

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

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
 

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) {