[PI Sprint 25/26] [Feature] - Add LAB Color Classification

include/ninshiki_cpp/detector/color_detector.hpp

@@ -53,6 +53,7 @@ class ColorDetector : public Detector
 
   cv::Mat classify(cv::Mat input);
   cv::Mat classify_gray(cv::Mat input);
+  cv::Mat classify_lab(cv::Mat input);
 
   int get_min_hue() {return min_hue;}
   int get_max_hue() {return max_hue;}
@@ -68,6 +69,20 @@ class ColorDetector : public Detector
   void set_min_value(int value) {min_value = keisan::clamp(value, 0, 100);}
   void set_max_value(int value) {max_value = keisan::clamp(value, 0, 100);}
 
+  int get_min_lightness() {return min_hue;}
+  int get_max_lightness() {return max_lightness;}
+  int get_min_a() {return min_a;}
+  int get_max_a() {return max_a;}
+  int get_min_b() {return min_b;}
+  int get_max_b() {return max_b;}
+
+  void set_min_lightness(int value) {min_lightness = keisan::clamp(value, 0, 255);}
+  void set_max_lightness(int value) {max_lightness = keisan::clamp(value, 0, 255);}
+  void set_min_a(int value) {min_a = keisan::clamp(value, -128, 128);}
+  void set_max_a(int value) {max_a = keisan::clamp(value, -128, 128);}
+  void set_min_b(int value) {min_b = keisan::clamp(value, -128, 128);}
+  void set_max_b(int value) {max_b = keisan::clamp(value, -128, 128);}
+
   // Function for Contours
   void find(cv::Mat binary_mat);
 
@@ -80,12 +95,19 @@ class ColorDetector : public Detector
   int classifier_type;
 
   bool invert_hue;
+  bool use_lab;
   int min_hue;
   int max_hue;
   int min_saturation;
   int max_saturation;
   int min_value;
   int max_value;
+  int min_lightness;
+  int max_lightness;
+  int min_a;
+  int max_a;
+  int min_b;
+  int max_b;
 
   std::vector<std::vector<cv::Point>> contours;
   std::vector<utils::Color> colors;

include/ninshiki_cpp/node/ninshiki_cpp_node.hpp

@@ -73,7 +73,6 @@ class NinshikiCppNode
   std::shared_ptr<LBPDetector> lbp_detection;
 
   cv::Mat received_frame;
-  cv::Mat hsv_frame;
 
   int count = 0;
 

include/ninshiki_cpp/utils/circle.hpp

@@ -36,7 +36,7 @@ class Circle
 public:
   Circle(const std::vector<cv::Point> & contour);
 
-  void draw(cv::Mat & image, int line_size) const;
+  void draw(cv::Mat & image, int line_size, const cv::Scalar & color = cv::Scalar(0, 255, 238)) const;
   cv::Mat get_binary_mat(const cv::Size & mat_size, int line_size = cv::FILLED);
 
   const cv::Point2f & get_center() const;

include/ninshiki_cpp/utils/color.hpp

@@ -30,18 +30,26 @@ class Color
 {
 public:
   Color(
-    const std::string & name, bool invert_hue, int min_hue,
-    int max_hue, int min_saturation, int max_saturation,
-    int min_value, int max_value);
+    const std::string & name, bool invert_hue, bool use_lab,
+    int min_hue, int max_hue, int min_saturation, int max_saturation,
+    int min_value, int max_value, int min_lightness, int max_lightness,
+    int min_a, int max_a, int min_b, int max_b);
 
   std::string name;
   bool invert_hue;
+  bool use_lab;
   int min_hue;
   int max_hue;
   int min_saturation;
   int max_saturation;
   int min_value;
   int max_value;
+  int min_lightness;
+  int max_lightness;
+  int min_a;
+  int max_a;
+  int min_b;
+  int max_b;
 };
 
 }  // namespace ninshiki_cpp::utils

launch/ninshiki_cpp_launch.py

@@ -1,15 +1,15 @@
 # Copyright (c) 2024 ICHIRO ITS
-# 
+#
 # Permission is hereby granted, free of charge, to any person obtaining a copy
 # of this software and associated documentation files (the "Software"), to deal
 # in the Software without restriction, including without limitation the rights
 # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 # copies of the Software, and to permit persons to whom the Software is
 # furnished to do so, subject to the following conditions:
-# 
+#
 # The above copyright notice and this permission notice shall be included in
 # all copies or substantial portions of the Software.
-# 
+#
 # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
@@ -25,9 +25,9 @@
 
 def generate_launch_description():
     hostname = socket.gethostname()
