[Sprint 25/26][Enhancement] - Gating and Landmark Config

CMakeLists.txt

@@ -23,6 +23,8 @@ find_package(jitsuyo REQUIRED)
 find_package(kansei REQUIRED)
 find_package(kansei_interfaces REQUIRED)
 find_package(keisan REQUIRED)
+find_package(ninshiki_cpp REQUIRED)
+find_package(ninshiki_interfaces REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(basho_interfaces REQUIRED)
@@ -48,6 +50,8 @@ ament_target_dependencies(${PROJECT_NAME}
   kansei
   kansei_interfaces
   keisan
+  ninshiki_cpp
+  ninshiki_interfaces
   rclcpp
   std_msgs
   basho_interfaces)
@@ -85,6 +89,8 @@ ament_export_dependencies(
   kansei
   kansei_interfaces
   keisan
+  ninshiki_cpp
+  ninshiki_interfaces
   rclcpp
   std_msgs
   basho_interfaces)

include/basho/localization/mcl.hpp

@@ -33,8 +33,14 @@ namespace basho
 
 struct ProjectedObject
 {
+  float score;
+  float left;
+  float top;
+  float width;
+  float height;
   std::string label;
-  keisan::Point3 position;
+  keisan::Point2 position;
+  bool has_projection;
 };
 
 struct Particle
@@ -85,6 +91,7 @@ class MCL
 
   void set_odometry_position(const keisan::Point2 & odom) { odometry_position = odom; }
 
+  // MCL configuration
   int num_particles;
   double min_centered_ratio;
   double short_term_avg_ratio;
@@ -93,8 +100,24 @@ class MCL
   double range_sigma;
   double chi_square_gate;
   double uncertainty_threshold;
+
+  // Gating configuration
   keisan::Point2 max_object_distance;
+  double max_roll_angle;
+  double max_pitch_angle;
+  double max_pan_angle;
+  double max_tilt_angle;
+  double confidence_threshold;
+  int min_num_observations;
+
+  // Landmark configuration
   bool generalized_landmark;
+  bool enable_x_intersection;
+  bool enable_l_intersection;
+  bool enable_t_intersection;
+  bool enable_goalpost;
+
+  // Debug
   bool print_debug;
 
   bool apply_localization;

include/basho/model/field.hpp

@@ -37,7 +37,6 @@ struct Field
   std::vector<keisan::Point2> landmarks_T;
   std::vector<keisan::Point2> landmarks_X;
   std::vector<keisan::Point2> landmarks_goalpost;
-  std::vector<keisan::Point2> landmarks_intersection;
 
   Field()
     : width(600),
@@ -82,10 +81,25 @@ struct Field
         {900.0, 390.0}    // Right goal - lower goalpost (on goal line)
       })
     {
-      landmarks_intersection.reserve(landmarks_L.size() + landmarks_T.size() + landmarks_X.size());
-      landmarks_intersection.insert(landmarks_intersection.end(), landmarks_L.begin(), landmarks_L.end());
-      landmarks_intersection.insert(landmarks_intersection.end(), landmarks_T.begin(), landmarks_T.end());
-      landmarks_intersection.insert(landmarks_intersection.end(), landmarks_X.begin(), landmarks_X.end());
+    }
+
+    std::vector<keisan::Point2> get_intersections(bool use_l, bool use_t, bool use_x)
+    {
+      std::vector<keisan::Point2> intersections;
+
+      size_t reserve_size = 0;
+
+      if (use_l) reserve_size += landmarks_L.size();
+      if (use_t) reserve_size += landmarks_T.size();
+      if (use_x) reserve_size += landmarks_X.size();
+
+      intersections.reserve(reserve_size);
+
+      if (use_l) intersections.insert(intersections.end(), landmarks_L.begin(), landmarks_L.end());
+      if (use_t) intersections.insert(intersections.end(), landmarks_T.begin(), landmarks_T.end());
+      if (use_x) intersections.insert(intersections.end(), landmarks_X.begin(), landmarks_X.end());
+
+      return intersections;
     }
 };
 

include/basho/node/basho_node.hpp

@@ -35,6 +35,7 @@
 #include "basho_interfaces/msg/particles.hpp"
 #include "gyakuenki_interfaces/msg/projected_objects.hpp"
 #include "kansei_interfaces/msg/status.hpp"
