diff --git a/.circleci/config.yml b/.circleci/config.yml index 17798d4b..eae5e8df 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -6,38 +6,93 @@ jobs: steps: - run: apt-get update -qq && apt-get install -qq git - checkout - - run: DEBIAN_FRONTEND=noninteractive ./InstallPackagesUbuntu.sh - - run: make + - run: + name: "Install dependencies" + command: | + DEBIAN_FRONTEND=noninteractive ./InstallPackagesUbuntu.sh + - run: + name: "Build" + command: make build-v4l: docker: - - image: ubuntu:18.04 + - image: ubuntu:18.04 steps: - run: apt-get update -qq && apt-get install -qq git - checkout - - run: DEBIAN_FRONTEND=noninteractive ./InstallPackagesUbuntu.sh - - run: mkdir -p build && cd build && cmake -DUSE_V4L=true .. && cd .. - - run: make + - run: + name: "Install dependencies" + command: | + DEBIAN_FRONTEND=noninteractive ./InstallPackagesUbuntu.sh + - run: + name: "Configure" + command: | + mkdir -p build + cmake -B build -DUSE_V4L=true . + - run: + name: "Build" + command: make build-splitter: docker: - image: ubuntu:18.04 steps: - run: apt-get update -qq && apt-get install -qq git - checkout - - run: DEBIAN_FRONTEND=noninteractive ./InstallPackagesUbuntu.sh - - run: mkdir -p build && cd build && cmake -DUSE_SPLITTER=true .. && cd .. - - run: cmake -B build -DUSE_SPLITTER=true . - - run: make + - run: + name: "Install dependencies" + command: | + DEBIAN_FRONTEND=noninteractive ./InstallPackagesUbuntu.sh + - run: + name: "Configure" + command: | + mkdir -p build + cmake -B build -DUSE_SPLITTER=true . + - run: + name: "Build" + command: make build-qt5: docker: - image: ubuntu:18.04 steps: - run: apt-get update -qq && apt-get install -qq git - checkout - - run: DEBIAN_FRONTEND=noninteractive ./InstallPackagesUbuntu.sh - - run: apt-get install -qq qtdeclarative5-dev - - run: mkdir -p build && cd build && cmake -DUSE_QT5=true .. && cd .. - - run: cmake -B build -DUSE_QT5=true . - - run: make + - run: + name: "Install dependencies" + command: | + DEBIAN_FRONTEND=noninteractive ./InstallPackagesUbuntu.sh + apt-get install -qq qtdeclarative5-dev + - run: + name: "Configure" + command: | + mkdir -p build + cmake -B build -DUSE_QT5=true . + - run: + name: "Build" + command: make + build-apriltag: + docker: + - image: ubuntu:18.04 + steps: + - run: apt-get update -qq && apt-get install -qq git + - checkout + - run: + name: "Install dependencies" + command: | + DEBIAN_FRONTEND=noninteractive ./InstallPackagesUbuntu.sh + - run: + name: "Install april tag" + command: | + git clone https://github.com/AprilRobotics/apriltag.git + cd apriltag + cmake . + make install + - run: + name: "Configure" + command: | + mkdir -p build + cmake -B build -DUSE_APRILTAG=true . + - run: + name: "Build" + command: make workflows: version: 2 @@ -47,3 +102,4 @@ workflows: - build-v4l - build-splitter - build-qt5 + - build-apriltag diff --git a/.gitignore b/.gitignore index 26c87e8f..d16e4b49 100644 --- a/.gitignore +++ b/.gitignore @@ -5,4 +5,8 @@ /backup /test-data/** /.idea +/.dir-locals.el +.ccls-cache +*~ cmake-build* +/compile_commands.json diff --git a/CMakeLists.txt b/CMakeLists.txt index dbd9a3f6..a785827a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,6 +8,7 @@ set(USE_PYLON FALSE CACHE BOOL "Compile with pylon driver (Basler cameras)") set(USE_FLYCAP FALSE CACHE BOOL "Compile with flycap driver (FLIR cameras, predecessor of Spinnaker)") set(USE_V4L FALSE CACHE BOOL "Compile with Video4Linux support (generic webcams)") set(USE_SPLITTER FALSE CACHE BOOL "Compile with Camera splitter support (virtual cameras with part of a full image)") +set(USE_APRILTAG FALSE CACHE BOOL "Compile with apriltag detection for robots") if(USE_DC1394 AND USE_mvIMPACT) message(FATAL_ERROR "DC1394 and mvImpact are not compatible: mvImpact crashes when creating device manager") @@ -190,6 +191,15 @@ else() message(STATUS "Camera splitter disabled") endif() +if(USE_APRILTAG) + find_package(apriltag REQUIRED) + add_definitions(-DAPRILTAG) + set(SRCS ${SRCS} src/app/plugins/plugin_apriltag.cpp) + message(STATUS "apriltag enabled") +else() + message(STATUS "apriltag disabled") +endif() + include_directories(${PROJECT_SOURCE_DIR}/src/app) include_directories(${PROJECT_SOURCE_DIR}/src/app/gui) include_directories(${PROJECT_SOURCE_DIR}/src/app/plugins) @@ -220,6 +230,7 @@ set (SRCS ${SRCS} src/app/plugins/plugin_detect_balls.cpp src/app/plugins/plugin_detect_robots.cpp src/app/plugins/plugin_find_blobs.cpp + src/app/plugins/plugin_greyscale.cpp src/app/plugins/plugin_publishgeometry.cpp src/app/plugins/plugin_legacypublishgeometry.cpp src/app/plugins/plugin_runlength_encode.cpp @@ -347,6 +358,9 @@ set (libs ${QT_LIBRARIES} ${SPINNAKER_LIBS} ) target_link_libraries(sslvision ${libs}) +if(USE_APRILTAG) + target_link_libraries(sslvision apriltag) +endif() if(USE_QT5) qt5_use_modules(sslvision Widgets) endif() @@ -356,6 +370,9 @@ set (libs ${libs} sslvision) set (target vision) add_executable(${target} ${UI_SRCS} ${MOC_SRCS} ${RC_SRCS} ${SRCS}) target_link_libraries(${target} ${libs}) +if (USE_APRILTAG) + target_link_libraries(${target} apriltag) +endif() if(USE_QT5) qt5_use_modules(${target} Widgets OpenGL) endif() diff --git a/README.md b/README.md index f0647b9b..8867868c 100644 --- a/README.md +++ b/README.md @@ -112,7 +112,44 @@ cmake -DUSE_WHAT_SO_EVER=true .. cd .. make ``` -The `USE_*` parameters are cached, so they do not have to be passed in each time. +The `USE_*` parameters are cached, so they do not have to be passed in +each time. + +### Apriltags Support + +There is experimental support for detecting and tracking robots via +[Apriltags](https://april.eecs.umich.edu/software/apriltag). The +standard tag sets in the [Apriltags Imgs +Repo](https://github.com/AprilRobotics/apriltag-imgs) are all +supported along with two customized tag sets: + +- Custom20h7: 26 unique tags + +To generate the custom tag images you must use the +[Apriltag-Generation +Repo](https://github.com/AprilRobotics/apriltag-generation). Follow +the instructions listed in the repo. The tag layouts are as follows: + +- Custom20h7 + + custom_xxddddxxxwwwwwwxdwbbbbwddwbddbwddwbddbwddwbbbbwdxwwwwwwxxxddddxx + +The hamming distance used during generation are as follows: + +- Custom20h7: 7 + +To enable you must set the `USE_APRILTAG` option to `ON`. e.g.: + +``` bash +cd build +cmake -DUSE_APRILTAG=ON .. +cd .. +make +``` + +This will add a new plugin to each camera that runs on every frame. At +the moment the old robot detection patterns are detected in addition +to the apriltag detection. ## Running @@ -199,3 +236,40 @@ program refuses to start completely when parsing the XML files (this should normally never occur) then simply delete all XML files and restart. The program will restore its default settings. + +#### Apriltags + +Unlike the butterfly pattern, there are no colors in the apriltags. So +you must manually assign tag ids to the different team colors. Under +Global settings there are two settings list "Blue April Tags" and +"Yellow April Tags". Expand each list and click "Add Tag" to create a +new tag entry for the associated team color. Expand the new entry and +edit the value to a valid tag id in order to begin detection. + +You must also configure the apriltag detection settings. The settings +are per camera under the "AprilTag" settings in each thread. Select +the tag family and configure the size in mm of the side of the +Apriltag quad. See the apriltags repository for explanation of the +other options and how they affect detection accuracy and speed. + +Apriltag detection takes place on a greyscale version of the image. To +see this image enable "greyscale" un the Visualization plugin for the +appropriate camera. If a tag is not detecting check the greyscale +image contrast. Verify that the tag doesn't have a glare or a large +contrast due to shadows. + +Under the Visualization plugin you can also enable the "detected +AprilTags" option. This will draw a cyan border around the quad of the +tag (i.e. the part you should measure and enter as quad size in the +detector settings). It will also draw the tag id in magenta over the +detected tag. + +##### Large Distortions + +Currently, the tag detection runs on a raw image provided by the +camera to maximize performance. Fixes for distortion happen *AFTER* +detection. However, the apriltag detector is designed to work on +undistorted (aka rectified) images. If your camera lens has a large +distortion (you can see this by the field lines and other image lines +being highly curved), then the detection accuracy may be lower or even +fail. diff --git a/src/app/plugins/plugin_apriltag.cpp b/src/app/plugins/plugin_apriltag.cpp new file mode 100644 index 00000000..0552d14e --- /dev/null +++ b/src/app/plugins/plugin_apriltag.cpp @@ -0,0 +1,344 @@ +#include "plugin_apriltag.h" +#include "messages_robocup_ssl_detection.pb.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using HammHist = std::array; +using Eigen::AngleAxisd; +using Eigen::Isometry3d; +using Eigen::Matrix3d; +using Eigen::Quaterniond; +using Eigen::Vector3d; + +enum class Team : int { None, Blue, Yellow }; + +PluginAprilTag::PluginAprilTag(FrameBuffer *buffer, + const CameraParameters &camera_params, + std::shared_ptr blue_team_tags, + std::shared_ptr yellow_team_tags, + CMPattern::TeamSelector &blue_team_selector, + CMPattern::TeamSelector &yellow_team_selector, ConvexHullImageMask& mask) + : VisionPlugin(buffer), settings(new VarList("AprilTag")), + v_enable(new VarBool("enable", true)), + v_quad_size(new VarDouble("quad size (mm)", 70.0)), + v_iters(new VarInt("iters", 1)), v_threads(new VarInt("threads", 1)), + v_hamming(new VarInt("hamming", 1)), + v_decimate(new VarDouble("decimate", 2.0)), + v_blur(new VarDouble("blur", 0.0)), + v_refine_edges(new VarBool("refine-edges", true)), + detections{nullptr, &apriltag_detections_destroy}, + camera_params(camera_params), blue_team_tags(blue_team_tags), + yellow_team_tags(yellow_team_tags), + blue_team_selector(blue_team_selector), + yellow_team_selector(yellow_team_selector), + image_mask(mask){ + + v_family.reset(new VarStringEnum("Tag Family", "tagCircle21h7")); + v_family->addItem("tag36h11"); + v_family->addItem("tag16h5"); + v_family->addItem("tagCircle21h7"); + v_family->addItem("tagCircle49h12"); + v_family->addItem("tagStandard41h12"); + v_family->addItem("tagStandard52h13"); + v_family->addItem("tagCustom48h12"); + v_family->addItem("tagCustom20h7"); + + settings->addChild(v_enable.get()); + settings->addChild(v_family.get()); + settings->addChild(v_quad_size.get()); + settings->addChild(v_iters.get()); + settings->addChild(v_threads.get()); + settings->addChild(v_hamming.get()); + settings->addChild(v_decimate.get()); + settings->addChild(v_blur.get()); + settings->addChild(v_refine_edges.get()); + + // construct apriltag detector with these settings + makeTagFamily(); + makeTagDetector(); +} + +ProcessResult PluginAprilTag::process(FrameData *data, RenderOptions *options) { + if (!v_enable->getBool()) { + return ProcessingOk; + } + image_mask.lock(); + if (!tag_family || v_family->getString() != tag_family->name) { + makeTagFamily(); + if (!tag_family) { + std::cerr << "AprilTag: Failed to create tag family '" + << v_family->getString() << "'\n"; + image_mask.unlock(); + return ProcessingFailed; + } + + apriltag_detector_clear_families(tag_detector.get()); + apriltag_detector_add_family_bits(tag_detector.get(), tag_family.get(), + v_hamming->getInt()); + } + + if (!tag_detector) { + makeTagDetector(); + if (!tag_detector) { + std::cerr << "AprilTag: Failed to create tag detector\n"; + image_mask.unlock(); + return ProcessingFailed; + } + } + + tag_detector->quad_decimate = v_decimate->getDouble(); + tag_detector->quad_sigma = v_blur->getDouble(); + tag_detector->nthreads = v_threads->getInt(); + tag_detector->refine_edges = v_refine_edges->getBool(); + + Image *img_greyscale = + reinterpret_cast *>(data->map.get("greyscale")); + if (img_greyscale == nullptr) { + std::cerr << "AprilTag: No greyscale image in frame data\n"; + image_mask.unlock(); + return ProcessingFailed; + } + + Image *img_masked; + if ((img_masked = reinterpret_cast *>( + data->map.get("masked"))) == nullptr) { + img_masked = reinterpret_cast *>( + data->map.insert("masked", new Image())); + } + img_masked->allocate(img_greyscale->getWidth(), img_greyscale->getHeight()); + + // TODO(dschwab): In the interest of speed on hi-res images, it + // might be useful to compute the bounding box of the convex hull + // and if it is x% smaller than original image, apply the mask and + // then crop the image so that the detection needs to work with less + // pixels. + + Image *input_image = img_greyscale; + const auto& hull = image_mask.getConvexHull(); + // there must be at least 3 points in the convex hull for the mask to have restricted + // the image size. If there are less than 3 skip masking the + // greyscale image. + if (hull.getNumPoints() >= 3) { + const auto &mask = image_mask.getMask(); + cv::Mat cv_mask(mask.getHeight(), mask.getWidth(), CV_8UC1, mask.getData()); + cv::Mat cv_greyscale(img_greyscale->getHeight(), img_greyscale->getWidth(), + CV_8UC1, img_greyscale->getData()); + cv::Mat cv_masked(img_masked->getHeight(), img_masked->getWidth(), CV_8UC1, img_masked->getData()); + + cv::Mat& input1 = cv_mask; + cv::Mat& input2 = cv_greyscale; + cv::Mat& output = cv_masked; + cv::bitwise_and(input1, input2, output); + + input_image = img_masked; + } + + const int maxiters = v_iters->getInt(); + for (int iter = 0; iter < maxiters; iter++) { + if (maxiters > 1) { + std::cout << "iter " << iter + 1 << " / " << maxiters << "\n"; + } + + // zero-copy. Just use the already allocated buffer data. + image_u8_t im{.width = input_image->getWidth(), + .height = input_image->getHeight(), + .stride = input_image->getWidth(), + .buf = input_image->getData()}; + + detections.reset(apriltag_detector_detect(tag_detector.get(), &im)); + data->map.update("apriltag_detections", detections.get()); + + // add the detections to the robot list so that the positions will + // be published + SSL_DetectionFrame *detection_frame = 0; + detection_frame = + (SSL_DetectionFrame *)data->map.get("ssl_detection_frame"); + if (detection_frame == 0) + detection_frame = (SSL_DetectionFrame *)data->map.insert( + "ssl_detection_frame", new SSL_DetectionFrame()); + + // add the detections to the detected robots list + Eigen::Matrix ideal_tag_points; + ideal_tag_points << -1, 1, -1, 1, 1, 1, -1, -1, 1, 1, 1, 1; + + for (int i = 0; i < zarray_size(detections.get()); ++i) { + apriltag_detection_t *det; + zarray_get(detections.get(), i, &det); + + // check if tag in map + // if not then ignore it + Team tag_team = Team::None; + double team_height; + if ((*blue_team_tags)->count(det->id)) { + tag_team = Team::Blue; + team_height = + blue_team_selector.getSelectedTeam()->_robot_height->getDouble(); + } + if ((*yellow_team_tags)->count(det->id)) { + tag_team = Team::Yellow; + team_height = + yellow_team_selector.getSelectedTeam()->_robot_height->getDouble(); + } + + if (tag_team == Team::None) { + continue; + } + + Eigen::Map tag_homography(det->H->data); + + Eigen::Matrix image_tag_points = + tag_homography.transpose() * ideal_tag_points; + image_tag_points.array().rowwise() /= image_tag_points.row(2).array(); + + // convert the points to field coordinates + vector3d p_top_left_field; + vector2d p_top_left_image(image_tag_points(0, 0), image_tag_points(1, 0)); + camera_params.image2field(p_top_left_field, p_top_left_image, + team_height); + + vector3d p_top_right_field; + vector2d p_top_right_image(image_tag_points(0, 1), + image_tag_points(1, 1)); + camera_params.image2field(p_top_right_field, p_top_right_image, + team_height); + + vector3d p_bottom_left_field; + vector2d p_bottom_left_image(image_tag_points(0, 2), + image_tag_points(1, 2)); + camera_params.image2field(p_bottom_left_field, p_bottom_left_image, + team_height); + + vector3d p_bottom_right_field; + vector2d p_bottom_right_image(image_tag_points(0, 3), + image_tag_points(1, 3)); + camera_params.image2field(p_bottom_right_field, p_bottom_right_image, + team_height); + + vector3d p_center_field; + vector2d p_center_image(det->c[0], det->c[1]); + camera_params.image2field(p_center_field, p_center_image, team_height); + + // the quad in field coordinates may not be perfect due to + // detection errors, numerical errors and camera calibration. So + // get the angles from each face, and average to get a better + // estimate. + Eigen::RowVector4d angles = Eigen::RowVector4d::Zero(); + + angles(0) = atan2(p_top_right_field.x - p_top_left_field.x, + p_top_left_field.y - p_top_right_field.y); + + angles(1) = atan2(p_bottom_right_field.y - p_top_right_field.y, + p_bottom_right_field.x - p_top_right_field.x); + + angles(2) = atan2(p_bottom_right_field.x - p_bottom_left_field.x, + p_bottom_left_field.y - p_bottom_right_field.y); + + angles(3) = atan2(p_bottom_left_field.y - p_top_left_field.y, + p_bottom_left_field.x - p_top_left_field.x); + + SSL_DetectionRobot *robot_detection = nullptr; + if (tag_team == Team::Blue) { + robot_detection = detection_frame->add_robots_blue(); + } else { + robot_detection = detection_frame->add_robots_yellow(); + } + robot_detection->set_confidence( + det->decision_margin); // TODO(dschwab): What value should I put here? + robot_detection->set_robot_id(det->id); + + robot_detection->set_x(p_center_field.x); + robot_detection->set_y(p_center_field.y); + robot_detection->set_orientation(angles.mean()); + robot_detection->set_pixel_x(det->c[0]); + robot_detection->set_pixel_y(det->c[1]); + robot_detection->set_height(team_height); + } + } + + image_mask.unlock(); + return ProcessingOk; +} + +VarList *PluginAprilTag::getSettings() { return settings.get(); } + +std::string PluginAprilTag::getName() { return "AprilTag"; } + +void PluginAprilTag::makeTagFamily() { + const auto tag_name = v_family->getString(); + if (tag_name == "tag36h11") { + tag_family = std::unique_ptr>{ + tag36h11_create(), &tag36h11_destroy}; + } else if (tag_name == "tag25h9") { + tag_family = std::unique_ptr>{ + tag25h9_create(), &tag25h9_destroy}; + } else if (tag_name == "tag16h5") { + tag_family = std::unique_ptr>{ + tag16h5_create(), &tag16h5_destroy}; + } else if (tag_name == "tagCircle21h7") { + tag_family = std::unique_ptr>{ + tagCircle21h7_create(), &tagCircle21h7_destroy}; + } else if (tag_name == "tagCircle49h12") { + tag_family = std::unique_ptr>{ + tagCircle49h12_create(), &tagCircle49h12_destroy}; + } else if (tag_name == "tagStandard41h12") { + tag_family = std::unique_ptr>{ + tagStandard41h12_create(), &tagStandard41h12_destroy}; + } else if (tag_name == "tagStandard52h13") { + tag_family = std::unique_ptr>{ + tagStandard52h13_create(), &tagStandard52h13_destroy}; + } else if (tag_name == "tagCustom48h12") { + tag_family = std::unique_ptr>{ + tagCustom48h12_create(), &tagCustom48h12_destroy}; + } else if (tag_name == "tagCustom20h7") { + tag_family = std::unique_ptr>{ + tagCustom20h7_create(), &tagCustom20h7_destroy}; + } else { + std::cerr << "AprilTag: Failed to create apriltag family. Unknown family '" + << tag_name << "'\n"; + tag_family = std::unique_ptr>{ + nullptr, [](apriltag_family_t *) {}}; + } +} + +void PluginAprilTag::makeTagDetector() { + if (!tag_family) { + std::cerr + << "AprilTag: Cannot construct tag detector because tag family has not " + "been created\n"; + tag_detector = std::unique_ptr>{ + nullptr, [](apriltag_detector_t *) {}}; + return; + } + + tag_detector = std::unique_ptr>{ + apriltag_detector_create(), &apriltag_detector_destroy}; + + apriltag_detector_add_family_bits(tag_detector.get(), tag_family.get(), + v_hamming->getInt()); +} diff --git a/src/app/plugins/plugin_apriltag.h b/src/app/plugins/plugin_apriltag.h new file mode 100644 index 00000000..9fcad9e5 --- /dev/null +++ b/src/app/plugins/plugin_apriltag.h @@ -0,0 +1,63 @@ +#ifndef PLUGIN_APRILTAG_H +#define PLUGIN_APRILTAG_H + +#include "convex_hull_image_mask.h" +#include "camera_calibration.h" +#include "cmpattern_teamdetector.h" +#include "team_tags.h" +#include +#include +#include +#include +#include + +class PluginAprilTag : public VisionPlugin { +protected: + std::unique_ptr settings; + std::unique_ptr v_enable; + std::unique_ptr v_family; + std::unique_ptr v_quad_size; + std::unique_ptr v_iters; + std::unique_ptr v_threads; + std::unique_ptr v_hamming; + std::unique_ptr v_decimate; + std::unique_ptr v_blur; + std::unique_ptr v_refine_edges; + + // april tags info + std::unique_ptr> + tag_family; + std::unique_ptr> + tag_detector; + std::unique_ptr> detections; + + const CameraParameters &camera_params; + const std::shared_ptr blue_team_tags; + const std::shared_ptr yellow_team_tags; + CMPattern::TeamSelector &blue_team_selector; + CMPattern::TeamSelector &yellow_team_selector; + ConvexHullImageMask& image_mask; + +public: + PluginAprilTag(FrameBuffer *buffer, const CameraParameters &camera_params, + std::shared_ptr blue_team_tags, + std::shared_ptr yellow_team_tags, + CMPattern::TeamSelector &blue_team_selector, + CMPattern::TeamSelector &yellow_team_selector, + ConvexHullImageMask& mask); + ~PluginAprilTag() override = default; + + ProcessResult process(FrameData *data, RenderOptions *options) override; + + VarList *getSettings() override; + + std::string getName() override; + +private: + + void makeTagFamily(); + void makeTagDetector(); +}; + +#endif /* PLUGIN_APRILTAG_H */ diff --git a/src/app/plugins/plugin_detect_robots.cpp b/src/app/plugins/plugin_detect_robots.cpp index fea42abf..bdf768a9 100644 --- a/src/app/plugins/plugin_detect_robots.cpp +++ b/src/app/plugins/plugin_detect_robots.cpp @@ -126,14 +126,14 @@ ProcessResult PluginDetectRobots::process(FrameData * data, RenderOptions * opti color_id=color_id_blue; team=global_team_selector_blue->getSelectedTeam(); num_robots=global_team_selector_blue->getNumberRobots(); - detection_frame->clear_robots_blue(); + // detection_frame->clear_robots_blue(); robotlist=detection_frame->mutable_robots_blue(); detector=team_detector_blue; } else { color_id=color_id_yellow; team=global_team_selector_yellow->getSelectedTeam(); num_robots=global_team_selector_yellow->getNumberRobots(); - detection_frame->clear_robots_yellow(); + // detection_frame->clear_robots_yellow(); robotlist=detection_frame->mutable_robots_yellow(); detector=team_detector_yellow; } diff --git a/src/app/plugins/plugin_greyscale.cpp b/src/app/plugins/plugin_greyscale.cpp new file mode 100644 index 00000000..dd941eb6 --- /dev/null +++ b/src/app/plugins/plugin_greyscale.cpp @@ -0,0 +1,98 @@ +#include "plugin_greyscale.h" +#include +#include +#include +#include + +PluginGreyscale::PluginGreyscale(FrameBuffer *buffer) + : VisionPlugin(buffer), settings(new VarList("Greyscale")) {} + +ProcessResult PluginGreyscale::process(FrameData *data, + RenderOptions *options) { + + Image *img_greyscale; + if ((img_greyscale = reinterpret_cast *>( + data->map.get("greyscale"))) == nullptr) { + img_greyscale = reinterpret_cast *>( + data->map.insert("greyscale", new Image())); + } + + // Allocate image if not already allocated. Allocate method will do + // nothing if data already allocated for correct width/height. + img_greyscale->allocate(data->video.getWidth(), data->video.getHeight()); + + switch (data->video.getColorFormat()) { + case COLOR_RGB8: + cvColor2Grey(data->video, CV_8UC3, img_greyscale, cv::COLOR_RGB2GRAY); + break; + case COLOR_RGBA8: + cvColor2Grey(data->video, CV_8UC4, img_greyscale, cv::COLOR_RGBA2GRAY); + break; + case COLOR_YUV422_UYVY: + cvColor2Grey(data->video, CV_8UC2, img_greyscale, cv::COLOR_YUV2GRAY_UYVY); + break; + case COLOR_YUV422_YUYV: + cvColor2Grey(data->video, CV_8UC2, img_greyscale, cv::COLOR_YUV2GRAY_YUYV); + break; + case COLOR_RGB16: + cvColor2Grey(data->video, CV_16UC3, img_greyscale, cv::COLOR_RGB2GRAY); + break; + case COLOR_RAW16: + [[fallthrough]]; + case COLOR_MONO16: + cv16bit2_8bit(data->video, img_greyscale); + break; + case COLOR_MONO8: + [[fallthrough]]; + case COLOR_RAW8: + // already in the correct format + copyData(data->video, img_greyscale); + break; + case COLOR_YUV411: + [[fallthrough]]; + case COLOR_YUV444: + [[fallthrough]]; + default: + manualColor2Grey(data->video, img_greyscale); + break; + } + + return ProcessingOk; +} + +VarList *PluginGreyscale::getSettings() { return settings.get(); } + +std::string PluginGreyscale::getName() { return "Greyscale"; } + +void PluginGreyscale::cvColor2Grey( + const RawImage &src, const int src_data_format, Image *dst, + const cv::ColorConversionCodes conversion_code) { + cv::Mat srcMat(src.getWidth(), src.getHeight(), src_data_format, + src.getData()); + cv::Mat dstMat(dst->getWidth(), dst->getHeight(), CV_8UC1, dst->getData()); + cv::cvtColor(srcMat, dstMat, conversion_code); +} + +void PluginGreyscale::cv16bit2_8bit(const RawImage &src, Image *dst) { + cv::Mat srcMat(src.getWidth(), src.getHeight(), CV_16UC1, src.getData()); + cv::Mat dstMat(dst->getWidth(), dst->getHeight(), CV_8UC1, dst->getData()); + // convertTo drops higher bits. Need to rescale 16 bit values to + // 8bit range. Should have a scale factor of 1/256. See + // https://stackoverflow.com/a/10420743 + srcMat.convertTo(dstMat, CV_8U, 0.00390625); +} + +void PluginGreyscale::manualColor2Grey(const RawImage &src, Image *dst) { + for (int i = 0; i < src.getWidth(); ++i) { + for (int j = 0; j < src.getHeight(); ++j) { + const auto pixel = src.getRgb(i, j); + *(dst->getPixelPointer(i, j)) = (pixel.r + pixel.g + pixel.b) / 3; + } + } +} + +void PluginGreyscale::copyData(const RawImage &src, Image *dst) { + cv::Mat srcMat(src.getWidth(), src.getHeight(), CV_8UC1, src.getData()); + cv::Mat dstMat(dst->getWidth(), dst->getHeight(), CV_8UC1, dst->getData()); + srcMat.copyTo(dstMat); +} diff --git a/src/app/plugins/plugin_greyscale.h b/src/app/plugins/plugin_greyscale.h new file mode 100644 index 00000000..20b8a5a6 --- /dev/null +++ b/src/app/plugins/plugin_greyscale.h @@ -0,0 +1,33 @@ +#ifndef PLUGIN_GREYSCALE_H +#define PLUGIN_GREYSCALE_H + +#include +#include +#include +#include +#include + +class PluginGreyscale : public VisionPlugin { +protected: + std::unique_ptr settings; + +public: + PluginGreyscale(FrameBuffer *buffer); + ~PluginGreyscale() override = default; + + ProcessResult process(FrameData *data, RenderOptions *options) override; + + VarList *getSettings() override; + + std::string getName() override; + +private: + void cvColor2Grey(const RawImage& src, const int src_data_format, + Image *dst, + const cv::ColorConversionCodes conversion_code); + void cv16bit2_8bit(const RawImage& src, Image *dst); + void manualColor2Grey(const RawImage& src, Image *dst); + void copyData(const RawImage& src, Image *dst); +}; + +#endif /* PLUGIN_GREYSCALE_H */ diff --git a/src/app/plugins/plugin_sslnetworkoutput.cpp b/src/app/plugins/plugin_sslnetworkoutput.cpp index 148409c3..93b1332b 100644 --- a/src/app/plugins/plugin_sslnetworkoutput.cpp +++ b/src/app/plugins/plugin_sslnetworkoutput.cpp @@ -46,6 +46,8 @@ ProcessResult PluginSSLNetworkOutput::process(FrameData * data, RenderOptions * detection_frame->set_camera_id(_camera_params.additional_calibration_information->camera_index->getInt()); detection_frame->set_t_sent(GetTimeSec()); _udp_server->send(*detection_frame); + detection_frame->clear_robots_blue(); + detection_frame->clear_robots_yellow(); } return ProcessingOk; } diff --git a/src/app/plugins/plugin_visualize.cpp b/src/app/plugins/plugin_visualize.cpp index 21d5487a..bdc23648 100644 --- a/src/app/plugins/plugin_visualize.cpp +++ b/src/app/plugins/plugin_visualize.cpp @@ -19,14 +19,18 @@ */ //======================================================================== #include "plugin_visualize.h" -#include #include #include "convex_hull.h" #include +#include + +#ifdef APRILTAG +#include +#endif namespace { typedef CameraParameters::AdditionalCalibrationInformation AddnlCalibInfo; -} // namespace +} // namespace PluginVisualize::PluginVisualize( FrameBuffer* _buffer, const CameraParameters& camera_params, @@ -44,6 +48,9 @@ PluginVisualize::PluginVisualize( _v_detected_edges = new VarBool("detected edges", false); _v_complete_sobel = new VarBool("complete edge detection", false); _v_complete_sobel->setBool(false); +#ifdef APRILTAG + _v_detected_apriltags = new VarBool("detected AprilTags", false); +#endif _v_mask_hull = new VarBool("image mask hull", false); @@ -57,45 +64,51 @@ PluginVisualize::PluginVisualize( _settings->addChild(_v_calibration_result); _settings->addChild(_v_detected_edges); _settings->addChild(_v_complete_sobel); +#ifdef APRILTAG + _settings->addChild(_v_detected_apriltags); +#endif _settings->addChild(_v_mask_hull); - _threshold_lut=0; + _threshold_lut = 0; edge_image = 0; temp_grey_image = 0; } - PluginVisualize::~PluginVisualize() { - if (edge_image) delete edge_image; - if (temp_grey_image) delete temp_grey_image; + if (edge_image) + delete edge_image; + if (temp_grey_image) + delete temp_grey_image; + +#ifdef APRILTAG + delete _v_detected_apriltags; +#endif } -VarList * PluginVisualize::getSettings() { - return _settings; -} +VarList *PluginVisualize::getSettings() { return _settings; } -string PluginVisualize::getName() { - return "Visualization"; -} +string PluginVisualize::getName() { return "Visualization"; } -void PluginVisualize::DrawCameraImage( - FrameData* data, VisualizationFrame* vis_frame) { - //if converting entire image then blanking is not needed +void PluginVisualize::DrawCameraImage(FrameData *data, + VisualizationFrame *vis_frame) { + // if converting entire image then blanking is not needed const ColorFormat source_format = data->video.getColorFormat(); if (source_format == COLOR_RGB8) { - //plain copy of data + // plain copy of data memcpy(vis_frame->data.getData(), data->video.getData(), - data->video.getNumBytes()); - } else if (source_format==COLOR_YUV422_UYVY) { + data->video.getNumBytes()); + } else if (source_format == COLOR_YUV422_UYVY) { Conversions::uyvy2rgb( data->video.getData(), - reinterpret_cast(vis_frame->data.getData()), + reinterpret_cast(vis_frame->data.getData()), data->video.getWidth(), data->video.getHeight()); - } else if (source_format==COLOR_RAW8) { - cv::Mat src(data->video.getWidth(), data->video.getHeight(), CV_8UC1, data->video.getData()); - cv::Mat dst(data->video.getWidth(), data->video.getHeight(), CV_8UC3, vis_frame->data.getData()); + } else if (source_format == COLOR_RAW8) { + cv::Mat src(data->video.getWidth(), data->video.getHeight(), CV_8UC1, + data->video.getData()); + cv::Mat dst(data->video.getWidth(), data->video.getHeight(), CV_8UC3, + vis_frame->data.getData()); cvtColor(src, dst, cv::COLOR_BayerBG2BGR); } else { - //blank it: + // blank it: vis_frame->data.fillBlack(); fprintf(stderr, "Unable to visualize color format: %s\n", Colors::colorFormatToString(source_format).c_str()); @@ -104,32 +117,32 @@ void PluginVisualize::DrawCameraImage( __FUNCTION__, __FILE__); } if (_v_greyscale->getBool() == true) { - unsigned int n = vis_frame->data.getNumPixels(); - rgb * vis_ptr = vis_frame->data.getPixelData(); - rgb color; - - for (unsigned int i = 0; i < n; i++) { - color = vis_ptr[i]; - color.r = color.g = color.b = ((color.r + color.g + color.b)/3); - vis_ptr[i] = color; + Image *img_greyscale = + reinterpret_cast *>(data->map.get("greyscale")); + + rgb *vis_ptr = vis_frame->data.getPixelData(); + raw8 *grey_ptr = img_greyscale->getPixelData(); + for (int i = 0; i < vis_frame->data.getNumPixels(); ++i) { + auto &color = vis_ptr[i]; + color.r = color.g = color.b = grey_ptr[i].v; } } } -void PluginVisualize::DrawThresholdedImage( - FrameData* data, VisualizationFrame* vis_frame) { +void PluginVisualize::DrawThresholdedImage(FrameData *data, + VisualizationFrame *vis_frame) { if (_threshold_lut != 0) { - Image* img_thresholded = - reinterpret_cast*>(data->map.get("cmv_threshold")); + Image *img_thresholded = + reinterpret_cast *>(data->map.get("cmv_threshold")); if (img_thresholded != 0) { int n = vis_frame->data.getNumPixels(); if (img_thresholded->getNumPixels() == n) { - rgb * vis_ptr = vis_frame->data.getPixelData(); - raw8 * seg_ptr = img_thresholded->getPixelData(); + rgb *vis_ptr = vis_frame->data.getPixelData(); + raw8 *seg_ptr = img_thresholded->getPixelData(); for (int i = 0; i < n; i++) { if (seg_ptr[i].getIntensity() != 0) { - vis_ptr[i] = _threshold_lut->getChannel( - seg_ptr[i].getIntensity()).draw_color; + vis_ptr[i] = _threshold_lut->getChannel(seg_ptr[i].getIntensity()) + .draw_color; } } } @@ -137,13 +150,13 @@ void PluginVisualize::DrawThresholdedImage( } } -void PluginVisualize::DrawBlobs( - FrameData* data, VisualizationFrame* vis_frame) { - CMVision::ColorRegionList* colorlist = - reinterpret_cast( +void PluginVisualize::DrawBlobs(FrameData *data, + VisualizationFrame *vis_frame) { + CMVision::ColorRegionList *colorlist = + reinterpret_cast( data->map.get("cmv_colorlist")); if (colorlist != 0) { - CMVision::RegionLinkedList * regionlist; + CMVision::RegionLinkedList *regionlist; regionlist = colorlist->getColorRegionArrayPointer(); for (int i = 0; i < colorlist->getNumColorRegions(); i++) { rgb blob_draw_color; @@ -152,97 +165,101 @@ void PluginVisualize::DrawBlobs( } else { blob_draw_color.set(255, 255, 255); } - CMVision::Region * blob=regionlist[i].getInitialElement(); + CMVision::Region *blob = regionlist[i].getInitialElement(); while (blob != 0) { - vis_frame->data.drawLine( - blob->x1,blob->y1,blob->x2,blob->y1,blob_draw_color); - vis_frame->data.drawLine( - blob->x1,blob->y1,blob->x1,blob->y2,blob_draw_color); - vis_frame->data.drawLine( - blob->x1,blob->y2,blob->x2,blob->y2,blob_draw_color); - vis_frame->data.drawLine( - blob->x2,blob->y1,blob->x2,blob->y2,blob_draw_color); + vis_frame->data.drawLine(blob->x1, blob->y1, blob->x2, blob->y1, + blob_draw_color); + vis_frame->data.drawLine(blob->x1, blob->y1, blob->x1, blob->y2, + blob_draw_color); + vis_frame->data.drawLine(blob->x1, blob->y2, blob->x2, blob->y2, + blob_draw_color); + vis_frame->data.drawLine(blob->x2, blob->y1, blob->x2, blob->y2, + blob_draw_color); blob = blob->next; } } } } -void PluginVisualize::DrawCameraCalibration( - FrameData* data, VisualizationFrame* vis_frame) { +void PluginVisualize::DrawCameraCalibration(FrameData *data, + VisualizationFrame *vis_frame) { // Principal point rgb ppoint_draw_color; ppoint_draw_color.set(255, 0, 0); int x = camera_parameters.principal_point_x->getDouble(); int y = camera_parameters.principal_point_y->getDouble(); - vis_frame->data.drawFatLine(x-15, y-15, x+15, y+15, ppoint_draw_color); - vis_frame->data.drawFatLine(x+15, y-15, x-15, y+15, ppoint_draw_color); + vis_frame->data.drawFatLine(x - 15, y - 15, x + 15, y + 15, + ppoint_draw_color); + vis_frame->data.drawFatLine(x + 15, y - 15, x - 15, y + 15, + ppoint_draw_color); // Calibration points rgb cpoint_draw_color; - cpoint_draw_color.set(0,255,255); + cpoint_draw_color.set(0, 255, 255); for (int i = 0; i < AddnlCalibInfo::kNumControlPoints; ++i) { - const int bx = camera_parameters.additional_calibration_information-> - control_point_image_xs[i]->getDouble(); - const int by = camera_parameters.additional_calibration_information-> - control_point_image_ys[i]->getDouble(); + const int bx = camera_parameters.additional_calibration_information + ->control_point_image_xs[i] + ->getDouble(); + const int by = camera_parameters.additional_calibration_information + ->control_point_image_ys[i] + ->getDouble(); vis_frame->data.drawFatBox(bx - 5, by - 5, 11, 11, cpoint_draw_color); - const string label = - camera_parameters.additional_calibration_information-> - control_point_names[i]->getString(); + const string label = camera_parameters.additional_calibration_information + ->control_point_names[i] + ->getString(); vis_frame->data.drawString(bx - 5, by + 15, label, cpoint_draw_color); char buff[20]; snprintf(buff, sizeof(buff), "(%.0f,%.0f)", - camera_parameters.additional_calibration_information->control_point_field_xs[i]->getDouble(), - camera_parameters.additional_calibration_information->control_point_field_ys[i]->getDouble()); + camera_parameters.additional_calibration_information + ->control_point_field_xs[i] + ->getDouble(), + camera_parameters.additional_calibration_information + ->control_point_field_ys[i] + ->getDouble()); std::string description = buff; vis_frame->data.drawString(bx + 10, by - 2, description, cpoint_draw_color); } } -void PluginVisualize::DrawCalibrationResult( - FrameData* data, VisualizationFrame* vis_frame) { +void PluginVisualize::DrawCalibrationResult(FrameData *data, + VisualizationFrame *vis_frame) { int steps_per_line(20); real_field.field_markings_mutex.lockForRead(); for (size_t i = 0; i < real_field.field_lines.size(); ++i) { - const FieldLine& line_segment = - *(real_field.field_lines[i]); - const GVector::vector3d p1( - line_segment.p1_x->getDouble(), line_segment.p1_y->getDouble(), 0.0); - const GVector::vector3d p2( - line_segment.p2_x->getDouble(), line_segment.p2_y->getDouble(), 0.0); + const FieldLine &line_segment = *(real_field.field_lines[i]); + const GVector::vector3d p1(line_segment.p1_x->getDouble(), + line_segment.p1_y->getDouble(), 0.0); + const GVector::vector3d p2(line_segment.p2_x->getDouble(), + line_segment.p2_y->getDouble(), 0.0); drawFieldLine(p1, p2, steps_per_line, vis_frame); } for (size_t i = 0; i < real_field.field_arcs.size(); ++i) { - const FieldCircularArc& arc = *(real_field.field_arcs[i]); - const GVector::vector3d center( - arc.center_x->getDouble(), arc.center_y->getDouble(), 0.0); + const FieldCircularArc &arc = *(real_field.field_arcs[i]); + const GVector::vector3d center(arc.center_x->getDouble(), + arc.center_y->getDouble(), 0.0); drawFieldArc(center, arc.radius->getDouble(), arc.a1->getDouble(), arc.a2->getDouble(), steps_per_line, vis_frame); } real_field.field_markings_mutex.unlock(); } -void PluginVisualize::DrawSobelImage( - FrameData* data, VisualizationFrame* vis_frame) { +void PluginVisualize::DrawSobelImage(FrameData *data, + VisualizationFrame *vis_frame) { if (edge_image == 0) { - edge_image = - new greyImage(data->video.getWidth(),data->video.getHeight()); + edge_image = new greyImage(data->video.getWidth(), data->video.getHeight()); temp_grey_image = - new greyImage(data->video.getWidth(),data->video.getHeight()); + new greyImage(data->video.getWidth(), data->video.getHeight()); } Images::convert(vis_frame->data, *temp_grey_image); // Draw sobel image: Contrast towards more brightness is painted white, // Contrast towards more darkness is painted green - for(int y=1; ygetHeight()-1; ++y) - { - for(int x=1; xgetWidth()-1; ++x) - { - int colVB,colVD,colHB,colHD; - colVB = Sobel::verticalBrighter(*temp_grey_image,x,y,30); - colVD = Sobel::verticalDarker(*temp_grey_image,x,y,30); - colHB = Sobel::horizontalBrighter(*temp_grey_image,x,y,30); - colHD = Sobel::horizontalDarker(*temp_grey_image,x,y,30); + for (int y = 1; y < temp_grey_image->getHeight() - 1; ++y) { + for (int x = 1; x < temp_grey_image->getWidth() - 1; ++x) { + int colVB, colVD, colHB, colHD; + colVB = Sobel::verticalBrighter(*temp_grey_image, x, y, 30); + colVD = Sobel::verticalDarker(*temp_grey_image, x, y, 30); + colHB = Sobel::horizontalBrighter(*temp_grey_image, x, y, 30); + colHD = Sobel::horizontalDarker(*temp_grey_image, x, y, 30); int dMax = colVD > colHD ? colVD : colHD; int bMax = colVB > colHB ? colVB : colHB; grey col; @@ -252,61 +269,61 @@ void PluginVisualize::DrawSobelImage( col.v = 2; else col.v = 0; - edge_image->setPixel(x,y,col); + edge_image->setPixel(x, y, col); } } unsigned int n = edge_image->getNumPixels(); - rgb * vis_ptr = vis_frame->data.getPixelData(); - grey * edge_ptr = edge_image->getPixelData(); + rgb *vis_ptr = vis_frame->data.getPixelData(); + grey *edge_ptr = edge_image->getPixelData(); rgb color; - for (unsigned int i=0;i& image_point, - const GVector::vector3d& field_point, - const GVector::vector3d& field_tangent, - VisualizationFrame* vis_frame, rgb edge_draw_color) { + const GVector::vector2d &image_point, + const GVector::vector3d &field_point, + const GVector::vector3d &field_tangent, + VisualizationFrame *vis_frame, rgb edge_draw_color) { const GVector::vector3d field_point_plus_tangent = field_point + 1000.0 * field_tangent; GVector::vector2d image_point_plus_tangent(0.0, 0.0); - camera_parameters.field2image( - field_point_plus_tangent, image_point_plus_tangent); + camera_parameters.field2image(field_point_plus_tangent, + image_point_plus_tangent); const GVector::vector2d edge_p1 = image_point + (image_point_plus_tangent - image_point).norm(6.0); const GVector::vector2d edge_p2 = image_point - (image_point_plus_tangent - image_point).norm(6.0); - vis_frame->data.drawLine(edge_p1.x, edge_p1.y, - edge_p2.x, edge_p2.y, edge_draw_color); + vis_frame->data.drawLine(edge_p1.x, edge_p1.y, edge_p2.x, edge_p2.y, + edge_draw_color); } -void PluginVisualize::DrawDetectedEdges( - FrameData* data, VisualizationFrame* vis_frame) { +void PluginVisualize::DrawDetectedEdges(FrameData *data, + VisualizationFrame *vis_frame) { // The edges: const rgb edge_draw_color = RGB::Red; for (size_t ls = 0; ls < camera_parameters.calibrationSegments.size(); ++ls) { - const CameraParameters::CalibrationData& segment = + const CameraParameters::CalibrationData &segment = camera_parameters.calibrationSegments[ls]; - for(unsigned int edge=0; edge& image_point = segment.imgPts[edge].first; - vis_frame->data.drawBox( - image_point.x - 5, image_point.y - 5, 11, 11, edge_draw_color); + for (unsigned int edge = 0; edge < segment.imgPts.size(); ++edge) { + if (!(segment.imgPts[edge].second)) + continue; + const GVector::vector2d &image_point = segment.imgPts[edge].first; + vis_frame->data.drawBox(image_point.x - 5, image_point.y - 5, 11, 11, + edge_draw_color); const double alpha = segment.alphas[edge]; if (segment.straightLine) { const GVector::vector3d field_point = @@ -318,12 +335,12 @@ void PluginVisualize::DrawDetectedEdges( } else { const double angle = alpha * segment.theta1 + (1.0 - alpha) * segment.theta2; - const GVector::vector3d field_radius_vector( - cos(angle), sin(angle), 0.0); + const GVector::vector3d field_radius_vector(cos(angle), + sin(angle), 0.0); const GVector::vector3d field_point = segment.center + segment.radius * field_radius_vector; - const GVector::vector3d field_tangent( - -sin(angle), cos(angle), 0.0); + const GVector::vector3d field_tangent(-sin(angle), cos(angle), + 0.0); DrawEdgeTangent(image_point, field_point, field_tangent, vis_frame, edge_draw_color); } @@ -332,33 +349,33 @@ void PluginVisualize::DrawDetectedEdges( DrawSearchCorridors(data, vis_frame); } -void PluginVisualize::DrawSearchCorridors( - FrameData* data, VisualizationFrame* vis_frame) { +void PluginVisualize::DrawSearchCorridors(FrameData *data, + VisualizationFrame *vis_frame) { static const int steps_per_line = 20; - const double half_corridor_width = camera_parameters. - additional_calibration_information->line_search_corridor_width-> - getDouble() * 0.5; + const double half_corridor_width = + camera_parameters.additional_calibration_information + ->line_search_corridor_width->getDouble() * + 0.5; real_field.field_markings_mutex.lockForRead(); for (size_t i = 0; i < real_field.field_lines.size(); ++i) { - const FieldLine& line_segment = - *(real_field.field_lines[i]); - const GVector::vector3d p1( - line_segment.p1_x->getDouble(), line_segment.p1_y->getDouble(), 0.0); - const GVector::vector3d p2( - line_segment.p2_x->getDouble(), line_segment.p2_y->getDouble(), 0.0); + const FieldLine &line_segment = *(real_field.field_lines[i]); + const GVector::vector3d p1(line_segment.p1_x->getDouble(), + line_segment.p1_y->getDouble(), 0.0); + const GVector::vector3d p2(line_segment.p2_x->getDouble(), + line_segment.p2_y->getDouble(), 0.0); const GVector::vector3d line_dir = (p2 - p1).norm(); const GVector::vector3d line_perp(-line_dir.y, line_dir.x, 0.0); drawFieldLine(p1 + half_corridor_width * line_perp, - p2 + half_corridor_width * line_perp, - steps_per_line, vis_frame, 180, 180, 255); + p2 + half_corridor_width * line_perp, steps_per_line, + vis_frame, 180, 180, 255); drawFieldLine(p1 - half_corridor_width * line_perp, - p2 - half_corridor_width * line_perp, - steps_per_line, vis_frame, 180, 180, 255); + p2 - half_corridor_width * line_perp, steps_per_line, + vis_frame, 180, 180, 255); } for (size_t i = 0; i < real_field.field_arcs.size(); ++i) { - const FieldCircularArc& arc = *(real_field.field_arcs[i]); - const GVector::vector3d center( - arc.center_x->getDouble(), arc.center_y->getDouble(), 0.0); + const FieldCircularArc &arc = *(real_field.field_arcs[i]); + const GVector::vector3d center(arc.center_x->getDouble(), + arc.center_y->getDouble(), 0.0); drawFieldArc(center, arc.radius->getDouble() + half_corridor_width, arc.a1->getDouble(), arc.a2->getDouble(), steps_per_line, vis_frame, 180, 180, 255); @@ -385,26 +402,54 @@ void PluginVisualize::DrawMaskHull( _image_mask.unlock(); } +#ifdef APRILTAG +void PluginVisualize::DrawDetectedAprilTags(FrameData *data, + VisualizationFrame *vis_frame) { + zarray_t *detections = + reinterpret_cast(data->map.get("apriltag_detections")); + if (detections == nullptr) { + return; + } + + // std::cout << "Detected " << zarray_size(detections) << " tags\n"; + for (int i = 0; i < zarray_size(detections); ++i) { + apriltag_detection_t *det; + zarray_get(detections, i, &det); + + for (int from_index = 0; from_index < 4; ++from_index) { + int to_index = (from_index + 1) % 4; + vis_frame->data.drawFatLine(det->p[from_index][0], det->p[from_index][1], + det->p[to_index][0], det->p[to_index][1], + RGB::Cyan); + } + + vis_frame->data.drawString(det->c[0], det->c[1], std::to_string(det->id), + RGB::Pink); + } +} +#endif + + ProcessResult PluginVisualize::process( FrameData* data, RenderOptions* options) { if (data == 0) return ProcessingFailed; - VisualizationFrame* vis_frame = - reinterpret_cast(data->map.get("vis_frame")); + VisualizationFrame *vis_frame = + reinterpret_cast(data->map.get("vis_frame")); if (vis_frame == 0) { - vis_frame = reinterpret_cast( - data->map.insert("vis_frame",new VisualizationFrame())); + vis_frame = reinterpret_cast( + data->map.insert("vis_frame", new VisualizationFrame())); } if (_v_enabled->getBool()) { - //check video data... - if (data->video.getWidth() == 0 || data->video.getHeight()==0) { - //there is no valid video data - //mark visualization data as invalid + // check video data... + if (data->video.getWidth() == 0 || data->video.getHeight() == 0) { + // there is no valid video data + // mark visualization data as invalid vis_frame->valid = false; return ProcessingOk; } else { - //allocate visualization frame accordingly: + // allocate visualization frame accordingly: vis_frame->data.allocate(data->video.getWidth(), data->video.getHeight()); } @@ -420,7 +465,7 @@ ProcessResult PluginVisualize::process( DrawThresholdedImage(data, vis_frame); } - //draw blob finding results: + // draw blob finding results: if (_v_blobs->getBool()) { DrawBlobs(data, vis_frame); } @@ -447,6 +492,12 @@ ProcessResult PluginVisualize::process( if(_v_mask_hull->getBool()) { DrawMaskHull(data, vis_frame); } +#ifdef APRILTAG + // Result of apriltag detection + if (_v_detected_apriltags->getBool()) { + DrawDetectedAprilTags(data, vis_frame); + } +#endif vis_frame->valid = true; } else { vis_frame->valid = false; @@ -454,40 +505,42 @@ ProcessResult PluginVisualize::process( return ProcessingOk; } -void PluginVisualize::setThresholdingLUT(LUT3D * threshold_lut) { - _threshold_lut=threshold_lut; +void PluginVisualize::setThresholdingLUT(LUT3D *threshold_lut) { + _threshold_lut = threshold_lut; } -void PluginVisualize::drawFieldArc( - const GVector::vector3d& center, - double radius, double theta1, double theta2, int steps, - VisualizationFrame* vis_frame, - unsigned char r, unsigned char g, unsigned char b) { +void PluginVisualize::drawFieldArc(const GVector::vector3d ¢er, + double radius, double theta1, double theta2, + int steps, VisualizationFrame *vis_frame, + unsigned char r, unsigned char g, + unsigned char b) { const double delta = (theta2 - theta1) / static_cast(steps); GVector::vector2d lastInImage(0.0, 0.0); - GVector::vector3d lastInWorld = center + + GVector::vector3d lastInWorld = + center + radius * GVector::vector3d(cos(theta1), sin(theta1), 0.0); camera_parameters.field2image(lastInWorld, lastInImage); for (int i = 1; i <= steps; ++i) { const double theta = theta1 + static_cast(i) * delta; - GVector::vector3d nextInWorld = center + + GVector::vector3d nextInWorld = + center + radius * GVector::vector3d(cos(theta), sin(theta), 0.0); GVector::vector2d nextInImage(0.0, 0.0); camera_parameters.field2image(nextInWorld, nextInImage); rgb draw_color; - draw_color.set(r,g,b); - vis_frame->data.drawFatLine( - lastInImage.x,lastInImage.y,nextInImage.x,nextInImage.y,draw_color); + draw_color.set(r, g, b); + vis_frame->data.drawFatLine(lastInImage.x, lastInImage.y, nextInImage.x, + nextInImage.y, draw_color); lastInWorld = nextInWorld; lastInImage = nextInImage; } } -void PluginVisualize::drawFieldLine( - const GVector::vector3d& start, - const GVector::vector3d& end, int steps, - VisualizationFrame* vis_frame, - unsigned char r, unsigned char g, unsigned char b) { +void PluginVisualize::drawFieldLine(const GVector::vector3d &start, + const GVector::vector3d &end, + int steps, VisualizationFrame *vis_frame, + unsigned char r, unsigned char g, + unsigned char b) { const GVector::vector3d delta = (end - start) / static_cast(steps); GVector::vector2d lastInImage(0.0, 0.0); @@ -498,9 +551,9 @@ void PluginVisualize::drawFieldLine( GVector::vector2d nextInImage; camera_parameters.field2image(nextInWorld, nextInImage); rgb draw_color; - draw_color.set(r,g,b); - vis_frame->data.drawFatLine( - lastInImage.x,lastInImage.y,nextInImage.x,nextInImage.y,draw_color); + draw_color.set(r, g, b); + vis_frame->data.drawFatLine(lastInImage.x, lastInImage.y, nextInImage.x, + nextInImage.y, draw_color); lastInWorld = nextInWorld; lastInImage = nextInImage; diff --git a/src/app/plugins/plugin_visualize.h b/src/app/plugins/plugin_visualize.h index 125b1cb5..bb8e30ea 100644 --- a/src/app/plugins/plugin_visualize.h +++ b/src/app/plugins/plugin_visualize.h @@ -65,6 +65,9 @@ class PluginVisualize : public VisionPlugin VarBool * _v_complete_sobel; VarBool * _v_detected_edges; VarBool * _v_mask_hull; +#ifdef APRILTAG + VarBool * _v_detected_apriltags; +#endif const CameraParameters& camera_parameters; const RoboCupField& real_field; @@ -108,6 +111,9 @@ class PluginVisualize : public VisionPlugin void DrawSearchCorridors(FrameData* data, VisualizationFrame* vis_frame); void DrawMaskHull(FrameData* data, VisualizationFrame* vis_frame); +#ifdef APRILTAG + void DrawDetectedAprilTags(FrameData* data, VisualizationFrame* vis_frame); +#endif public: PluginVisualize(FrameBuffer* _buffer, const CameraParameters& camera_params, const RoboCupField& real_field, const ConvexHullImageMask &mask); diff --git a/src/app/stacks/multistack_robocup_ssl.cpp b/src/app/stacks/multistack_robocup_ssl.cpp index ee1b5d39..4b3b4108 100644 --- a/src/app/stacks/multistack_robocup_ssl.cpp +++ b/src/app/stacks/multistack_robocup_ssl.cpp @@ -42,6 +42,14 @@ MultiStackRoboCupSSL::MultiStackRoboCupSSL(RenderOptions *_opts, int num_normal_ global_team_selector_yellow = new CMPattern::TeamSelector("Yellow Team", global_team_settings); settings->addChild(global_team_selector_yellow->getSettings()); +#ifdef APRILTAG + global_blue_team_tags = std::make_shared("Blue"); + settings->addChild(global_blue_team_tags->getSettings()); + + global_yellow_team_tags = std::make_shared("Yellow"); + settings->addChild(global_yellow_team_tags->getSettings()); +#endif /* APRILTAG */ + global_network_output_settings = new PluginSSLNetworkOutputSettings(); settings->addChild(global_network_output_settings->getSettings()); connect(global_network_output_settings->multicast_port, @@ -108,6 +116,10 @@ MultiStackRoboCupSSL::MultiStackRoboCupSSL(RenderOptions *_opts, int num_normal_ global_team_settings, global_team_selector_blue, global_team_selector_yellow, +#ifdef APRILTAG + global_blue_team_tags, + global_yellow_team_tags, +#endif /* APRILTAG */ ds_udp_server_new, ds_udp_server_old, "robocup-ssl-cam-" + QString::number(i).toStdString())); diff --git a/src/app/stacks/multistack_robocup_ssl.h b/src/app/stacks/multistack_robocup_ssl.h index 011b2bb3..f09bf15c 100644 --- a/src/app/stacks/multistack_robocup_ssl.h +++ b/src/app/stacks/multistack_robocup_ssl.h @@ -28,7 +28,9 @@ #include "plugin_publishgeometry.h" #include "cmpattern_teamdetector.h" #include "robocup_ssl_server.h" +#include "team_tags.h" #include "field.h" +#include using namespace std; /*! @@ -46,6 +48,10 @@ class MultiStackRoboCupSSL : public QObject, public MultiVisionStack { CMPattern::TeamDetectorSettings * global_team_settings; CMPattern::TeamSelector * global_team_selector_blue; CMPattern::TeamSelector * global_team_selector_yellow; +#ifdef APRILTAG + std::shared_ptr global_blue_team_tags; + std::shared_ptr global_yellow_team_tags; +#endif /* APRILTAG */ PluginSSLNetworkOutputSettings * global_network_output_settings; PluginLegacySSLNetworkOutputSettings * legacy_network_output_settings; diff --git a/src/app/stacks/stack_robocup_ssl.cpp b/src/app/stacks/stack_robocup_ssl.cpp index 67e88326..c3c45399 100644 --- a/src/app/stacks/stack_robocup_ssl.cpp +++ b/src/app/stacks/stack_robocup_ssl.cpp @@ -31,6 +31,10 @@ StackRoboCupSSL::StackRoboCupSSL( CMPattern::TeamDetectorSettings* _global_team_settings, CMPattern::TeamSelector * _global_team_selector_blue, CMPattern::TeamSelector * _global_team_selector_yellow, +#ifdef APRILTAG + std::shared_ptr _global_blue_team_tags, + std::shared_ptr _global_yellow_team_tags, +#endif /* APRILTAG */ RoboCupSSLServer * ds_udp_server_new, RoboCupSSLServer * ds_udp_server_old, string cam_settings_filename) : @@ -42,6 +46,10 @@ StackRoboCupSSL::StackRoboCupSSL( global_team_settings(_global_team_settings), global_team_selector_blue(_global_team_selector_blue), global_team_selector_yellow(_global_team_selector_yellow), +#ifdef APRILTAG + global_blue_team_tags(_global_blue_team_tags), + global_yellow_team_tags(_global_yellow_team_tags), +#endif /* APRILTAG */ _ds_udp_server_new(ds_udp_server_new), _ds_udp_server_old(ds_udp_server_old) { (void)_fb; @@ -70,6 +78,12 @@ StackRoboCupSSL::StackRoboCupSSL( stack.push_back(new PluginColorThreshold(_fb,lut_yuv, *_image_mask)); + stack.push_back(new PluginGreyscale(_fb)); + +#ifdef APRILTAG + stack.push_back(new PluginAprilTag(_fb, *camera_parameters, _global_blue_team_tags, _global_yellow_team_tags, *global_team_selector_blue, *global_team_selector_yellow, *_image_mask)); +#endif + stack.push_back(new PluginRunlengthEncode(_fb)); stack.push_back(new PluginFindBlobs(_fb,lut_yuv)); diff --git a/src/app/stacks/stack_robocup_ssl.h b/src/app/stacks/stack_robocup_ssl.h index 6ad3a5fb..d3bfe5c8 100644 --- a/src/app/stacks/stack_robocup_ssl.h +++ b/src/app/stacks/stack_robocup_ssl.h @@ -28,6 +28,11 @@ #include "plugin_dvr.h" #include "plugin_colorcalib.h" #include "plugin_cameracalib.h" +#include "plugin_greyscale.h" +#ifdef APRILTAG +#include "plugin_apriltag.h" +#include "team_tags.h" +#endif #include "plugin_visualize.h" #include "plugin_colorthreshold.h" #include "plugin_runlength_encode.h" @@ -65,6 +70,10 @@ class StackRoboCupSSL : public VisionStack { CMPattern::TeamDetectorSettings * global_team_settings; CMPattern::TeamSelector * global_team_selector_blue; CMPattern::TeamSelector * global_team_selector_yellow; +#ifdef APRILTAG + std::shared_ptr global_blue_team_tags; + std::shared_ptr global_yellow_team_tags; +#endif /* APRILTAG */ // UDP Server for Double-Sized field, new protobuf format. RoboCupSSLServer * _ds_udp_server_new; // UDP Server for Double-Sized field, old protobuf format. @@ -80,6 +89,10 @@ class StackRoboCupSSL : public VisionStack { CMPattern::TeamDetectorSettings* _global_team_settings, CMPattern::TeamSelector* _global_team_selector_blue, CMPattern::TeamSelector* _global_team_selector_yellow, +#ifdef APRILTAG + std::shared_ptr _global_blue_team_tags, + std::shared_ptr _global_yellow_team_tags, +#endif /* APRILTAG */ RoboCupSSLServer* ds_udp_server_new, RoboCupSSLServer* ds_udp_server_old, string cam_settings_filename); diff --git a/src/shared/CMakeLists.txt.inc b/src/shared/CMakeLists.txt.inc index d4911985..6ff713fe 100644 --- a/src/shared/CMakeLists.txt.inc +++ b/src/shared/CMakeLists.txt.inc @@ -12,6 +12,10 @@ include_directories(${shared_dir}/vartypes/enhanced) include_directories(${shared_dir}/vartypes/gui) include_directories(${shared_dir}/vartypes/xml) +if(USE_APRILTAG) + include_directories(${shared_dir}/apriltag) +endif() + set (SHARED_SRCS ${shared_dir}/capture/capturefromfile.cpp ${shared_dir}/capture/capture_generator.cpp @@ -95,10 +99,16 @@ set (SHARED_SRCS ${shared_dir}/util/helpers.cpp ${shared_dir}/util/field.cpp - ${shared_dir}/util/convex_hull.cpp - ${shared_dir}/util/convex_hull_image_mask.cpp + ${shared_dir}/util/convex_hull.cpp + ${shared_dir}/util/convex_hull_image_mask.cpp + + ${shared_dir}/util/team_tags.cpp ) +if(USE_APRILTAG) + set(SHARED_SRCS ${SHARED_SRCS} ${shared_dir}/apriltag/tagCustom20h7.c) +endif() + #only ones that need to be moc'ed set (SHARED_HEADERS ${shared_dir}/capture/capturefromfile.h @@ -108,6 +118,7 @@ set (SHARED_HEADERS ${shared_dir}/cmpattern/cmpattern_teamdetector.h ${shared_dir}/util/field.h + ${shared_dir}/util/team_tags.h ${shared_dir}/vartypes/VarNotifier.h @@ -131,6 +142,11 @@ set (SHARED_HEADERS ${shared_dir}/vartypes/gui/VarTreeView.h ) +if(USE_APRILTAG) + set(SHARED_HEADERS ${SHARED_HEADERS} ${shared_dir}/apriltag/tagCustom20h7.h) +endif() + + set (SHARED_RC ${shared_dir}/vartypes/gui/icons/icons.qrc ) diff --git a/src/shared/apriltag/tagCustom20h7.c b/src/shared/apriltag/tagCustom20h7.c new file mode 100644 index 00000000..f9d05c65 --- /dev/null +++ b/src/shared/apriltag/tagCustom20h7.c @@ -0,0 +1,93 @@ +#include +#include "apriltag.h" + +apriltag_family_t *tagCustom20h7_create() +{ + apriltag_family_t *tf = calloc(1, sizeof(apriltag_family_t)); + tf->name = strdup("tagCustom20h7"); + tf->h = 7; + tf->ncodes = 26; + tf->codes = calloc(26, sizeof(uint64_t)); + tf->codes[0] = 0x0000000000094dddUL; + tf->codes[1] = 0x0000000000075967UL; + tf->codes[2] = 0x0000000000065f2cUL; + tf->codes[3] = 0x0000000000046ab6UL; + tf->codes[4] = 0x0000000000017c05UL; + tf->codes[5] = 0x00000000000d9319UL; + tf->codes[6] = 0x000000000009aa2dUL; + tf->codes[7] = 0x00000000000abf1dUL; + tf->codes[8] = 0x00000000000f0459UL; + tf->codes[9] = 0x0000000000041ceaUL; + tf->codes[10] = 0x0000000000022874UL; + tf->codes[11] = 0x000000000008208bUL; + tf->codes[12] = 0x00000000000493efUL; + tf->codes[13] = 0x00000000000d1236UL; + tf->codes[14] = 0x000000000006dad7UL; + tf->codes[15] = 0x000000000009070aUL; + tf->codes[16] = 0x000000000008c56fUL; + tf->codes[17] = 0x00000000000b1f81UL; + tf->codes[18] = 0x00000000000e4025UL; + tf->codes[19] = 0x0000000000013129UL; + tf->codes[20] = 0x000000000008dcc3UL; + tf->codes[21] = 0x00000000000e8e12UL; + tf->codes[22] = 0x0000000000045a0bUL; + tf->codes[23] = 0x00000000000def03UL; + tf->codes[24] = 0x00000000000ef14dUL; + tf->codes[25] = 0x00000000000d9c0fUL; + tf->nbits = 20; + tf->bit_x = calloc(20, sizeof(uint32_t)); + tf->bit_y = calloc(20, sizeof(uint32_t)); + tf->bit_x[0] = 0; + tf->bit_y[0] = -2; + tf->bit_x[1] = 1; + tf->bit_y[1] = -2; + tf->bit_x[2] = 2; + tf->bit_y[2] = -2; + tf->bit_x[3] = 3; + tf->bit_y[3] = -2; + tf->bit_x[4] = 1; + tf->bit_y[4] = 1; + tf->bit_x[5] = 5; + tf->bit_y[5] = 0; + tf->bit_x[6] = 5; + tf->bit_y[6] = 1; + tf->bit_x[7] = 5; + tf->bit_y[7] = 2; + tf->bit_x[8] = 5; + tf->bit_y[8] = 3; + tf->bit_x[9] = 2; + tf->bit_y[9] = 1; + tf->bit_x[10] = 3; + tf->bit_y[10] = 5; + tf->bit_x[11] = 2; + tf->bit_y[11] = 5; + tf->bit_x[12] = 1; + tf->bit_y[12] = 5; + tf->bit_x[13] = 0; + tf->bit_y[13] = 5; + tf->bit_x[14] = 2; + tf->bit_y[14] = 2; + tf->bit_x[15] = -2; + tf->bit_y[15] = 3; + tf->bit_x[16] = -2; + tf->bit_y[16] = 2; + tf->bit_x[17] = -2; + tf->bit_y[17] = 1; + tf->bit_x[18] = -2; + tf->bit_y[18] = 0; + tf->bit_x[19] = 1; + tf->bit_y[19] = 2; + tf->width_at_border = 4; + tf->total_width = 8; + tf->reversed_border = false; + return tf; +} + +void tagCustom20h7_destroy(apriltag_family_t *tf) +{ + free(tf->codes); + free(tf->bit_x); + free(tf->bit_y); + free(tf->name); + free(tf); +} diff --git a/src/shared/apriltag/tagCustom20h7.h b/src/shared/apriltag/tagCustom20h7.h new file mode 100644 index 00000000..065b32a6 --- /dev/null +++ b/src/shared/apriltag/tagCustom20h7.h @@ -0,0 +1,15 @@ +#ifndef _TAGCustom20H7 +#define _TAGCustom20H7 + +#ifdef __cplusplus +extern "C" { +#endif + +apriltag_family_t *tagCustom20h7_create(); +void tagCustom20h7_destroy(apriltag_family_t *tf); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/shared/cmpattern/cmpattern_teamdetector.cpp b/src/shared/cmpattern/cmpattern_teamdetector.cpp index db4c1bd2..15f08f90 100644 --- a/src/shared/cmpattern/cmpattern_teamdetector.cpp +++ b/src/shared/cmpattern/cmpattern_teamdetector.cpp @@ -181,7 +181,7 @@ TeamDetector::~TeamDetector() void TeamDetector::update(::google::protobuf::RepeatedPtrField< ::SSL_DetectionRobot >* robots, int team_color_id, int max_robots, const Image * image, CMVision::ColorRegionList * colorlist, CMVision::RegionTree & reg_tree) { color_id_team=team_color_id; _max_robots=max_robots; - robots->Clear(); + // robots->Clear(); if (_unique_patterns) { findRobotsByModel(robots,team_color_id,image,colorlist,reg_tree); diff --git a/src/shared/util/camera_calibration.cpp b/src/shared/util/camera_calibration.cpp index 1add95aa..e339c300 100644 --- a/src/shared/util/camera_calibration.cpp +++ b/src/shared/util/camera_calibration.cpp @@ -80,7 +80,7 @@ void CameraParameters::toProtoBuffer(SSL_GeometryCameraCalibration &buffer) cons } -GVector::vector3d< double > CameraParameters::getWorldLocation() { +GVector::vector3d< double > CameraParameters::getWorldLocation() const { Quaternion q; q.set(q0->getDouble(),q1->getDouble(),q2->getDouble(),q3->getDouble()); q.invert(); diff --git a/src/shared/util/camera_calibration.h b/src/shared/util/camera_calibration.h index b0bffa28..3e4841df 100644 --- a/src/shared/util/camera_calibration.h +++ b/src/shared/util/camera_calibration.h @@ -77,7 +77,7 @@ class CameraParameters AdditionalCalibrationInformation* additional_calibration_information; - GVector::vector3d getWorldLocation(); + GVector::vector3d getWorldLocation() const; void field2image(const GVector::vector3d &p_f, GVector::vector2d &p_i) const; void image2field(GVector::vector3d< double >& p_f, const GVector::vector2d< double >& p_i, double z) const; void calibrate(std::vector > &p_f, std::vector > &p_i, int cal_type); diff --git a/src/shared/util/field.cpp b/src/shared/util/field.cpp index 7d8efc8d..65ebe315 100644 --- a/src/shared/util/field.cpp +++ b/src/shared/util/field.cpp @@ -351,7 +351,7 @@ RoboCupField::RoboCupField() { num_cameras_local = new VarInt( "Local Number of Cameras", FieldConstantsRoboCup2018A::kNumCamerasLocal); - updateShapes = new VarTrigger("Field Lines/Arcs","Update"); + updateShapes = new VarTrigger("Field Lines/Arcs","Use Defaults"); var_num_lines = new VarInt("Number of Line Segments", 0); var_num_arcs = new VarInt("Number of Arcs", 0); field_lines_list = new VarList("Field Lines"); diff --git a/src/shared/util/team_tags.cpp b/src/shared/util/team_tags.cpp new file mode 100644 index 00000000..d5a4d3f8 --- /dev/null +++ b/src/shared/util/team_tags.cpp @@ -0,0 +1,198 @@ +#include "team_tags.h" +#include + +using namespace VarTypes; + +TeamTagPair::TeamTagPair() + : tag_pair{new VarList{"Unset"}}, tag_id{new VarInt{"Value", -1, -1}}, + delete_button{new VarTrigger{"Delete", "Delete"}} { + tag_pair->addChild(tag_id.get()); + tag_pair->addChild(delete_button.get()); + + connect(tag_id.get(), SIGNAL(wasEdited(VarType *)), this, SLOT(tagChanged())); +} + +TeamTagPair::TeamTagPair(VarList *tag_pair) : tag_pair{tag_pair} { + for (const auto &child : tag_pair->getChildren()) { + if (VarInt *loaded_tag_id = dynamic_cast(child)) { + tag_id.reset(loaded_tag_id); + connect(tag_id.get(), SIGNAL(wasEdited(VarType *)), this, + SLOT(tagChanged())); + } else if (VarTrigger *loaded_delete_button = + dynamic_cast(child)) { + loaded_delete_button->setLabel("Delete"); + delete_button.reset(loaded_delete_button); + } + } +} + +void TeamTagPair::tagChanged() { + const int value = tag_id->getInt(); + if (value >= 0) { + tag_pair->setName(std::to_string(value)); + } else { + tag_pair->setName("Unset"); + } + + emit teamTagPairChanged(this); +} + +TeamTags::TeamTags(const std::string &team_name) + : settings{new VarList{"" + team_name + " April Tags"}}, + add_tag_trigger{new VarTrigger{"Add Tag", "Add Tag"}} { + + settings->addChild(add_tag_trigger.get()); + connect(settings.get(), SIGNAL(XMLwasRead(VarType *)), this, + SLOT(tagListLoaded())); + connect(add_tag_trigger.get(), SIGNAL(wasEdited(VarType *)), this, + SLOT(addTag())); +} + +void TeamTags::tagListLoaded() { + const auto children = settings->getChildren(); + + tags.clear(); + tag_to_var_map.clear(); + + for (const auto &child : children) { + if (VarList *tag_pair = dynamic_cast(child)) { + auto team_tag_pair = + std::unique_ptr(new TeamTagPair{tag_pair}); + const int id = team_tag_pair->tag_id->getInt(); + + auto result = tags.insert(id); + if (result.second) { + connect(team_tag_pair.get(), SIGNAL(teamTagPairChanged(TeamTagPair *)), + this, SLOT(tagChanged(TeamTagPair *))); + connect(team_tag_pair->delete_button.get(), + SIGNAL(hasChanged(VarType *)), this, + SLOT(deleteTag(VarType *))); + tag_to_var_map.insert({id, std::move(team_tag_pair)}); + } else { + // this is a duplicate, remove from settings + settings->removeChild(team_tag_pair->tag_pair.get()); + } + } + } + + sortSettingsList(); +} + +void TeamTags::tagChanged(TeamTagPair *changed_tag_pair) { + // find the tag pair in the map + int old_tag_id; + std::unique_ptr team_tag_pair; + for (auto &tag_var_pair : tag_to_var_map) { + if (tag_var_pair.second.get() == changed_tag_pair) { + old_tag_id = tag_var_pair.first; + team_tag_pair = std::move(tag_var_pair.second); + break; + } + } + + if (!team_tag_pair) { + return; + } + + tags.erase(old_tag_id); + tag_to_var_map.erase(old_tag_id); + + // try to insert it + const int tag_id = team_tag_pair->tag_id->getInt(); + auto result = tags.insert(tag_id); + if (result.second) { + tag_to_var_map.insert({tag_id, std::move(team_tag_pair)}); + + sortSettingsList(); + } else { + settings->removeChild(team_tag_pair->tag_pair.get()); + } +} + +void TeamTags::addTag() { + // insert the new tag + tags.insert(-1); + auto result = tag_to_var_map.insert( + {-1, std::unique_ptr(new TeamTagPair{})}); + + // if the insert failed then there is already an unset tag, so don't + // bother connecting the new vartype, it will be delete after this + // function exits + if (result.second) { + settings->addChild(result.first->second->tag_pair.get()); + connect(result.first->second.get(), + SIGNAL(teamTagPairChanged(TeamTagPair *)), this, + SLOT(tagChanged(TeamTagPair *))); + connect(result.first->second->delete_button.get(), + SIGNAL(hasChanged(VarType *)), this, SLOT(deleteTag(VarType *))); + + sortSettingsList(); + } +} + +void TeamTags::deleteTag(VarType *deleted_var) { + int tag_id; + std::unique_ptr team_tag_pair; + for (auto &tag_var_pair : tag_to_var_map) { + if (tag_var_pair.second->delete_button.get() == deleted_var) { + tag_id = tag_var_pair.first; + team_tag_pair = std::move(tag_var_pair.second); + } + } + + if (!team_tag_pair) { + return; + } + + settings->removeChild(team_tag_pair->tag_pair.get()); + tag_to_var_map.erase(tag_id); + tags.erase(tag_id); +} + +void TeamTags::sortSettingsList() { + auto children = settings->getChildren(); + for (const auto &child : children) { + settings->removeChild(child); + } + + std::sort(children.begin(), children.end(), + // should be no nullptr + [](VarType *a, VarType *b) { + assert(a != nullptr); + assert(b != nullptr); + + if (VarList *tag_pair_a = dynamic_cast(a)) { + if (VarList *tag_pair_b = dynamic_cast(b)) { + // find tag id var in pair a + int tag_id_a = -1; + for (const auto &child : tag_pair_a->getChildren()) { + if (VarInt *tag_id_var_a = dynamic_cast(child)) { + tag_id_a = tag_id_var_a->getInt(); + } + } + int tag_id_b = -1; + for (const auto &child : tag_pair_b->getChildren()) { + if (VarInt *tag_id_var_b = dynamic_cast(child)) { + tag_id_b = tag_id_var_b->getInt(); + } + } + + return tag_id_a < tag_id_b; + } else { + return false; + } + } else { + // a isn't a list, so it is a button (or something else that + // belongs at the top) + if (VarList *tag_pair_b = dynamic_cast(b)) { + return true; + } else { + return false; + } + } + }); + + for (auto &child : children) { + settings->addChild(child); + } +} diff --git a/src/shared/util/team_tags.h b/src/shared/util/team_tags.h new file mode 100644 index 00000000..c746157f --- /dev/null +++ b/src/shared/util/team_tags.h @@ -0,0 +1,67 @@ +#ifndef TEAM_TAGS_H +#define TEAM_TAGS_H + +#include +#include +#include +#include +#include + +class TeamTags; + +class TeamTagPair : public QObject { + Q_OBJECT +private: + using VarType = VarTypes::VarType; + +public: + TeamTagPair(); + TeamTagPair(VarTypes::VarList *tag_pair); + ~TeamTagPair() = default; + +public slots: + void tagChanged(); + +signals: + void teamTagPairChanged(TeamTagPair*); +private: + std::unique_ptr tag_pair; + std::unique_ptr tag_id; + std::unique_ptr delete_button; + + friend TeamTags; +}; + +class TeamTags : public QObject { + Q_OBJECT +private: + using TagVarMap = std::unordered_map>; + using TagSet = std::unordered_set; + using VarType = VarTypes::VarType; + +public: + TeamTags(const std::string &team_name); + ~TeamTags() = default; + + VarTypes::VarList *getSettings() const { return settings.get(); } + + const TagSet &operator*() const { return tags; } + + const TagSet *operator->() const { return &tags; } + +public slots: + void tagListLoaded(); + void tagChanged(TeamTagPair* changed_tag_pair); + void addTag(); + void deleteTag(VarType *deleted_var); + +private: + void sortSettingsList(); +private: + std::unique_ptr settings; + TagSet tags; + TagVarMap tag_to_var_map; + std::unique_ptr add_tag_trigger; +}; + +#endif /* TEAM_TAGS_H */