Merge pull request #3 from StrayedCats/feat/add_simple_detector

Add panel simple detector with red panel color

detector2d_base/include/detector2d_base/detector2d_base.hpp

@@ -1,3 +1,17 @@
+// Copyright 2023 StrayedCats.
+//
+// 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 <detector2d_param/detector2d_param.hpp>

detector2d_node/include/detector2d_node/detector2d_node.hpp

@@ -1,3 +1,17 @@
+// Copyright 2023 StrayedCats.
+//
+// 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 <cv_bridge/cv_bridge.h>

detector2d_node/src/detector2d_node.cpp

@@ -1,3 +1,17 @@
+// Copyright 2023 StrayedCats.
+//
+// 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 <detector2d_node/detector2d_node.hpp>
 
 namespace detector2d_node

detector2d_param/src/detector2d_parameters.yaml

@@ -1,6 +1,14 @@
 detector2d_parameters:
+  debug:
+    type: bool
+    description: "debug"
+    default_value: false
   load_target_plugin:
     type: string
     description: "load target plugin"
     default_value: "detector2d_plugins::PublishCenter"
-
+  panel_simple_detector:
+    panel_color:
+      type: string
+      description: "panel color"
+      default_value: "red"

detector2d_plugins/CMakeLists.txt

@@ -25,7 +25,8 @@ if(BUILD_TESTING)
   find_package(ament_cmake_gmock REQUIRED)
 
   set(TEST_TARGETS
-    test_publish_center_plugin)
+    test_publish_center_plugin
+    test_panel_simple_detector_plugin)
   foreach(TARGET ${TEST_TARGETS})
     ament_add_gtest(${TARGET} test/src/${TARGET}.cpp)
     target_link_libraries(${TARGET} ${PROJECT_NAME} ${OpenCV_LIBRARIES})
@@ -35,4 +36,6 @@ endif()
 pluginlib_export_plugin_description_file(detector2d_base detector2d_plugins.xml)
 
 include_directories(include)
-ament_auto_package()
+ament_auto_package(INSTALL_TO_SHARE
+  test/test_data
+)

detector2d_plugins/detector2d_plugins.xml

@@ -2,4 +2,7 @@
     <class type="detector2d_plugins::PublishCenter" base_class_type="detector2d_base::Detector">
         <description>publish image center example</description>
     </class>
+    <class type="detector2d_plugins::PanelSimpleDetector" base_class_type="detector2d_base::Detector">
+        <description>panel simple detector example</description>
+    </class>
 </library>

detector2d_plugins/include/detector2d_plugins/panel_simple_detector.hpp

@@ -0,0 +1,43 @@
+// Copyright 2023 StrayedCats.
+//
+// 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 <detector2d_base/detector2d_base.hpp>
+#include <detector2d_param/detector2d_param.hpp>
+#include <vision_msgs/msg/detection2_d_array.hpp>
+
+#include <memory>
+
+namespace detector2d_plugins
+{
+typedef vision_msgs::msg::Detection2DArray Detection2DArray;
+typedef std::vector<std::vector<cv::Point>> Contours;
+typedef std::vector<cv::Point> CenterPoints;
+
+class PanelSimpleDetector : public detector2d_base::Detector
+{
+public:
+  void init(const detector2d_parameters::ParamListener &) override;
+  Detection2DArray detect(const cv::Mat &) override;
+
+private:
+  void draw(const cv::Mat &, const CenterPoints &, const std::string &);
+  CenterPoints get_center_points(const Contours &);
+  CenterPoints merge_center_points(const CenterPoints &, const int &);
+
+private:
+  detector2d_parameters::Params params_;
+};
+}

detector2d_plugins/include/detector2d_plugins/publish_center_plugin.hpp

@@ -1,3 +1,17 @@
+// Copyright 2023 StrayedCats.
+//
+// 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 <detector2d_base/detector2d_base.hpp>

detector2d_plugins/src/panel_simple_detector.cpp