+#include "ninshiki_interfaces/msg/contours.hpp"
 
 namespace basho
 {
@@ -49,6 +50,7 @@ class BashoNode
   using Point2 = aruku_interfaces::msg::Point2;
   using WalkingStatus = aruku_interfaces::msg::Status;
   using HeadData = atama_interfaces::msg::Head;
+  using Contours = ninshiki_interfaces::msg::Contours;
 
   static std::string get_node_prefix();
 
@@ -78,6 +80,7 @@ class BashoNode
   rclcpp::Subscription<Point2>::SharedPtr delta_position_subscriber;
   rclcpp::Subscription<Point2>::SharedPtr init_localization_subscriber;
   rclcpp::Subscription<HeadData>::SharedPtr head_data_subscriber;
+  rclcpp::Subscription<Contours>::SharedPtr contours_subscriber;
 
   std::shared_ptr<Localization> localization;
 

include/basho/process/localization.hpp

@@ -28,6 +28,7 @@
 #include "nlohmann/json.hpp"
 #include "basho/localization/mcl.hpp"
 #include "basho/localization/fusion.hpp"
+#include "ninshiki_cpp/utils/contours.hpp"
 
 namespace basho
 {
@@ -49,12 +50,17 @@ class Localization
 
   std::shared_ptr<MCL> get_mcl() const;
 
+  void set_field_contours(const ninshiki_cpp::utils::Contours & contours);
+  bool is_landmark_valid(const ProjectedObject & obj);
+
   std::string config_path;
   std::string config_name;
 
 private:
   std::shared_ptr<MCL> mcl;
   Fusion2D fusion;
+
+  ninshiki_cpp::utils::Contours field_contours;
 };
 
 }  // namespace basho

package.xml

@@ -14,6 +14,8 @@
   <depend>kansei</depend>
   <depend>kansei_interfaces</depend>
   <depend>keisan</depend>
+  <depend>ninshiki_cpp</depend>
+  <depend>ninshiki_interfaces</depend>
   <depend>nlohmann-json-dev</depend>
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>

src/basho/localization/mcl.cpp

@@ -54,10 +54,11 @@ void MCL::localize()
 
   update_motion();
 
-  bool evaluate_particles = num_projected_objects > 0;
-  evaluate_particles &= fabs(orientation_roll.degree()) <= 30;
-  evaluate_particles &= fabs(orientation_pitch.degree()) <= 30;
-  evaluate_particles &= fabs(head_pan.degree()) <= 60;
+  bool evaluate_particles = num_projected_objects >= min_num_observations;
+  evaluate_particles &= fabs(orientation_roll.degree()) <= max_roll_angle;
+  evaluate_particles &= fabs(orientation_pitch.degree()) <= max_pitch_angle;
+  evaluate_particles &= fabs(head_pan.degree()) <= max_pan_angle;
+  evaluate_particles &= fabs(head_tilt.degree()) <= max_tilt_angle;
 
   apply_localization = false;
 
@@ -134,7 +135,6 @@ void MCL::clear_projected_objects()
   projected_X.clear();
   projected_L.clear();
   projected_T.clear();
-  projected_intersection.clear();
   projected_goalpost.clear();
   num_projected_objects = 0;
 }
@@ -144,7 +144,7 @@ void MCL::update_motion()
   double k_trans = 0.05;
   double min_trans_noise = 0.5;
 
-  double sigma_theta = 5.0;
+  double sigma_theta = 10.0;
 
   double dx = delta_position.x;
   double dy = delta_position.y;
@@ -217,7 +217,7 @@ void MCL::resample_particles_wheel()
 
   std::normal_distribution<double> noise_x(0.0, resample_sigma_x);
   std::normal_distribution<double> noise_y(0.0, resample_sigma_y);
