auto-aim

armor/include/armor_finder/armor_finder.h

@@ -0,0 +1,57 @@
+//
+// Created by xinyang on 19-3-27.
+//
+
+#ifndef _ARMOR_FINDER_H_
+#define _ARMOR_FINDER_H_
+
+#include <opencv2/core.hpp>
+#include <opencv2/tracking.hpp>
+#include <uart/uart.h>
+
+typedef enum{
+    ENEMY_BLUE, ENEMY_RED
+} EnemyColor;
+
+class ArmorFinder{
+public:
+    ArmorFinder(EnemyColor color, Uart &u);
+    ~ArmorFinder() = default;
+
+private:
+    typedef cv::TrackerKCF TrackerToUse;
+
+    typedef enum{
+        SEARCHING_STATE, TRACKING_STATE, STANDBY_STATE
+    } State;
+
+    EnemyColor enemy_color;
+    State state;
+    cv::Rect2d armor_box;
+    cv::Ptr<cv::Tracker> tracker;
+
+    int  contour_area;
+    Uart &uart;
+
+    bool stateSearchingTarget(cv::Mat &src);
+    bool stateTrackingTarget(cv::Mat &src);
+    bool stateStandBy();
+public:
+    void run(cv::Mat &src);
+    bool sendBoxPosition();
+};
+
+struct LightBlob {
+    cv::RotatedRect rect;
+    double length;
+
+    explicit LightBlob(cv::RotatedRect &r) : rect(r) {
+        length = std::max(rect.size.height, rect.size.width);
+    };
+    bool operator<(LightBlob &l2) { return this->rect.center.x < l2.rect.center.x; }
+    bool operator<=(LightBlob &l2) { return this->rect.center.x <= l2.rect.center.x; }
+    bool operator>(LightBlob &l2) { return this->rect.center.x > l2.rect.center.x; }
+    bool operator>=(LightBlob &l2) { return this->rect.center.x >= l2.rect.center.x; }
+};
+
+#endif /* _ARMOR_FINDER_H_ */

armor/include/show_images/show_images.h

@@ -0,0 +1,17 @@
+//
+// Created by xinyang on 19-3-27.
+//
+
+#ifndef _SHOW_IMAGES_H_
+#define _SHOW_IMAGES_H_
+
+#include <opencv2/core.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+#include <armor_finder/armor_finder.h>
+
+void showArmorBoxVector(std::string windows_name, const cv::Mat &src, const std::vector<cv::Rect2d> &armor_box);
+void showArmorBox(std::string windows_name, const cv::Mat &src, cv::Rect2d armor_box);
+void showContours(std::string windows_name, const cv::Mat &src, const std::vector<LightBlob> &light_blobs);
+
+#endif /* _SHOW_IMAGES_H_ */

armor/src/armor_finder/armor_finder.cpp