-    ninshiki_config_path = os.path.expanduser(f'~/ros2-ws/configuration/{hostname}/detection/')
-    shisen_config_path = os.path.expanduser(f'~/ros2-ws/configuration/{hostname}/camera/')
-    
+    ninshiki_config_path = os.path.expanduser(f'~/ichiro-ws/configuration/{hostname}/detection/')
+    shisen_config_path = os.path.expanduser(f'~/ichiro-ws/configuration/{hostname}/camera/')
+
     return LaunchDescription([
         Node(
             package='shisen_cpp',

src/ninshiki_cpp/config/node/config_node.cpp

@@ -73,9 +73,13 @@ ConfigNode::ConfigNode(rclcpp::Node::SharedPtr node, const std::string & path,
         utils::Color color(
           request->name,
           request->invert_hue,
+          request->use_lab,
           request->min_hue, request->max_hue,
           request->min_saturation, request->max_saturation,
-          request->min_value, request->max_value
+          request->min_value, request->max_value,
+          request->min_lightness, request->max_lightness,
+          request->min_a, request->max_a,
+          request->min_b, request->max_b
         );
 
         color_detection->configure_color_setting(color);

src/ninshiki_cpp/detector/color_detector.cpp

@@ -67,12 +67,19 @@ bool ColorDetector::load_configuration(const std::string & path)
       utils::Color color(
         item.key(),
         item.value().at("invert_hue").get<bool>(),
+        item.value().at("use_lab").get<bool>(),
         item.value().at("min_hsv")[0],
         item.value().at("max_hsv")[0],
         item.value().at("min_hsv")[1],
         item.value().at("max_hsv")[1],
         item.value().at("min_hsv")[2],
-        item.value().at("max_hsv")[2]
+        item.value().at("max_hsv")[2],
+        item.value().at("min_lab")[0],
+        item.value().at("max_lab")[0],
+        item.value().at("min_lab")[1],
+        item.value().at("max_lab")[1],
+        item.value().at("min_lab")[2],
+        item.value().at("max_lab")[2]
       );
 
       colors.push_back(color);
@@ -98,14 +105,20 @@ bool ColorDetector::save_configuration()
 
   for (auto & item : colors) {
     bool invert_hue = item.invert_hue;
+    bool use_lab = item.use_lab;
     int min_hsv[] = {item.min_hue, item.min_saturation, item.min_value};
     int max_hsv[] = {item.max_hue, item.max_saturation, item.max_value};
+    int min_lab[] = {item.min_lightness, item.min_a, item.min_b};
+    int max_lab[] = {item.max_lightness, item.max_a, item.max_b};
 
     nlohmann::json color = {
       {item.name, {
         {"invert_hue", invert_hue},
+        {"use_lab", use_lab},
         {"min_hsv", min_hsv},
-        {"max_hsv", max_hsv}
+        {"max_hsv", max_hsv},
+        {"min_lab", min_lab},
+        {"max_lab", max_lab},
       }}
     };
 
@@ -133,12 +146,19 @@ void ColorDetector::configure_color_setting(utils::Color color)
   for (auto & item : colors) {
     if (item.name == color.name) {
       item.invert_hue = color.invert_hue;
+      item.use_lab = color.use_lab;
       item.min_hue = color.min_hue;
       item.max_hue = color.max_hue;
       item.min_saturation = color.min_saturation;
       item.max_saturation = color.max_saturation;
       item.min_value = color.min_value;
       item.max_value = color.max_value;
+      item.min_lightness = color.min_lightness;
+      item.max_lightness = color.max_lightness;
+      item.min_a = color.min_a;
+      item.max_a = color.max_a;
+      item.min_b = color.min_b;
+      item.max_b = color.max_b;
 
       break;
     }
@@ -178,11 +198,7 @@ cv::Mat ColorDetector::classify(cv::Mat input)
 
     cv::bitwise_or(mask1, mask2, output);
   } else {
-    cv::inRange(input,
-      cv::Scalar(h_min, s_min, v_min),
-      cv::Scalar(h_max, s_max, v_max),
-      output
-    );
+    cv::inRange(input, hsv_min, hsv_max, output);
   }
 
   cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3, 3), cv::Point(1, 1));
@@ -191,6 +207,33 @@ cv::Mat ColorDetector::classify(cv::Mat input)
   return output;
 }
 
+cv::Mat ColorDetector::classify_lab(cv::Mat input)
+{
+  int l_min = min_lightness;
+  int l_max = max_lightness;
+
+  int a_min = min_a + 128;
+  int a_max = max_a + 128;
+
+  int b_min = min_b + 128;
+  int b_max = max_b + 128;
+
+  printf("LAB min: [%d, %d, %d]\n", l_min, a_min, b_min);
+  printf("LAB max: [%d, %d, %d]\n", l_max, a_max, b_max);
+
+  cv::Scalar lab_min = cv::Scalar(l_min, a_min, b_min);
+  cv::Scalar lab_max = cv::Scalar(l_max, a_max, b_max);
+
+  cv::Mat output = input.clone();
+
+  cv::inRange(input, lab_min, lab_max, output);
+
+  cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3, 3), cv::Point(1, 1));
+  cv::morphologyEx(output, output, cv::MORPH_CLOSE, element);
+
+  return output;
+}
+
 cv::Mat ColorDetector::classify_gray(cv::Mat input)
 {
   int v_min = (min_value * 255) / 100;
@@ -229,15 +272,31 @@ void ColorDetector::detection(const cv::Mat & image)
   for (auto & color : colors) {
     color_name = color.name;
     invert_hue = color.invert_hue;
+    use_lab = color.use_lab;
     min_hue = color.min_hue;
     max_hue = color.max_hue;
     min_saturation = color.min_saturation;
     max_saturation = color.max_saturation;
     min_value = color.min_value;
     max_value = color.max_value;
-
-    cv::Mat field_binary_mat = classify(image);
-    find(field_binary_mat);
+    min_lightness = color.min_lightness;
+    max_lightness = color.max_lightness;
+    min_a = color.min_a;
+    max_a = color.max_a;
+    min_b = color.min_b;
+    max_b = color.max_b;
+
+    if (!use_lab) {
+      cv::Mat hsv_image;
+      cv::cvtColor(image, hsv_image, cv::COLOR_BGR2HSV);
+      cv::Mat field_binary_mat = classify(hsv_image);
+      find(field_binary_mat);
+    } else {
+      cv::Mat lab_image;
+      cv::cvtColor(image, lab_image, cv::COLOR_BGR2Lab);
+      cv::Mat field_binary_mat = classify_lab(lab_image);
+      find(field_binary_mat);
+    }
 
     // Copy contours to ros2 msg
     if (contours.size() >= 0) {

src/ninshiki_cpp/node/ninshiki_cpp_node.cpp

@@ -46,15 +46,14 @@ NinshikiCppNode::NinshikiCppNode(
   image_subscriber =
     node->create_subscription<Image>("camera/image", 10, [this](const Image::SharedPtr message) {
       if (!message->data.empty()) {
-        received_frame = cv_bridge::toCvShare(message)->image;
+        received_frame = cv_bridge::toCvCopy(message, "bgr8")->image;
       }
     });
 
   node_timer = node->create_wall_timer(
     std::chrono::milliseconds(frequency),
     [this]() {
       if (!received_frame.empty()) {
-        cv::cvtColor(received_frame, hsv_frame, cv::COLOR_BGR2HSV);
         publish();
       }
     }
@@ -71,7 +70,7 @@ void NinshikiCppNode::publish()
   }
 
   if (color_detection) {
-    color_detection->detection(hsv_frame);
+    color_detection->detection(received_frame);
     color_segmentation_publisher->publish(color_detection->detection_result);
 
     color_detection->detection_result.contours.clear();

src/ninshiki_cpp/utils/circle.cpp

@@ -28,9 +28,9 @@ Circle::Circle(const std::vector<cv::Point> & contour) : center(cv::Point2f(-1,
   cv::minEnclosingCircle(contour, center, radius);
 }
 
-void Circle::draw(cv::Mat & image, int line_size) const
+void Circle::draw(cv::Mat & image, int line_size, const cv::Scalar & color) const
 {
-  cv::circle(image, center, radius, cv::Scalar(0, 255, 238), line_size);
+  cv::circle(image, center, radius, color, line_size);
 }
 
 cv::Mat Circle::get_binary_mat(const cv::Size & mat_size, int line_size)

src/ninshiki_cpp/utils/color.cpp

@@ -26,12 +26,35 @@ namespace ninshiki_cpp::utils
 {
 
 Color::Color(
-  const std::string & name, bool invert_hue, int min_hue,
-  int max_hue, int min_saturation, int max_saturation,
-  int min_value, int max_value)
-: name(name), invert_hue(invert_hue), min_hue(min_hue), max_hue(max_hue),
-  min_saturation(min_saturation), max_saturation(max_saturation),
-  min_value(min_value), max_value(max_value)
+  const std::string & name,
+  bool invert_hue,
+  bool use_lab,
+  int min_hue,
+  int max_hue,
+  int min_saturation,
+  int max_saturation,
+  int min_value,
+  int max_value,
+  int min_lightness,
+  int max_lightness,
+  int min_a,
+  int max_a,
+  int min_b,
+  int max_b)
+: name(name),
+  invert_hue(invert_hue),
+  use_lab(use_lab),
+  min_hue(min_hue),
+  max_hue(max_hue),
+  min_saturation(min_saturation),
+  max_saturation(max_saturation),
+  min_value(min_value),
+  max_value(max_value),
+  min_lightness(min_lightness),
+  max_lightness(max_lightness),min_a(min_a),
+  max_a(max_a),
+  min_b(min_b),
+  max_b(max_b)
 {
 }