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 41f10f12c4ef3..4ca8c01b3be64 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/common/prediction_util.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/prediction_util.hpp index ab001d58eddf3..140f1553b88ee 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 @@ -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 svd; svd.compute(cov, Eigen::ComputeFullU | Eigen::ComputeFullV); 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 3a857fd52f76b..f22662a188355 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 @@ -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 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 6c3a753f8d7f7..d6cc4553e29bc 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 @@ -47,8 +47,9 @@ struct Area y = static_cast(std::floor(v.y() / unit_length)); } - [[nodiscard]] Eigen::Vector2f real_scale() const { return { - static_cast(x) * unit_length, static_cast(y) * unit_length}; + [[nodiscard]] Eigen::Vector2f real_scale() const + { + return {static_cast(x) * unit_length, static_cast(y) * unit_length}; } [[nodiscard]] std::array real_scale_boundary() const 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 f35b226054dac..7b3aa8553f90e 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 @@ -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; 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 d5ada69dc4ad8..8a21cdaf4c8fc 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,7 +15,6 @@ #include "yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp" #include "yabloc_particle_filter/camera_corrector/logit.hpp" -#include #include #include #include @@ -26,6 +25,8 @@ #include +#include + 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 rgb_cloud; pcl::PointCloud 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 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(v3.angle)) * v3.intensity - 0.5f); + logit = gain * (abs_cos(tangent, static_cast(v3.angle)) * v3.intensity - 0.5f); } pcl::PointXYZI xyzi(logit_to_prob(logit, 10.f)); 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 64cc020c1e9ca..105f72920a6de 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,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(diff / M_PI_2 - 1.0); - } throw std::runtime_error("invalid cos"); + } + throw std::runtime_error("invalid cos"); } std::pair diff --git a/localization/yabloc/yabloc_particle_filter/src/common/mean.cpp b/localization/yabloc/yabloc_particle_filter/src/common/mean.cpp index b40d134a5c37e..1a0ef05508c76 100644 --- a/localization/yabloc/yabloc_particle_filter/src/common/mean.cpp +++ b/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(ori.w), - static_cast(ori.x), - static_cast(ori.y), - static_cast(ori.z) - ); + 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) { 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 3e63dbf2385ee..f2e5aaf2e5654 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 @@ -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(position.x), - static_cast(position.y), - static_cast(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); 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 93e4f77144fd0..de4548eb64dc0 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 @@ -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(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); + 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 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(image_size_), static_cast(image_size_)), CV_8UC3); + 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)) - ); + CostMapValue v3 = + this->at(to_vector2f(static_cast(h_index), static_cast(w_index))); if (v3.unmapped) image.at(h_index, w_index) = cv::Vec3b(v3.angle, static_cast(255 * v3.intensity), 50); else image.at(h_index, w_index) = cv::Vec3b( - v3.angle, - static_cast(255 * v3.intensity), - static_cast(255 * v3.intensity) - ); + v3.angle, static_cast(255 * v3.intensity), + static_cast(255 * v3.intensity)); } } diff --git a/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp index 317adf757979f..701a2b8763fa3 100644 --- a/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp +++ b/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(position.x), - static_cast(position.y), - static_cast(position.z); + pos_vec3f << static_cast(position.x), static_cast(position.y), + static_cast(position.z); auto orientation = initialpose->pose.pose.orientation; auto theta = static_cast(2 * std::atan2(orientation.z, orientation.w)); 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 d7793d510edd6..915237b2b125c 100644 --- a/localization/yabloc/yabloc_particle_filter/test/src/test_resampler.cpp +++ b/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(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); } } @@ -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); }