From 897dcff768b51be25fc0987ba118c200a49a7936 Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Tue, 25 Apr 2023 11:28:12 +0900 Subject: [PATCH] feature(graph_segment, gnss_particle_corrector): make some features switchable (#17) * make additional-graph-segment-pickup disablable Signed-off-by: Kento Yabuuchi * enlarge gnss_mahalanobis_distance_threshold in expressway.launch Signed-off-by: Kento Yabuuchi --------- Signed-off-by: Kento Yabuuchi --- .../imgproc/graph_segment/CMakeLists.txt | 23 +---- .../include/graph_segment/graph_segment.hpp | 7 +- .../graph_segment/similar_area_searcher.hpp | 40 ++++++++ .../graph_segment/src/graph_segment_core.cpp | 84 ++-------------- .../src/similar_area_searcher.cpp | 96 +++++++++++++++++++ .../gnss_particle_corrector.hpp | 4 + .../src/gnss_corrector_core.cpp | 46 +++++---- .../launch/impl/imgproc.launch.xml | 5 +- .../pcdless_launch/launch/impl/pf.launch.xml | 3 +- .../launch/tier4_expressway.launch.xml | 2 + 10 files changed, 192 insertions(+), 118 deletions(-) create mode 100644 localization/yabloc/imgproc/graph_segment/include/graph_segment/similar_area_searcher.hpp create mode 100644 localization/yabloc/imgproc/graph_segment/src/similar_area_searcher.cpp diff --git a/localization/yabloc/imgproc/graph_segment/CMakeLists.txt b/localization/yabloc/imgproc/graph_segment/CMakeLists.txt index 83251f99a2f1b..e31d02c9e3441 100644 --- a/localization/yabloc/imgproc/graph_segment/CMakeLists.txt +++ b/localization/yabloc/imgproc/graph_segment/CMakeLists.txt @@ -28,24 +28,11 @@ find_package(PCL REQUIRED COMPONENTS common) # =================================================== # Executable -macro(ADD_EXE) - cmake_parse_arguments(_ARG "" "TARGET" "SRC;LIB;INC" ${ARGN}) - - set(SOURCE "") - - foreach(QUERY IN ITEMS ${_ARG_SRC}) - file(GLOB SOURCE_TMP ${QUERY}) - set(SOURCE "${SOURCE};${SOURCE_TMP}") - endforeach() - - set(TARGET_NAME ${_ARG_TARGET}) - ament_auto_add_executable(${TARGET_NAME} ${SOURCE}) - target_include_directories(${TARGET_NAME} PUBLIC ${_ARG_INC}) - target_include_directories(${TARGET_NAME} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) - target_link_libraries(${TARGET_NAME} ${_ARG_LIB} ${OpenCV_LIBS}) -endmacro() - -ADD_EXE(TARGET graph_segment_node SRC src/graph_segment_node.cpp src/graph_segment_core.cpp INC include) +set(TARGET graph_segment_node) +ament_auto_add_executable(${TARGET} src/graph_segment_node.cpp src/graph_segment_core src/similar_area_searcher.cpp) +target_include_directories(${TARGET} PUBLIC include) +target_include_directories(${TARGET} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) +target_link_libraries(${TARGET} ${_ARG_LIB} ${OpenCV_LIBS}) # =================================================== ament_auto_package() \ No newline at end of file diff --git a/localization/yabloc/imgproc/graph_segment/include/graph_segment/graph_segment.hpp b/localization/yabloc/imgproc/graph_segment/include/graph_segment/graph_segment.hpp index 6ae75dbeb0471..b334671ba2182 100644 --- a/localization/yabloc/imgproc/graph_segment/include/graph_segment/graph_segment.hpp +++ b/localization/yabloc/imgproc/graph_segment/include/graph_segment/graph_segment.hpp @@ -13,6 +13,8 @@ // limitations under the License. #pragma once +#include "graph_segment/similar_area_searcher.hpp" + #include #include @@ -31,18 +33,15 @@ class GraphSegment : public rclcpp::Node private: const float target_height_ratio_; const int target_candidate_box_width_; - const float similarity_score_threshold_; rclcpp::Subscription::SharedPtr sub_image_; rclcpp::Publisher::SharedPtr pub_mask_image_; rclcpp::Publisher::SharedPtr pub_debug_image_; cv::Ptr segmentation_; + std::unique_ptr similar_area_searcher_{nullptr}; void on_image(const Image & msg); - std::set search_similar_areas( - const cv::Mat & rgb_image, const cv::Mat & segmented, int best_roadlike_class); - int search_most_road_like_class(const cv::Mat & segmented) const; void draw_and_publish_image( diff --git a/localization/yabloc/imgproc/graph_segment/include/graph_segment/similar_area_searcher.hpp b/localization/yabloc/imgproc/graph_segment/include/graph_segment/similar_area_searcher.hpp new file mode 100644 index 0000000000000..070eaaadfee17 --- /dev/null +++ b/localization/yabloc/imgproc/graph_segment/include/graph_segment/similar_area_searcher.hpp @@ -0,0 +1,40 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once +#include +#include +#include + +#include + +namespace pcdless::graph_segment +{ +class SimilarAreaSearcher +{ +public: + SimilarAreaSearcher(float similarity_score_threshold) + : similarity_score_threshold_(similarity_score_threshold), + logger_(rclcpp::get_logger("similar_area_searcher")) + { + } + + std::set search( + const cv::Mat & rgb_image, const cv::Mat & segmented, int best_roadlike_class); + +private: + const float similarity_score_threshold_; + rclcpp::Logger logger_; +}; +} // namespace pcdless::graph_segment diff --git a/localization/yabloc/imgproc/graph_segment/src/graph_segment_core.cpp b/localization/yabloc/imgproc/graph_segment/src/graph_segment_core.cpp index 6f4b81fbb466f..729c32b96178c 100644 --- a/localization/yabloc/imgproc/graph_segment/src/graph_segment_core.cpp +++ b/localization/yabloc/imgproc/graph_segment/src/graph_segment_core.cpp @@ -21,15 +21,12 @@ #include #include -#include - namespace pcdless::graph_segment { GraphSegment::GraphSegment() : Node("graph_segment"), target_height_ratio_(declare_parameter("target_height_ratio", 0.85)), - target_candidate_box_width_(declare_parameter("target_candidate_box_width", 15)), - similarity_score_threshold_(declare_parameter("similarity_score_threshold", 0.8)) + target_candidate_box_width_(declare_parameter("target_candidate_box_width", 15)) { using std::placeholders::_1; @@ -44,78 +41,12 @@ GraphSegment::GraphSegment() const float k = declare_parameter("k", 300); const int min_size = declare_parameter("min_size", 100); segmentation_ = cv::ximgproc::segmentation::createGraphSegmentation(sigma, k, min_size); -} - -std::set GraphSegment::search_similar_areas( - const cv::Mat & rgb_image, const cv::Mat & segmented, int best_roadlike_class) -{ - std::unordered_map histogram_map; - std::unordered_map count_map; - - for (int h = 0; h < rgb_image.rows; h++) { - const int * seg_ptr = segmented.ptr(h); - const cv::Vec3b * rgb_ptr = rgb_image.ptr(h); - - for (int w = 0; w < rgb_image.cols; w++) { - int key = seg_ptr[w]; - cv::Vec3b rgb = rgb_ptr[w]; - if (count_map.count(key) == 0) { - count_map[key] = 1; - histogram_map[key].add(rgb); - } else { - count_map[key]++; - histogram_map[key].add(rgb); - } - } - } - - struct KeyAndArea - { - KeyAndArea(int key, int count) : key(key), count(count) {} - int key; - int count; - }; - - auto compare = [](KeyAndArea a, KeyAndArea b) { return a.count < b.count; }; - std::priority_queue, decltype(compare)> key_queue{compare}; - for (auto [key, count] : count_map) { - key_queue.push({key, count}); - } - Eigen::MatrixXf ref_histogram = histogram_map.at(best_roadlike_class).eval(); - - std::stringstream debug_ss; - debug_ss << "histogram equality "; - - int index = 0; - std::set acceptable_keys; - while (!key_queue.empty()) { - KeyAndArea key = key_queue.top(); - key_queue.pop(); - - Eigen::MatrixXf query = histogram_map.at(key.key).eval(); - float score = Histogram::eval_histogram_intersection(ref_histogram, query); - debug_ss << " " << score; - - if (score > similarity_score_threshold_) acceptable_keys.insert(key.key); - if (++index > 10) break; + // additional area pickup module + if (declare_parameter("pickup_additional_areas", true)) { + similar_area_searcher_ = std::make_unique( + declare_parameter("similarity_score_threshold", 0.8)); } - RCLCPP_INFO_STREAM(get_logger(), debug_ss.str()); - - // // DEBUG: Visualilze - // cv::Mat new_segmented = rgb_image.clone(); - // for (int h = 0; h < rgb_image.rows; h++) { - // const int * seg_ptr = segmented.ptr(h); - // cv::Vec3b * rgb_ptr = new_segmented.ptr(h); - - // for (int w = 0; w < rgb_image.cols; w++) { - // int key = seg_ptr[w]; - // if (acceptable_keys.count(key)) rgb_ptr[w] = cv::Vec3b(0, 0, 255); - // if (key == best_roadlike_class) rgb_ptr[w] = cv::Vec3b(0, 255, 255); - // } - // } - - return acceptable_keys; } cv::Vec3b random_hsv(int index) @@ -169,7 +100,10 @@ void GraphSegment::on_image(const Image & msg) // int target_class = search_most_road_like_class(segmented); // - std::set road_keys = search_similar_areas(resized, segmented, target_class); + std::set road_keys = {target_class}; + if (similar_area_searcher_) { + road_keys = similar_area_searcher_->search(resized, segmented, target_class); + } // Draw output image and debug image // TODO: use ptr instead of at() diff --git a/localization/yabloc/imgproc/graph_segment/src/similar_area_searcher.cpp b/localization/yabloc/imgproc/graph_segment/src/similar_area_searcher.cpp new file mode 100644 index 0000000000000..d69da5ac94b9b --- /dev/null +++ b/localization/yabloc/imgproc/graph_segment/src/similar_area_searcher.cpp @@ -0,0 +1,96 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "graph_segment/similar_area_searcher.hpp" + +#include "graph_segment/histogram.hpp" + +#include + +#include + +namespace pcdless::graph_segment +{ +struct KeyAndArea +{ + KeyAndArea(int key, int count) : key(key), count(count) {} + int key; + int count; +}; + +std::set SimilarAreaSearcher::search( + const cv::Mat & rgb_image, const cv::Mat & segmented, int best_roadlike_class) +{ + std::unordered_map histogram_map; + std::unordered_map count_map; + + for (int h = 0; h < rgb_image.rows; h++) { + const int * seg_ptr = segmented.ptr(h); + const cv::Vec3b * rgb_ptr = rgb_image.ptr(h); + + for (int w = 0; w < rgb_image.cols; w++) { + int key = seg_ptr[w]; + cv::Vec3b rgb = rgb_ptr[w]; + if (count_map.count(key) == 0) { + count_map[key] = 1; + histogram_map[key].add(rgb); + } else { + count_map[key]++; + histogram_map[key].add(rgb); + } + } + } + + auto compare = [](KeyAndArea a, KeyAndArea b) { return a.count < b.count; }; + std::priority_queue, decltype(compare)> key_queue{compare}; + for (auto [key, count] : count_map) { + key_queue.push({key, count}); + } + + Eigen::MatrixXf ref_histogram = histogram_map.at(best_roadlike_class).eval(); + + std::stringstream debug_ss; + debug_ss << "histogram equality "; + + int index = 0; + std::set acceptable_keys; + while (!key_queue.empty()) { + KeyAndArea key = key_queue.top(); + key_queue.pop(); + + Eigen::MatrixXf query = histogram_map.at(key.key).eval(); + float score = Histogram::eval_histogram_intersection(ref_histogram, query); + debug_ss << " " << score; + + if (score > similarity_score_threshold_) acceptable_keys.insert(key.key); + if (++index > 10) break; + } + RCLCPP_INFO_STREAM(logger_, debug_ss.str()); + + // // DEBUG: Visualilze + // cv::Mat new_segmented = rgb_image.clone(); + // for (int h = 0; h < rgb_image.rows; h++) { + // const int * seg_ptr = segmented.ptr(h); + // cv::Vec3b * rgb_ptr = new_segmented.ptr(h); + + // for (int w = 0; w < rgb_image.cols; w++) { + // int key = seg_ptr[w]; + // if (acceptable_keys.count(key)) rgb_ptr[w] = cv::Vec3b(0, 0, 255); + // if (key == best_roadlike_class) rgb_ptr[w] = cv::Vec3b(0, 255, 255); + // } + // } + + return acceptable_keys; +} +} // namespace pcdless::graph_segment \ No newline at end of file diff --git a/localization/yabloc/particle_filter/gnss_particle_corrector/include/gnss_particle_corrector/gnss_particle_corrector.hpp b/localization/yabloc/particle_filter/gnss_particle_corrector/include/gnss_particle_corrector/gnss_particle_corrector.hpp index b72e20c35a4bb..85bce1f8c62d3 100644 --- a/localization/yabloc/particle_filter/gnss_particle_corrector/include/gnss_particle_corrector/gnss_particle_corrector.hpp +++ b/localization/yabloc/particle_filter/gnss_particle_corrector/include/gnss_particle_corrector/gnss_particle_corrector.hpp @@ -59,6 +59,10 @@ class GnssParticleCorrector : public AbstCorrector void on_ublox(const NavPVT::ConstSharedPtr ublox_msg); void on_pose(const PoseCovStamped::ConstSharedPtr pose_msg); + bool is_gnss_observation_valid( + const Eigen::Matrix3f & sigma, const Eigen::Vector3f & meaned_position, + const Eigen::Vector3f & gnss_position); + ParticleArray weight_particles( const ParticleArray & predicted_particles, const Eigen::Vector3f & pose, bool is_rtk_fixed); diff --git a/localization/yabloc/particle_filter/gnss_particle_corrector/src/gnss_corrector_core.cpp b/localization/yabloc/particle_filter/gnss_particle_corrector/src/gnss_corrector_core.cpp index 8b76f3ec669f2..46c7b1238f560 100644 --- a/localization/yabloc/particle_filter/gnss_particle_corrector/src/gnss_corrector_core.cpp +++ b/localization/yabloc/particle_filter/gnss_particle_corrector/src/gnss_corrector_core.cpp @@ -23,8 +23,8 @@ namespace pcdless::modularized_particle_filter { GnssParticleCorrector::GnssParticleCorrector() : AbstCorrector("gnss_particle_corrector"), - ignore_less_than_float_(declare_parameter("ignore_less_than_float", true)), - mahalanobis_distance_threshold_(declare_parameter("mahalanobis_distance_threshold", 20)), + ignore_less_than_float_(declare_parameter("ignore_less_than_float", true)), + mahalanobis_distance_threshold_(declare_parameter("mahalanobis_distance_threshold", 20.0)), weight_manager_(this) { using std::placeholders::_1; @@ -63,6 +63,26 @@ Eigen::Vector3f extract_enu_vel(const GnssParticleCorrector::NavPVT & msg) return enu_vel; } +bool GnssParticleCorrector::is_gnss_observation_valid( + const Eigen::Matrix3f & sigma, const Eigen::Vector3f & meaned_position, + const Eigen::Vector3f & gnss_position) +{ + Eigen::Matrix3f inv_sigma = sigma.completeOrthogonalDecomposition().pseudoInverse(); + Eigen::Vector3f diff = gnss_position - meaned_position; + diff.z() = 0; + + float mahalanobis_distance = std::sqrt(diff.dot(inv_sigma * diff)); + RCLCPP_INFO_STREAM(get_logger(), "mahalanobis: " << mahalanobis_distance); + + if (mahalanobis_distance > mahalanobis_distance_threshold_) { + RCLCPP_WARN_STREAM( + get_logger(), "Mahalanobis distance is too large: " << mahalanobis_distance << ">" + << mahalanobis_distance_threshold_); + return false; + } + return true; +} + void GnssParticleCorrector::on_ublox(const NavPVT::ConstSharedPtr ublox_msg) { const rclcpp::Time stamp = common::ublox_time_to_stamp(*ublox_msg); @@ -101,6 +121,7 @@ void GnssParticleCorrector::on_ublox(const NavPVT::ConstSharedPtr ublox_msg) } std::optional opt_particles = get_synchronized_particle_array(stamp); + if (!opt_particles.has_value()) return; auto dt = (stamp - rclcpp::Time(opt_particles->header.stamp)); if (std::abs(dt.seconds()) > 0.1) { @@ -108,26 +129,13 @@ void GnssParticleCorrector::on_ublox(const NavPVT::ConstSharedPtr ublox_msg) get_logger(), "Timestamp gap between gnss and particles is too large: " << dt.seconds()); } + const Eigen::Matrix3f sigma = modularized_particle_filter::std_of_distribution(*opt_particles); const geometry_msgs::msg::Pose meaned_pose = mean_pose(opt_particles.value()); + const Eigen::Vector3f meaned_position = common::pose_to_affine(meaned_pose).translation(); // Check validity of GNSS measurement by mahalanobis distance - { - Eigen::Matrix3f sigma = modularized_particle_filter::std_of_distribution(*opt_particles); - Eigen::Matrix3f inv_sigma = sigma.completeOrthogonalDecomposition().pseudoInverse(); - - Eigen::Vector3f meaned_position = common::pose_to_affine(meaned_pose).translation(); - Eigen::Vector3f diff = gnss_position - meaned_position; - diff.z() = 0; - - float mahalanobis_distance = std::sqrt(diff.dot(inv_sigma * diff)); - RCLCPP_INFO_STREAM(get_logger(), "mahalanobis: " << mahalanobis_distance); - - if (mahalanobis_distance > mahalanobis_distance_threshold_) { - RCLCPP_WARN_STREAM( - get_logger(), "Mahalanobis distance is too large: " << mahalanobis_distance << ">" - << mahalanobis_distance_threshold_); - return; - } + if (!is_gnss_observation_valid(sigma, meaned_position, gnss_position)) { + return; } ParticleArray weighted_particles = diff --git a/localization/yabloc/pcdless_launch/launch/impl/imgproc.launch.xml b/localization/yabloc/pcdless_launch/launch/impl/imgproc.launch.xml index ae39641ec1c55..4297230ed64e1 100644 --- a/localization/yabloc/pcdless_launch/launch/impl/imgproc.launch.xml +++ b/localization/yabloc/pcdless_launch/launch/impl/imgproc.launch.xml @@ -6,6 +6,7 @@ + @@ -13,7 +14,8 @@ - + + + diff --git a/localization/yabloc/pcdless_launch/launch/impl/pf.launch.xml b/localization/yabloc/pcdless_launch/launch/impl/pf.launch.xml index 61f46b3d87148..0a817bde486ad 100644 --- a/localization/yabloc/pcdless_launch/launch/impl/pf.launch.xml +++ b/localization/yabloc/pcdless_launch/launch/impl/pf.launch.xml @@ -12,6 +12,7 @@ + @@ -75,7 +76,7 @@ - + diff --git a/localization/yabloc/pcdless_launch/launch/tier4_expressway.launch.xml b/localization/yabloc/pcdless_launch/launch/tier4_expressway.launch.xml index 1f62bfe3c1bbc..0dc9afe2e9a76 100644 --- a/localization/yabloc/pcdless_launch/launch/tier4_expressway.launch.xml +++ b/localization/yabloc/pcdless_launch/launch/tier4_expressway.launch.xml @@ -11,6 +11,8 @@ + +