@@ -0,0 +1,66 @@
+//
+// Created by xinyang on 19-3-27.
+//
+#include <log.h>
+#include <armor_finder/armor_finder.h>
+
+ArmorFinder::ArmorFinder(EnemyColor color, Uart &u) :
+                            uart(u),
+                            enemy_color(color),
+                            state(STANDBY_STATE)
+                            {
+    auto para = TrackerToUse::Params();
+    para.desc_npca = 1;
+    para.desc_pca = 0;
+    tracker = TrackerToUse::create(para);
+    if(!tracker){
+        LOGW("Tracker Not init");
+    }
+}
+
+void ArmorFinder::run(cv::Mat &src) {
+    cv::Mat src_use;
+    if (src.type() == CV_8UC3) {
+        cv::cvtColor(src, src_use, CV_RGB2GRAY);
+    }else{
+        src_use = src.clone();
+    }
+
+//    return stateSearchingTarget(src_use);
+
+    switch (state){
+        case SEARCHING_STATE:
+            if(stateSearchingTarget(src_use)){
+                if((armor_box & cv::Rect2d(0, 0, 640, 480)) == armor_box) {
+                    cv::Mat roi = src_use.clone()(armor_box);
+                    cv::threshold(roi, roi, 200, 255, cv::THRESH_BINARY);
+                    contour_area = cv::countNonZero(roi);
+                    tracker->init(src_use, armor_box);
+                    state = TRACKING_STATE;
+                    LOGW("into track");
+                }
+            }
+            break;
+        case TRACKING_STATE:
+            if(!stateTrackingTarget(src_use)){
+                state = SEARCHING_STATE;
+                //std::cout << "into search!" << std::endl;
+            }
+            break;
+        case STANDBY_STATE:
+        default:
+            stateStandBy();
+    }
+}
+
+#define FOCUS_PIXAL      (0.36/0.48*640)
+
+bool ArmorFinder::sendBoxPosition() {
+    auto rect = armor_box;
+    double dx = rect.x + rect.width/2 - 320;
+    double dy = rect.y + rect.height/2 - 240;
+    double yaw   = atan(dx / FOCUS_PIXAL) * 180 / 3.14159265459;
+    double pitch = atan(dy / FOCUS_PIXAL) * 180 / 3.14159265459;
+    uart.sendTarget(yaw, pitch, 0);
+    return true;
+}

armor/src/armor_finder/state_machine/searching_state/image_process/image_process.cpp

@@ -0,0 +1,63 @@
+//
+// Created by xinyang on 19-3-27.
+//
+
+#include "image_process.h"
+
+static void splitBayerBG(cv::Mat &src, cv::Mat &blue, cv::Mat &red) {
+    uchar* data;
+    uchar* bayer_data[2];
+    for (int i = 0; i < src.rows; ++i) {
+        data = src.ptr<uchar>(i);
+        bayer_data[0] = blue.ptr<uchar>(i / 2);
+        for (int j = 0; j < blue.cols; ++j, data += 2) {
+            bayer_data[0][j] = *data;
+        }
+        data = src.ptr<uchar>(++i) + 1;
+        bayer_data[1] = red.ptr<uchar>(i / 2);
+        for (int j = 0; j < red.cols; ++j, data += 2) {
+            bayer_data[1][j] = *data;
+        }
+    }
+}
+
+void imageColorSplit(cv::Mat &src_input, cv::Mat &split, EnemyColor color) {
+    cv::Mat blue(240, 320, CV_8UC1), red(240, 320, CV_8UC1);
+    if(src_input.type() == CV_8UC1){
+        splitBayerBG(src_input, blue, red);
+        if(color == ENEMY_RED){
+            split = red - blue;
+        }else if(color == ENEMY_BLUE){
+            split = blue - red;
+        }
+    }else if(src_input.type() == CV_8UC3){
+        std::vector<cv::Mat> channels;
+        cv::split(src_input, channels);
+        resize(channels.at(0), blue, cv::Size(640, 480));
+        resize(channels.at(2), red, cv::Size(640, 480));
+        if(color == ENEMY_RED){
+            split = red;
+        }else if(color == ENEMY_BLUE){
+            split = blue;
+        }
+    }
+
+}
+
+void imagePreProcess(cv::Mat &src) {
+    static cv::Mat kernel_erode = getStructuringElement(cv::MORPH_RECT, cv::Size(1, 4));
+    erode(src, src, kernel_erode);
+
+    static cv::Mat kernel_dilate = getStructuringElement(cv::MORPH_RECT, cv::Size(2, 4));
+    dilate(src, src, kernel_dilate);
+
+    static cv::Mat kernel_erode2 = getStructuringElement(cv::MORPH_RECT, cv::Size(2, 4));
+    erode(src, src, kernel_erode2);
+
+    static cv::Mat kernel_dilate2 = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 6));
+    dilate(src, src, kernel_dilate2);
+
+    float alpha = 1.5;
+    int beta = 0;
+    src.convertTo(src, -1, alpha, beta);
+}

