style(pre-commit): autofix

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/common/prediction_util.hpp

@@ -27,7 +27,7 @@ namespace yabloc::modularized_particle_filter::util
 inline std::random_device seed_gen;
 inline std::default_random_engine engine(seed_gen());
 
-inline 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);

localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector/weight_manager.hpp

@@ -72,7 +72,8 @@ struct WeightManager
   {
     if (is_rtk_fixed) {
       return normal_pdf(distance, for_fixed_);
-    }       return normal_pdf(distance, for_not_fixed_);
+    }
+    return normal_pdf(distance, for_not_fixed_);
   }
 
   [[nodiscard]] static float inverse_normal_pdf(float prob, const Parameter & param)
@@ -88,7 +89,8 @@ struct WeightManager
   {
     if (is_rtk_fixed) {
       return inverse_normal_pdf(prob, for_fixed_);
-    }       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/hierarchical_cost_map.hpp

@@ -47,8 +47,9 @@ struct Area
     y = static_cast<int>(std::floor(v.y() / unit_length));
   }
 
-  [[nodiscard]] Eigen::Vector2f real_scale() const { return {
-    static_cast<float>(x) * unit_length, static_cast<float>(y) * unit_length};
+  [[nodiscard]] Eigen::Vector2f real_scale() const
+  {
+    return {static_cast<float>(x) * unit_length, static_cast<float>(y) * unit_length};
   }
 
   [[nodiscard]] std::array<Eigen::Vector2f, 2> real_scale_boundary() const

localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampler.hpp

@@ -56,7 +56,7 @@ class RetroactiveResampler
   int latest_resampling_generation_;
 
   // Random generator from 0 to 1
-  [[nodiscard]] static double random_from_01_uniformly() ;
+  [[nodiscard]] static double random_from_01_uniformly();
   // Check the sanity of the particles obtained from the particle corrector.
   [[nodiscard]] bool check_weighted_particles_validity(
     const ParticleArray & weighted_particles) const;

localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp

@@ -15,7 +15,6 @@
 #include "yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp"
 #include "yabloc_particle_filter/camera_corrector/logit.hpp"
 
-#include <cmath>
 #include <opencv4/opencv2/imgproc.hpp>
 #include <tier4_autoware_utils/math/trigonometry.hpp>
 #include <tier4_autoware_utils/system/stop_watch.hpp>
@@ -26,6 +25,8 @@
 
 #include <pcl_conversions/pcl_conversions.h>
 
+#include <cmath>
+
 namespace yabloc::modularized_particle_filter
 {
 
@@ -113,7 +114,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](const LineSegments & cloud, const 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();
@@ -201,9 +202,9 @@ void CameraParticleCorrector::on_line_segments(const PointCloud2 & line_segments
     pcl::PointCloud<pcl::PointXYZRGB> rgb_cloud;
     pcl::PointCloud<pcl::PointXYZRGB> rgb_iffy_cloud;
 
-    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)); }
-    );
+    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();
@@ -312,11 +313,10 @@ pcl::PointCloud<pcl::PointXYZI> CameraParticleCorrector::evaluate_cloud(
 
       CostMapValue v3 = cost_map_.at(p.topRows(2));
       float logit = 0;
-      if (!v3.unmapped){
+      if (!v3.unmapped) {
         float gain = std::exp(-far_weight_gain_ * squared_norm);
 
-        logit =
-          gain * (abs_cos(tangent, static_cast<float>(v3.angle)) * v3.intensity - 0.5f);
+        logit = gain * (abs_cos(tangent, static_cast<float>(v3.angle)) * v3.intensity - 0.5f);
       }
 
       pcl::PointXYZI xyzi(logit_to_prob(logit, 10.f));

localization/yabloc/yabloc_particle_filter/src/camera_corrector/filter_line_segments.cpp

@@ -22,7 +22,7 @@
 
 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;
@@ -42,7 +42,8 @@ float normalized_atan2(const Eigen::Vector3f & t, float deg)
 
   if (diff < M_PI) {
     return static_cast<float>(diff / M_PI_2 - 1.0);
-  }     throw std::runtime_error("invalid cos");
+  }
+  throw std::runtime_error("invalid cos");
 }
 
 std::pair<CameraParticleCorrector::LineSegments, CameraParticleCorrector::LineSegments>

localization/yabloc/yabloc_particle_filter/src/common/mean.cpp