-  std::normal_distribution<double> noise_theta(0.0, 5.0);
+  std::normal_distribution<double> noise_theta(0.0, 10.0);
 
   for (int i = 0; i < num_particles; ++i) {
     if (rand_prob(rand_gen) < prob) {
@@ -283,7 +283,7 @@ void MCL::resample_particles_low_variance()
   // TOOD: get noise value from config
   std::normal_distribution<double> noise_x(0.0, 75.0);
   std::normal_distribution<double> noise_y(0.0, 75.0);
-  std::normal_distribution<double> noise_theta(0.0, 8.0);
+  std::normal_distribution<double> noise_theta(0.0, 10.0);
 
   for (int i = 0; i < num_random; ++i) {
     int idx = num_particles - 1 - i;
@@ -297,7 +297,7 @@ void MCL::resample_particles_low_variance()
       (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize();
 
     // Assign small weight so it does not dominate immediately
-    new_particles[idx].weight = 1e-3;
+    new_particles[idx].weight = 1e-6;
   }
 
   int num_inject_odom = 0;
@@ -313,7 +313,7 @@ void MCL::resample_particles_low_variance()
       new_particles[idx].position.y = odometry_position.y + noise_y(rand_gen);
       new_particles[idx].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize();
 
-      new_particles[idx].weight = 1e-3;
+      new_particles[idx].weight = 1e-6;
     }
   }
 
@@ -330,7 +330,7 @@ void MCL::resample_particles_low_variance()
         new_particles[idx].position.y = goalpost_hypothesis.y + noise_y(rand_gen);
         new_particles[idx].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize();
 
-        new_particles[idx].weight = 1e-3;
+        new_particles[idx].weight = 1e-6;
       }
     }
   }
@@ -483,7 +483,9 @@ double MCL::calculate_total_likelihood(const Particle & particle)
   };
 
   if (generalized_landmark) {
-    landmark_groups.push_back({&projected_intersection, &field.landmarks_intersection});
+  auto field_intersections = field.get_intersections(
+    enable_l_intersection, enable_t_intersection, enable_x_intersection);
+  landmark_groups.push_back({&projected_intersection, &field_intersections});
   } else {
     landmark_groups.push_back({&projected_X, &field.landmarks_X});
     landmark_groups.push_back({&projected_L, &field.landmarks_L});
@@ -502,7 +504,7 @@ double MCL::calculate_total_likelihood(const Particle & particle)
       for (int j = 0; j < group.landmarks->size(); ++j) {
         if (result[i][j] == 1) {
           if (cost_matrix[i][j] >= 1e8) {
-            log_total += std::log(1e-3);
+            log_total += std::log(1e-6);
             valid_associations++;
             continue;
           }
@@ -683,7 +685,7 @@ void MCL::print_particles() {
     return;
   }
 
-  printf("\033c");
+  printf("\033[2J\033[H");
   printf("Particles num: %d\n", particles.size());
   printf("Prob: %.5f\n", prob);
   printf("Short term avg: %.8f\n", short_term_avg);

src/basho/node/basho_node.cpp

@@ -77,26 +77,21 @@ BashoNode::BashoNode(
       10,
       [this](const ProjectedObjects::SharedPtr msg) {
         auto mcl = this->localization->get_mcl();
-
         mcl->clear_projected_objects();
 
         for (const auto & obj : msg->projected_objects) {
-          const bool ignore =
-            obj.label == "ball" ||
-            obj.label == "robot" ||
-            obj.label == "self" ||
-            std::abs(obj.position.x * 100) > mcl->max_object_distance.x ||
-            std::abs(obj.position.y * 100) > mcl->max_object_distance.y;
-
-          if (ignore) {
-            continue;
-          }
-
           ProjectedObject projected{
+            obj.confidence,
+            obj.left, obj.top, obj.right, obj.bottom,
             obj.label,
-            {obj.position.x, obj.position.y, obj.position.z}
+            {obj.position.x, obj.position.y},
+            obj.has_projection
           };
 
+          if (!this->localization->is_landmark_valid(projected)) {
+            continue;
+          }
+
           if (obj.label == "goalpost") {
             mcl->projected_goalpost.push_back(projected);
           } else if (mcl->generalized_landmark) {
@@ -113,6 +108,26 @@ BashoNode::BashoNode(
         }
       });
 
+  contours_subscriber =
+    node->create_subscription<Contours>(
+      "/ninshiki_cpp/color_detection",
+      10,
+      [this](const Contours::SharedPtr msg) {
+        ninshiki_cpp::utils::Contours contours;
+
+        for (const auto & contour : msg->contours) {
+          if (contour.name != "field") continue;
+
+          std::vector<cv::Point> temp_contour;
+          for (const auto & point : contour.contour) {
+            temp_contour.emplace_back(point.x, point.y);
+          }
+
+          contours.contours.push_back(temp_contour);
+        }
+        this->localization->set_field_contours(contours);
+      });
+
   init_localization_subscriber =
     node->create_subscription<Point2>(
       get_node_prefix() + "/init_localization",