armor/src/armor_finder/state_machine/searching_state/image_process/image_process.h

@@ -0,0 +1,14 @@
+//
+// Created by xinyang on 19-3-27.
+//
+
+#ifndef _IMAGE_PROCESS_H_
+#define _IMAGE_PROCESS_H_
+
+#include <opencv2/core.hpp>
+#include <armor_finder/armor_finder.h>
+
+void imageColorSplit(cv::Mat &src_input, cv::Mat &split, EnemyColor color);
+void imagePreProcess(cv::Mat &src);
+
+#endif /* _IMAGE_PROCESS_H_ */

armor/src/armor_finder/state_machine/searching_state/searching_state.cpp

@@ -0,0 +1,192 @@
+//
+// Created by xinyang on 19-3-27.
+//
+
+#include <armor_finder/armor_finder.h>
+#include <opencv2/highgui.hpp>
+#include "image_process/image_process.h"
+#include <log.h>
+#include <show_images/show_images.h>
+
+#include <options/options.h>
+
+typedef std::vector<LightBlob> LightBlobs;
+
+static double lw_rate(const cv::RotatedRect &rect){
+    return (rect.size.height > rect.size.width)?
+           (rect.size.height / rect.size.width):
+           (rect.size.width / rect.size.height);
+}
+
+static bool isValidLightBlob(const cv::RotatedRect &rect){
+    return (lw_rate(rect) > 1.2) &&
+           ((rect.size.width * rect.size.height) < 3000) &&
+           ((rect.size.width * rect.size.height) > 1);
+}
+
+static void pipelineLightBlobPreprocess(cv::Mat &src) {
+    src -= 150;
+    src *= 3.5;
+    src -= 150;
+    src *= 3.5;
+}
+
+static bool findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) {
+    static cv::Mat src_bin;
+
+    cv::threshold(src, src_bin, 80, 255, CV_THRESH_BINARY);
+    std::vector<std::vector<cv::Point> > light_contours;
+    cv::findContours(src_bin, light_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
+    for (auto &light_contour : light_contours) {
+        cv::RotatedRect rect = cv::minAreaRect(light_contour);
+        if(isValidLightBlob(rect)){
+            light_blobs.emplace_back(rect);
+        }
+    }
+    return light_blobs.size() >= 2;
+}
+
+bool angelJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) {
+
+    float angle_i = light_blob_i.rect.size.width > light_blob_i.rect.size.height ? light_blob_i.rect.angle :
+                    light_blob_i.rect.angle - 90;
+    float angle_j = light_blob_j.rect.size.width > light_blob_j.rect.size.height ? light_blob_j.rect.angle :
+                    light_blob_j.rect.angle - 90;
+    return abs(angle_i-angle_j)<10;
+}
+bool heightJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) {
+    cv::Point2f centers = light_blob_i.rect.center - light_blob_j.rect.center;
+
+    return abs(centers.y)<30;
+}
+
+bool lengthJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) {
+    double side_length;
+    cv::Point2f centers = light_blob_i.rect.center - light_blob_j.rect.center;
+    side_length = sqrt(centers.ddot(centers));
+//    std::cout << "side:" << side_length << " length:" << light_blob_i.length  << std::endl;
+    return (side_length / light_blob_i.length < 6 && side_length / light_blob_i.length > 0.5);
+}
+
+bool lengthRatioJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) {
+//    std::cout << "i:" << light_blob_i.length << " j:" << light_blob_j.length << std::endl;
+    return (light_blob_i.length / light_blob_j.length < 2
+            && light_blob_i.length / light_blob_j.length > 0.5);
+}
+
+bool isCoupleLight(const LightBlob &light_blob_i, const LightBlob &light_blob_j) {
+    if(!lengthRatioJudge(light_blob_i, light_blob_j)){
+//        std::cout << "lengthRatioJudge" << std::endl;
+        return false;
+    }
+    if(!lengthJudge(light_blob_i, light_blob_j)){
+//        std::cout << "lengthJudge" << std::endl;
+        return false;
+    }
+    if(!heightJudge(light_blob_i, light_blob_j)){
+//        std::cout << "heightJudge" << std::endl;
+        return false;
+    }
+    if(!angelJudge(light_blob_i, light_blob_j)){
+//        std::cout << "angelJudge" << std::endl;
+        return false;
+    }
+    return true;
+    return lengthRatioJudge(light_blob_i, light_blob_j) &&
+           lengthJudge(light_blob_i, light_blob_j) &&
+           heightJudge(light_blob_i, light_blob_j) &&
+           angelJudge(light_blob_i, light_blob_j);
+
+}
+
+double centerDistance(cv::Rect2d box){
+    double dx = box.x-box.width/2 - 320;
+    double dy = box.y-box.height/2 - 240;
+    return dx*dx + dy*dy;
+}
+
+static bool findArmorBoxes(LightBlobs &light_blobs, std::vector<cv::Rect2d> &armor_boxes) {
+    for (int i = 0; i < light_blobs.size() - 1; ++i) {
+        for (int j = i + 1; j < light_blobs.size(); ++j) {
+            if (!isCoupleLight(light_blobs.at(i), light_blobs.at(j))) {
+                continue;
+            }
+            cv::Rect2d rect_left = light_blobs.at(static_cast<unsigned long>(i)).rect.boundingRect();
+            cv::Rect2d rect_right = light_blobs.at(static_cast<unsigned long>(j)).rect.boundingRect();
+            double min_x, min_y, max_x, max_y;
+            min_x = fmin(rect_left.x, rect_right.x) - 5;
+            max_x = fmax(rect_left.x + rect_left.width, rect_right.x + rect_right.width) + 5;
+            min_y = fmin(rect_left.y, rect_right.y) - 5;
+            max_y = fmax(rect_left.y + rect_left.height, rect_right.y + rect_right.height) + 5;
+            if (min_x < 0 || max_x > 640 || min_y < 0 || max_y > 480) {
+                continue;
+            }
+            armor_boxes.emplace_back(cv::Rect2d(min_x, min_y, max_x - min_x, max_y - min_y));
+        }
+    }
+    if(armor_boxes.empty()){
+        return false;
+    }
+    sort(armor_boxes.begin(), armor_boxes.end(), [](cv::Rect2d box1, cv::Rect2d box2)->bool{
+        return centerDistance(box1) < centerDistance(box2);
+    });
+    return true;
+}
+
+bool judge_light_color(std::vector<LightBlob> &light, std::vector<LightBlob> &color, std::vector<LightBlob> &result) {
+    for (auto &i:color) {
+        for (auto &j:light) {
+            cv::Rect2d a = i.rect.boundingRect2f();
+            cv::Rect2d b = j.rect.boundingRect2f();
+            cv::Rect2d ab = a & b;
+            if (ab.area() / fmin(a.area(), b.area()) >= 0.2) {
+                result.emplace_back(j);
+                break;
+            }
+        }
+    }
+    return !result.empty();
+}
+
+bool ArmorFinder::stateSearchingTarget(cv::Mat &src) {
+    cv::Mat split, pmsrc=src.clone();
+    LightBlobs light_blobs, pm_light_blobs, light_blobs_real;
+    std::vector<cv::Rect2d> armor_boxes;
+
+//    cv::resize(src, pmsrc, cv::Size(320, 240));
+    imageColorSplit(src, split, enemy_color);
+    imagePreProcess(split);
+    cv::resize(split, split, cv::Size(640, 480));
+//    pipelineLightBlobPreprocess(pmsrc);
+//    if(!findLightBlobs(pmsrc, pm_light_blobs)){
+//        return false;
+//    }
+    if(!findLightBlobs(split, light_blobs)){
+        return false;
+    }
+//    if(!judge_light_color(light_blobs, pm_light_blobs, light_blobs_real)){
+//        return false;
+//    }
+    if(show_light_blobs){
+        showContours("blobs", split, light_blobs);
+//        showContours("pm blobs", pmsrc, pm_light_blobs);
+//        showContours("blobs real", src, light_blobs_real);
+        cv::waitKey(1);
+    }
+    if(!findArmorBoxes(light_blobs, armor_boxes)){
+        return false;
+    }
+    armor_box = armor_boxes[0];
+    if(show_armor_boxes){
+        showArmorBoxVector("boxes", split, armor_boxes);
+        cv::waitKey(1);
+    }
+    if(split.size() == cv::Size(320, 240)){
+        armor_box.x *= 2;
+        armor_box.y *= 2;
+        armor_box.width *= 2;
+        armor_box.height *= 2;
+    }
+    
+    return sendBoxPosition();
+}