@@ -96,11 +96,8 @@ 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(
-    static_cast<float>(ori.w),
-    static_cast<float>(ori.x),
-    static_cast<float>(ori.y),
-    static_cast<float>(ori.z)
-  );
+    static_cast<float>(ori.w), static_cast<float>(ori.x), static_cast<float>(ori.y),
+    static_cast<float>(ori.z));
   float inv_n = 1.f / static_cast<float>(array.particles.size());
   Eigen::Vector3f mean = Eigen::Vector3f::Zero();
   for (const Particle & p : array.particles) {

localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp

@@ -62,9 +62,8 @@ 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 << static_cast<float>(position.x),
-                   static_cast<float>(position.y),
-                   static_cast<float>(position.z);
+  gnss_position << static_cast<float>(position.x), static_cast<float>(position.y),
+    static_cast<float>(position.z);
 
   constexpr bool is_rtk_fixed = false;
   process(gnss_position, stamp, is_rtk_fixed);

localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp

@@ -103,10 +103,11 @@ void HierarchicalCostMap::build_map(const Area & area)
 {
   if (!cloud_.has_value()) return;
 
-  cv::Mat image = 255 * cv::Mat::ones(
-    cv::Size(static_cast<int>(image_size_), static_cast<int>(image_size_)), CV_8UC1);
-  cv::Mat orientation = cv::Mat::zeros(cv::Size(
-    static_cast<int>(image_size_), static_cast<int>(image_size_)), CV_8UC1);
+  cv::Mat image =
+    255 *
+    cv::Mat::ones(cv::Size(static_cast<int>(image_size_), static_cast<int>(image_size_)), CV_8UC1);
+  cv::Mat orientation =
+    cv::Mat::zeros(cv::Size(static_cast<int>(image_size_), static_cast<int>(image_size_)), CV_8UC1);
 
   auto cv_point = [this, area](const Eigen::Vector3f & p) -> cv::Point {
     return this->to_cv_point(area, p.topRows(2));
@@ -177,8 +178,7 @@ HierarchicalCostMap::MarkerArray HierarchicalCostMap::show_map_range() const
     marker.points.push_back(point_msg(xy.x(), xy.y()));
     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)
-    );
+      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);
@@ -206,22 +206,19 @@ cv::Mat HierarchicalCostMap::get_map_image(const Pose & pose)
     return center + r * offset;
   };
 
-  cv::Mat image = cv::Mat::zeros(
-    cv::Size(static_cast<int>(image_size_), static_cast<int>(image_size_)), CV_8UC3);
+  cv::Mat image =
+    cv::Mat::zeros(cv::Size(static_cast<int>(image_size_), static_cast<int>(image_size_)), CV_8UC3);
   for (int w_index = 0; static_cast<float>(w_index) < image_size_; w_index++) {
     for (int h_index = 0; static_cast<float>(h_index) < image_size_; h_index++) {
-      CostMapValue v3 = this->at(
-        to_vector2f(static_cast<float>(h_index), static_cast<float>(w_index))
-      );
+      CostMapValue v3 =
+        this->at(to_vector2f(static_cast<float>(h_index), static_cast<float>(w_index)));
       if (v3.unmapped)
         image.at<cv::Vec3b>(h_index, w_index) =
           cv::Vec3b(v3.angle, static_cast<unsigned char>(255 * v3.intensity), 50);
       else
         image.at<cv::Vec3b>(h_index, w_index) = cv::Vec3b(
-          v3.angle,
-          static_cast<unsigned char>(255 * v3.intensity),
-          static_cast<unsigned char>(255 * v3.intensity)
-        );
+          v3.angle, static_cast<unsigned char>(255 * v3.intensity),
+          static_cast<unsigned char>(255 * v3.intensity));
     }
   }
 

localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp

@@ -116,9 +116,8 @@ 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 << static_cast<float>(position.x),
-               static_cast<float>(position.y),
-               static_cast<float>(position.z);
+  pos_vec3f << static_cast<float>(position.x), static_cast<float>(position.y),
+    static_cast<float>(position.z);
 
   auto orientation = initialpose->pose.pose.orientation;
   auto theta = static_cast<float>(2 * std::atan2(orientation.z, orientation.w));

localization/yabloc/yabloc_particle_filter/test/src/test_resampler.cpp

@@ -146,9 +146,9 @@ TEST(ResamplerTestSuite, simpleResampling)
     predicted = resampled;
     EXPECT_EQ(predicted.id, 2);
 
-    int centroid = std::accumulate(predicted.particles.begin(), predicted.particles.end(), 0,
-      [](int sum, const auto& p) { return sum + static_cast<int>(p.pose.position.x); }
-    );
+    int centroid = std::accumulate(
+      predicted.particles.begin(), predicted.particles.end(), 0,
+      [](int sum, const auto & p) { return sum + static_cast<int>(p.pose.position.x); });
     EXPECT_GT(centroid, 0);
   }
 }
@@ -180,11 +180,9 @@ TEST(ResamplerTestSuite, resamplingWithRetrogression)
 
   // Update by ancient measurement
   {
-    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; }
-    );
-
+    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;
@@ -201,10 +199,9 @@ TEST(ResamplerTestSuite, resamplingWithRetrogression)
 
     predicted = resampler.add_weight_retroactively(predicted, weighted);
 
-    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; }
-    );
+    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);
   }