@@ -0,0 +1,121 @@
+// Copyright 2023 StrayedCats.
+//
+// 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 "detector2d_plugins/panel_simple_detector.hpp"
+
+namespace detector2d_plugins
+{
+
+void PanelSimpleDetector::init(const detector2d_parameters::ParamListener & param_listener)
+{
+  this->params_ = param_listener.get_params();
+}
+
+Detection2DArray PanelSimpleDetector::detect(const cv::Mat & image)
+{
+  auto panel_color_ = this->params_.panel_simple_detector.panel_color;
+  auto debug = this->params_.debug;
+
+  cv::Mat3b hsv;
+  cv::cvtColor(image, hsv, cv::COLOR_BGR2HSV);
+
+  cv::Mat1b image_mask = cv::Mat1b::zeros(hsv.rows, hsv.cols);
+
+  // red
+  for (int i = 0; i < hsv.rows; i++) {
+    for (int j = 0; j < hsv.cols; j++) {
+      if ((hsv(i, j)[0] > 0 && hsv(i, j)[0] < 20) || (hsv(i, j)[0] > 170 && hsv(i, j)[0] < 180)) {
+        image_mask(i, j) = hsv(i, j)[2] > 30 ? 255 : 0;
+      } else {
+        image_mask(i, j) = 0;
+      }
+    }
+  }
+
+  // TODO: blue
+
+  cv::medianBlur(image_mask, image_mask, 7);
+
+  // find contours
+  Contours contours;
+  cv::findContours(image_mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
+  CenterPoints center_points = get_center_points(contours);
+  CenterPoints merged_center_points = merge_center_points(center_points, image.cols);
+
+  if (debug) {
+    draw(image, merged_center_points, "merged_center_points");
+  }
+
+  Detection2DArray pose;
+  for (auto center_point : merged_center_points) {
+    vision_msgs::msg::Detection2D detection;
+    detection.bbox.center.position.x = center_point.x;
+    detection.bbox.center.position.y = center_point.y;
+    detection.bbox.size_x = 10;
+    detection.bbox.size_y = 10;
+    detection.results.resize(1);
+    pose.detections.push_back(detection);
+  }
+  return pose;
+}
+
+void PanelSimpleDetector::draw(
+  const cv::Mat & image, const CenterPoints & center_points,
+  const std::string & window_name)
+{
+  cv::Mat3b canvas = image.clone();
+  for (auto center_point : center_points) {
+    cv::circle(canvas, center_point, 2, cv::Scalar(0, 255, 0), 2);
+  }
+  cv::imshow(window_name, canvas);
+  cv::waitKey(1);
+}
+
+CenterPoints PanelSimpleDetector::get_center_points(const Contours & contours)
+{
+  CenterPoints center_points;
+  for (auto contour : contours) {
+    cv::Rect rect = cv::boundingRect(contour);
+    center_points.push_back(cv::Point(rect.x + rect.width / 2, rect.y + rect.height / 2));
+  }
+  return center_points;
+}
+
+CenterPoints PanelSimpleDetector::merge_center_points(
+  const CenterPoints & center_points,
+  const int & width)
+{
+  CenterPoints merged_center_points;
+  uint32_t threshold = width * 0.05;
+  for (auto center_point : center_points) {
+    bool merged = false;
+    for (size_t i = 0; i < merged_center_points.size(); i++) {
+      if (abs(center_point.x - merged_center_points[i].x) < threshold) {
+        merged_center_points[i].x = (center_point.x + merged_center_points[i].x) / 2;
+        merged_center_points[i].y = (center_point.y + merged_center_points[i].y) / 2;
+        merged = true;
+        break;
+      }
+    }
+    if (!merged) {
+      merged_center_points.push_back(center_point);
+    }
+  }
+  return merged_center_points;
+
+}
+}
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(detector2d_plugins::PanelSimpleDetector, detector2d_base::Detector)

detector2d_plugins/src/publish_center_plugin.cpp

@@ -1,3 +1,17 @@
+// Copyright 2023 StrayedCats.
+//
+// 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 "detector2d_plugins/publish_center_plugin.hpp"
 
 namespace detector2d_plugins

detector2d_plugins/test/src/test_panel_simple_detector_plugin.cpp

@@ -0,0 +1,71 @@
+// Copyright 2023 StrayedCats.
+//
+// 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 <ament_index_cpp/get_package_share_directory.hpp>
+#include <gtest/gtest.h>
+#include <opencv2/opencv.hpp>
+#include <detector2d_base/detector2d_base.hpp>
+#include <detector2d_param/detector2d_param.hpp>
+#include <detector2d_plugins/panel_simple_detector.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+class TestDetector2dClass : public ::testing::Test
+{
+protected:
+  std::unique_ptr<detector2d_plugins::PanelSimpleDetector> target_class_;
+  std::shared_ptr<detector2d_parameters::ParamListener> param_listener_;
+
+  std::string image_path;
+
+  virtual void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+    auto node = std::make_shared<rclcpp::Node>("test_node");
+
+    this->image_path = ament_index_cpp::get_package_share_directory("detector2d_plugins") +
+      "/test_data/dark_00.png";
+
+    this->target_class_ =
+      std::make_unique<detector2d_plugins::PanelSimpleDetector>();
+
+    this->param_listener_ =
+      std::make_shared<detector2d_parameters::ParamListener>(
+      node->get_node_parameters_interface());
+    this->target_class_->init(*this->param_listener_);
+  }
+};
+
+TEST_F(TestDetector2dClass, test_default)
+{
+  cv::Mat3b image = cv::imread(this->image_path, cv::IMREAD_COLOR);
+  auto result = this->target_class_->detect(image);
+
+  // bboxes [0]: 468, 328
+  // bboxes [1]: 632, 325
+  // bboxes [2]: 882, 345 <-- remove
+  // bboxes [3]: 797, 298
+  // bboxes [4]: 555, 228
+
+  ASSERT_EQ(result.detections[0].bbox.center.position.x, 468);
+  ASSERT_EQ(result.detections[0].bbox.center.position.y, 328);
+
+  ASSERT_EQ(result.detections[1].bbox.center.position.x, 632);
+  ASSERT_EQ(result.detections[1].bbox.center.position.y, 325);
+
+  ASSERT_EQ(result.detections[3].bbox.center.position.x, 797);
+  ASSERT_EQ(result.detections[3].bbox.center.position.y, 298);
+
+  ASSERT_EQ(result.detections[4].bbox.center.position.x, 555);
+  ASSERT_EQ(result.detections[4].bbox.center.position.y, 228);
+}

detector2d_plugins/test/src/test_publish_center_plugin.cpp

@@ -1,3 +1,17 @@
+// Copyright 2023 StrayedCats.
+//
+// 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 <gtest/gtest.h>
 #include <opencv2/opencv.hpp>
 #include <detector2d_base/detector2d_base.hpp>