armor/src/armor_finder/state_machine/standby_state/standby_state.cpp

@@ -0,0 +1,11 @@
+//
+// Created by xinyang on 19-3-27.
+//
+
+#include <armor_finder/armor_finder.h>
+
+bool ArmorFinder::stateStandBy() {
+    state = SEARCHING_STATE;
+    return true;
+}
+

armor/src/armor_finder/state_machine/tracking_state/tracking_state.cpp

@@ -0,0 +1,21 @@
+//
+// Created by xinyang on 19-3-27.
+//
+#include <log.h>
+#include <armor_finder/armor_finder.h>
+
+bool ArmorFinder::stateTrackingTarget(cv::Mat &src) {
+    auto last = armor_box;
+    tracker->update(src, armor_box);
+
+    if((armor_box & cv::Rect2d(0, 0, 640, 480)) != armor_box){
+        return false;
+    }
+    cv::Mat roi = src(armor_box);
+    threshold(roi, roi, 200, 255, cv::THRESH_BINARY);
+
+    if(abs(cv::countNonZero(roi) - contour_area) > contour_area * 0.3){
+        return false;
+    }
+    return sendBoxPosition();
+}

armor/src/show_images/show_images.cpp

@@ -0,0 +1,52 @@
+#include <show_images/show_images.h>
+
+using namespace cv;
+
+void showArmorBoxVector(std::string windows_name, const cv::Mat &src, const std::vector<cv::Rect2d> &armor_box) {
+    static Mat image2show;
+    if (src.type() == CV_8UC1) // 黑白图像
+    {
+        cvtColor(src, image2show, COLOR_GRAY2RGB);
+    } else if(src.type() == CV_8UC3) //RGB 彩色
+    {
+        image2show = src.clone();
+    }
+
+    for (auto &box:armor_box) {
+        rectangle(image2show, box, Scalar(0, 255, 0), 1);
+    }
+    imshow(windows_name, image2show);
+}
+
+void showArmorBox(std::string windows_name, const cv::Mat &src, cv::Rect2d armor_box) {
+    static Mat image2show;
+    if (src.type() == CV_8UC1) // 黑白图像
+    {
+        cvtColor(src, image2show, COLOR_GRAY2RGB);
+    } else if(src.type() == CV_8UC3) //RGB 彩色
+    {
+        image2show = src.clone();
+    }
+    rectangle(image2show, armor_box, Scalar(0, 255, 0), 1);
+    imshow(windows_name, image2show);
+}
+
+void showContours(std::string windows_name, const cv::Mat &src, const std::vector<LightBlob> &light_blobs) {
+    static Mat image2show;
+
+    if(src.type() == CV_8UC1) // 黑白图像
+    {
+        cvtColor(src, image2show, COLOR_GRAY2RGB);
+    }
+    else if(src.type() == CV_8UC3) //RGB 彩色
+    {
+        image2show = src.clone();
+    }
+
+    for(const auto &light_blob:light_blobs)
+    {
+        rectangle(image2show, light_blob.rect.boundingRect(), Scalar(255,0,0), 3);
+    }
+    imshow(windows_name, image2show);
+}
+