Merge pull request #277 from UbiquityRobotics/add_stag_detect

STag Detect
UbiquityRobotics · Sep 23, 2022 · 6c09104 · 6c09104
2 parents 6ccb097 + abb7d02
commit 6c09104
Show file tree

Hide file tree

Showing 65 changed files with 44,305 additions and 3 deletions.
diff --git a/README.md b/README.md
@@ -1,8 +1,6 @@
 
 # Simultaneous Localization and Mapping Using Fiducial Markers
-Travis:  [![Build Status](https://travis-ci.org/UbiquityRobotics/fiducials.svg?branch=kinetic-devel)](https://travis-ci.org/UbiquityRobotics/fiducials)
-
-Jenkins: [![Build Status](http://build.ros.org/view/Kdev/job/Kdev__fiducials__ubuntu_xenial_amd64/badge/icon)](http://build.ros.org/view/Kdev/job/Kdev__fiducials__ubuntu_xenial_amd64/)
+[![License](https://img.shields.io/badge/License-BSD_3--Clause-blue.svg)](LICENSE)
 
 ## Overview
 

diff --git a/stag_detect/CMakeLists.txt b/stag_detect/CMakeLists.txt
@@ -0,0 +1,96 @@
+
+cmake_minimum_required(VERSION 2.8.3)
+project(stag_detect)
+
+find_package(catkin REQUIRED COMPONENTS
+  camera_info_manager
+  cv_bridge
+  geometry_msgs
+  tf
+  roscpp
+  tf2_geometry_msgs
+  tf2_ros
+  tf2
+  visualization_msgs
+  image_transport
+  cv_bridge
+  sensor_msgs
+  std_msgs
+  fiducial_msgs
+  dynamic_reconfigure
+)
+
+find_package(OpenCV REQUIRED)
+find_package(Threads REQUIRED)
+find_package(PkgConfig REQUIRED)
+
+generate_dynamic_reconfigure_options(cfg/DetectorParams.cfg)
+
+catkin_package(
+  INCLUDE_DIRS include src/stag
+  LIBRARIES stag_ros
+  CATKIN_DEPENDS camera_info_manager cv_bridge geometry_msgs image_transport roscpp sensor_msgs std_msgs
+)
+
+###########
+## Build ##
+###########
+
+add_definitions(-std=c++11)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+)
+
+## Runnables
+add_library(stag_core
+  src/stag/Decoder.cpp
+  src/stag/Drawer.cpp
+  src/stag/EDInterface.cpp
+  src/stag/Ellipse.cpp
+  src/stag/Marker.cpp
+  src/stag/PoseRefiner.cpp
+  src/stag/Quad.cpp
+  src/stag/QuadDetector.cpp
+  src/stag/Stag.cpp
+  src/stag/utility.cpp
+  src/stag/ED/ED.cpp
+  src/stag/ED/EDInternals.cpp
+  src/stag/ED/EDLines.cpp
+  src/stag/ED/GradientOperators.cpp
+  src/stag/ED/ImageSmooth.cpp
+  src/stag/ED/LineSegment.cpp
+  src/stag/ED/MyMath.cpp
+  src/stag/ED/NFA.cpp
+# src/stag/ED/Utilities.cpp
+  src/stag/ED/ValidateEdgeSegments.cpp
+  include/stag_ros/load_yaml_tags.h
+)
+
+add_executable(stag_detect src/stag_ros/stag_detect.cpp)
+add_dependencies(stag_detect ${catkin_EXPORTED_TARGETS})
+target_link_libraries(stag_core ${OpenCV_LIBS})
+target_link_libraries(stag_detect ${catkin_LIBRARIES} ${OpenCV_LIBS} stag_core)
+
+#############
+## Install ##
+#############
+
+## Mark executables and/or libraries for installation
+
+install(DIRECTORY launch/
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+)
+
+install(DIRECTORY cfg/
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/cfg
+)
+
+install(TARGETS
+  stag_detect
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
diff --git a/stag_detect/LICENSES/LICENSE_STag b/stag_detect/LICENSES/LICENSE_STag
@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2019 Burak Benligiray
+
+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 THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
diff --git a/stag_detect/LICENSES/LICENSE_STag_ROS b/stag_detect/LICENSES/LICENSE_STag_ROS
@@ -0,0 +1,22 @@
+MIT License
+
+Copyright (c) 2020 Michail Kalaitzakis and Brennan Cain (Unmanned Systems and
+Robotics Lab, University of South Carolina, USA)
+
+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 THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
diff --git a/stag_detect/README.md b/stag_detect/README.md
@@ -0,0 +1,24 @@
+# stag_detect
+
+## Overview
+
+This package consist of files for generating and launching STag marker related software.
+
+The stag_detect node finds STag markers in images stream and estimates 3D transforms from the camera to the fiducials.
+
+Based on:
+- https://github.com/usrl-uofsc/stag_ros
+- https://github.com/bbenligiray/stag
+
+With added support for vision_msgs which are required for full integration.
+
+#### Library HD
+
+You can generate STag markers from any library you prefer. 
+Make sure you set the corresponding `libraryHD` parameter in launch file.
+We performed most of the test with library HD11, which suited our needs.
+
+## Packs
+
+Generally each pack should correspond to a standalone route, but you can generate yourself packs according to your preference.
+Take note that each marker has a unique id, while a number is unique only to the pack.
diff --git a/stag_detect/cfg/DetectorParams.cfg b/stag_detect/cfg/DetectorParams.cfg
@@ -0,0 +1,92 @@
+#!/usr/bin/env python2
+PACKAGE = "stag_detect"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("adaptiveThreshConstant",                 double_t, 0,
+        "Constant for adaptive thresholding before finding contours",
+         7, 0)
+
+gen.add("adaptiveThreshWinSizeMin",               int_t,    0,
+        "Minimum window size for adaptive thresholding before finding contours",
+         3, 1)
+
+gen.add("adaptiveThreshWinSizeMax",               int_t,    0,
+        "Maximum window size for adaptive thresholding before finding contours",
+        53, 1)
+
+gen.add("adaptiveThreshWinSizeStep",              int_t,    0,
+        "Increments from adaptiveThreshWinSizeMin to adaptiveThreshWinSizeMax during the thresholding",
+         4, 1)
+
+gen.add("cornerRefinementMaxIterations",          int_t,    0,
+        "Maximum number of iterations for stop criteria of the corner refinement process",
+        30, 1)
+
+gen.add("cornerRefinementMinAccuracy",            double_t, 0,
+        "Minimum error for the stop criteria of the corner refinement process",
+        0.01, 0, 1)
+
+gen.add("cornerRefinementWinSize",                int_t,    0,
+        "Window size for the corner refinement process (in pixels)",
+         5, 1)
+
+gen.add("doCornerRefinement",                     bool_t,   0,
+        "Whether to do subpixel corner refinement",
+         True)
+
+gen.add("cornerRefinementSubpix",                     bool_t,   0,
+        "Whether to do subpixel corner refinement (true) or contour (false)",
+         True)
+
+gen.add("errorCorrectionRate",                    double_t, 0,
+        "Error correction rate respect to the maximum error correction capability for each dictionary",
+        0.6, 0, 1)
+
+gen.add("minCornerDistanceRate",                  double_t, 0,
+        "Minimum distance between corners for detected markers relative to its perimeter",
+        0.05, 0)
+
+gen.add("markerBorderBits",                       int_t,    0,
+        "Number of bits of the marker border, i.e. marker border width",
+        1, 0)
+
+gen.add("maxErroneousBitsInBorderRate",           double_t, 0,
+        "Maximum number of accepted erroneous bits in the border (i.e. number of allowed white bits in the border)",
+       0.04, 0, 1)
+
+gen.add("minDistanceToBorder",                    int_t,    0,
+        "Minimum distance of any corner to the image border for detected markers (in pixels)",
+        3, 0)
+
+gen.add("minMarkerDistanceRate",                  double_t, 0,
+        "minimum mean distance beetween two marker corners to be considered similar, so that the smaller one is removed. The rate is relative to the smaller perimeter of the two markers",
+        0.1, 0, 1)
+
+gen.add("minMarkerPerimeterRate",                 double_t, 0,
+        "Determine minumum perimeter for marker contour to be detected. This is defined as a rate respect to the maximum dimension of the input image",
+        0.03, 0, 1)
+
+gen.add("maxMarkerPerimeterRate",                 double_t, 0,
+        "Determine maximum perimeter for marker contour to be detected. This is defined as a rate respect to the maximum dimension of the input image",
+        4.0, 0, 1)
+
+gen.add("minOtsuStdDev",                          double_t, 0,
+        "Minimun standard deviation in pixels values during the decodification step to apply Otsu thresholding (otherwise, all the bits are set to 0 or 1 depending on mean higher than 128 or not)",
+        5.0, 0)
+
+gen.add("perspectiveRemoveIgnoredMarginPerCell",  double_t, 0,
+        "Width of the margin of pixels on each cell not considered for the determination of the cell bit. Represents the rate respect to the total size of the cell, i.e. perpectiveRemovePixelPerCell",
+        0.13, 0, 1)
+
+gen.add("perspectiveRemovePixelPerCell",          int_t,    0,
+        "Number of bits (per dimension) for each cell of the marker when removing the perspective",
+        8, 1)
+
+gen.add("polygonalApproxAccuracyRate",            double_t, 0,
+        "Minimum accuracy during the polygonal approximation process to determine which contours are squares",
+        0.01, 0, 1)
+
+exit(gen.generate(PACKAGE, "stag_detect", "DetectorParams"))
diff --git a/stag_detect/cfg/single.yaml b/stag_detect/cfg/single.yaml
@@ -0,0 +1,13 @@
+# STag parameters
+libraryHD: 11
+errorCorrection: 2
+
+# Camera configuration
+camera_info_topic: raspicam_node/camera_info
+raw_image_topic: raspicam_node/image
+is_compressed: true
+
+# Node configuration
+tag_tf_prefix: fiducial_
+show_markers: true
+publish_tf: true
diff --git a/stag_detect/include/stag/Decoder.h b/stag_detect/include/stag/Decoder.h
@@ -0,0 +1,25 @@
+#ifndef DECODER_H
+#define DECODER_H
+
+#include <vector>
+#include <bitset>
+#include "stag/MarkerIDs.h"
+
+using std::vector;
+using std::bitset;
+
+typedef bitset<48> Codeword;
+
+class Decoder {
+  int wordSize = 48;
+  int noOfCodewords;
+
+  vector<Codeword> codewords;
+
+ public:
+  Decoder() {}
+  Decoder(int hd);
+  bool decode(const Codeword& c, int errCorr, int& id, int& shift);
+};
+
+#endif
diff --git a/stag_detect/include/stag/Drawer.h b/stag_detect/include/stag/Drawer.h
@@ -0,0 +1,40 @@
+#ifndef DRAWER_H
+#define DRAWER_H
+
+#include <string>
+#include <opencv2/opencv.hpp>
+
+#include "stag/ED/EDLines.h"
+#include "stag/ED/EdgeMap.h"
+#include "stag/QuadDetector.h"
+#include "stag/Marker.h"
+
+using std::string;
+
+class Drawer {
+  void colorAPixel(cv::Mat& img, int x, int y, cv::Scalar color, int dotWidth);
+
+ public:
+  // draws edge segments
+  void drawEdgeMap(const string& path, cv::Mat image, EdgeMap* edgeMap);
+
+  // draws line segments
+  void drawLines(const string& path, cv::Mat image, EDLines* edLines);
+
+  // draws corners (intersections of line segments)
+  void drawCorners(const string& path, cv::Mat image,
+                   const vector<vector<Corner>>& cornerGroups);
+
+  // draws quads
+  void drawQuads(const string& path, cv::Mat image, const vector<Quad>& quads);
+
+  // draws markers
+  cv::Mat drawMarkers(const string& path, cv::Mat image,
+                      const vector<Marker>& markers);
+
+  // draws refined markers and their ellipses
+  cv::Mat drawEllipses(const string& path, cv::Mat image,
+                       const vector<Marker>& markers);
+};
+
+#endif