Merge remote-tracking branch 'origin/master'

# Conflicts:
#	main.cpp

CMakeLists.txt

@@ -12,16 +12,17 @@ IF(CCACHE_FOUND)
 ENDIF()
 
 FIND_PACKAGE(OpenCV 3 REQUIRED)
+FIND_PACKAGE(Eigen3 REQUIRED)
 FIND_PACKAGE(Threads)
 
+include_directories( ${EIGEN3_INCLUDE_DIR} )
 include_directories( ${PROJECT_SOURCE_DIR}/energy/include )
 include_directories( ${PROJECT_SOURCE_DIR}/armor/include )
 include_directories( ${PROJECT_SOURCE_DIR}/include )
-include_directories( ${PROJECT_SOURCE_DIR}/src )
-FILE(GLOB_RECURSE sourcefiles "src/*.cpp" "energy/src/*cpp" "armor/src/*.cpp")
 
+FILE(GLOB_RECURSE sourcefiles "src/*.cpp" "energy/src/*cpp" "armor/src/*.cpp")
 add_executable(run main.cpp ${sourcefiles} )
 
-TARGET_LINK_LIBRARIES (run ${CMAKE_THREAD_LIBS_INIT})
+TARGET_LINK_LIBRARIES(run ${CMAKE_THREAD_LIBS_INIT})
-TARGET_LINK_LIBRARIES(run  ${OpenCV_LIBS})
+TARGET_LINK_LIBRARIES(run ${OpenCV_LIBS})
-TARGET_LINK_LIBRARIES(run  ${PROJECT_SOURCE_DIR}/libMVSDK.so)
+TARGET_LINK_LIBRARIES(run ${PROJECT_SOURCE_DIR}/libMVSDK.so)

armor/include/armor_finder/armor_finder.h

@@ -8,6 +8,7 @@
 #include <opencv2/core.hpp>
 #include <opencv2/tracking.hpp>
 #include <uart/uart.h>
+#include <armor_finder/classifier/classifier.h>
 
 typedef enum{
     ENEMY_BLUE, ENEMY_RED
@@ -15,7 +16,7 @@ typedef enum{
 
 class ArmorFinder{
 public:
-    ArmorFinder(EnemyColor color, Uart &u);
+    ArmorFinder(EnemyColor color, Uart &u, string paras_folder);
     ~ArmorFinder() = default;
 
 private:
@@ -29,6 +30,9 @@ private:
     State state;
     cv::Rect2d armor_box;
     cv::Ptr<cv::Tracker> tracker;
+    cv::Mat src_gray;
+
+    Classifier classifier;
 
     int  contour_area;
     Uart &uart;

armor/include/armor_finder/classifier/classifier.h

@@ -0,0 +1,51 @@
+//
+// Created by xinyang on 19-4-19.
+//
+
+#ifndef _CLASSIFIER_H_
+#define _CLASSIFIER_H_
+
+#include <Eigen/Dense>
+#include <opencv2/core/eigen.hpp>
+#include <vector>
+#include <string>
+#include <opencv2/core.hpp>
+
+using namespace std;
+using namespace Eigen;
+
+class Classifier {
+private:
+    bool state;
+
+    vector<vector<MatrixXd>> conv1_w, conv2_w;
+    vector<double> conv1_b, conv2_b;
+    MatrixXd fc1_w, fc2_w;
+    VectorXd fc1_b, fc2_b;
+
+    vector<vector<MatrixXd>> load_conv_w(const string &file);
+    vector<double> load_conv_b(const string &file);
+    MatrixXd load_fc_w(const string &file);
+    VectorXd load_fc_b(const string &file);
+
+    MatrixXd softmax(const MatrixXd &input);
+    MatrixXd relu(const MatrixXd &input);
+    vector<vector<MatrixXd>> apply_bias(const vector<vector<MatrixXd>> &input, const vector<double> &bias);
+    vector<vector<MatrixXd>> relu(const vector<vector<MatrixXd>> &input);
+    vector<vector<MatrixXd>> pool(const vector<vector<MatrixXd>> &input, int size);
+    vector<vector<MatrixXd>> pand(const vector<vector<MatrixXd>> &input, int val);
+    MatrixXd conv(const MatrixXd &filter, const MatrixXd &input);
+    vector<vector<MatrixXd>> conv2(const vector<vector<MatrixXd>> &filter, const vector<vector<MatrixXd>> &input);
+    MatrixXd flatten(const vector<vector<MatrixXd>> &input);
+
+public:
+    explicit Classifier(const string &folder);
+    ~Classifier() = default;
+
+    MatrixXd calculate(const vector<vector<MatrixXd>> &input);
+    explicit operator bool() const;
+    int operator()(const cv::Mat &image);
+};
+
+
+#endif //RUNCNN_CLASSIFIER_H

armor/src/armor_finder/armor_finder.cpp

@@ -4,10 +4,12 @@
 #include <log.h>
 #include <armor_finder/armor_finder.h>
 
-ArmorFinder::ArmorFinder(EnemyColor color, Uart &u) :
+ArmorFinder::ArmorFinder(EnemyColor color, Uart &u, string paras_folder) :
                             uart(u),
                             enemy_color(color),
-                            state(STANDBY_STATE)
+                            state(STANDBY_STATE),
+                            classifier(std::move(paras_folder)),
+                            contour_area(0)
                             {
     auto para = TrackerToUse::Params();
     para.desc_npca = 1;
@@ -25,6 +27,7 @@ void ArmorFinder::run(cv::Mat &src) {
     }else{
         src_use = src.clone();
     }
+    cv::cvtColor(src_use, src_gray, CV_BayerBG2GRAY);
 
 //    return stateSearchingTarget(src_use);
 

armor/src/armor_finder/classifier/classifier.cpp

@@ -0,0 +1,270 @@
+//
+// Created by xinyang on 19-4-19.
+//
+
+#include <armor_finder/classifier/classifier.h>
+#include <log.h>
+#include <cstdio>
+#include <iostream>
+
+
+vector<vector<MatrixXd>> Classifier::load_conv_w(const string &file){
+    vector<vector<MatrixXd>> result;
+    FILE *fp = fopen(file.data(), "r");
+    if(fp == nullptr){
+        LOGE("%s open fail!", file.data());
+        state = false;
+        return result;
+    }
+    int channel_in, channel_out, row, col;
+    fscanf(fp, "%d %d %d %d", &channel_in, &channel_out, &row, &col);
+    for(int o=0; o<channel_in; o++){
+        vector<MatrixXd> sub;
+        for(int i=0; i<channel_out; i++){
+            MatrixXd f(row, col);
+            for(int r=0; r<row; r++){
+                for(int c=0; c<col; c++){
+                    fscanf(fp, "%lf", &f(r, c));
+                }
+            }
+            sub.emplace_back(f);
+        }
+        result.emplace_back(sub);
+    }
+    return result;
+}
+
+vector<double> Classifier::load_conv_b(const string &file){
+    vector<double> result;
+    FILE *fp = fopen(file.data(), "r");
+    if(fp == nullptr){
+        LOGE("%s open fail!", file.data());
+        state = false;
+        return result;
+    }
+    int len;
+    fscanf(fp, "%d", &len);
+    for(int i=0; i<len; i++) {
+        double v;
+        fscanf(fp, "%lf", &v);
+        result.emplace_back(v);
+    }
+    return result;
+}
+
+MatrixXd Classifier::load_fc_w(const string &file){
+    FILE *fp = fopen(file.data(), "r");
+    if(fp == nullptr){
+        LOGE("%s open fail!", file.data());
+        state = false;
+        return MatrixXd::Zero(1,1);
+    }
+    int row, col;
+    fscanf(fp, "%d %d", &col, &row);
+    MatrixXd mat(row, col);
+    for(int c=0; c<col; c++){
+        for(int r=0; r<row; r++){
+            fscanf(fp, "%lf", &mat(r, c));
+        }
+    }
+    return mat;
+}
+
+VectorXd Classifier::load_fc_b(const string &file){
+    FILE *fp = fopen(file.data(), "r");
+    if(fp == nullptr){
+        LOGE("%s open fail!", file.data());
+        state = false;
+        return VectorXd::Zero(1,1);
+    }
+    int row;
+    fscanf(fp, "%d", &row);
+    VectorXd vec(row, 1);
+    for(int r=0; r<row; r++){
+        fscanf(fp, "%lf", &vec(r));
+    }
+    return vec;
+}
+
+MatrixXd Classifier::softmax(const MatrixXd &input){
+    MatrixXd tmp = input.array() - input.maxCoeff();
+    return tmp.array().exp() / tmp.array().exp().sum();
+}
+
+vector<vector<MatrixXd>> Classifier::pool(const vector<vector<MatrixXd>> &input, int size){
+    vector<vector<MatrixXd>> output;
+    for(int sample=0; sample<input.size(); sample++) {
+        vector<MatrixXd> sub;
+        for (int channel = 0; channel < input[0].size(); channel++) {
+            MatrixXd tmp(input[0][0].rows() / size, input[0][0].cols() / size);
+            for (int row = 0; row < input[0][0].rows() / size; row++) {
+                for (int col = 0; col < input[0][0].cols() / size; col++) {
+                    double val = 0;
+                    for (int x = 0; x < size; x++) {
+                        for (int y = 0; y < size; y++) {
+                            val += input[sample][channel](row * size + x, col * size + y);
+                        }
+                    }
+                    tmp(row, col) = val / size / size;
+                }
+            }
+            sub.emplace_back(tmp);
+        }
+        output.emplace_back(sub);
+    }
+    return output;
+}
+
+vector<vector<MatrixXd>> Classifier::apply_bias(const vector<vector<MatrixXd>> &input, const vector<double> &bias){
+    assert(input[0].size()==bias.size());
+    vector<vector<MatrixXd>> result;
+    for(int samples=0; samples<input.size(); samples++){
+        vector<MatrixXd> sub;
+        for(int channels=0; channels<input[0].size(); channels++){
+            MatrixXd mat = input[samples][channels].array() + bias[channels];
+            sub.emplace_back(mat);
+        }
+        result.emplace_back(sub);
+    }
+    return result;
+}
+
+MatrixXd Classifier::relu(const MatrixXd &input){
+    return input.unaryExpr([](double val){
+       return (val>0)?(val):(0);
+    });
+}
+
+vector<vector<MatrixXd>> Classifier::relu(const vector<vector<MatrixXd>> &input){
+    vector<vector<MatrixXd>> result;
+    for(int samples=0; samples<input.size(); samples++){
+        vector<MatrixXd> sub;
+        for(int channels=0; channels<input[0].size(); channels++){
+            sub.emplace_back(relu(input[samples][channels]));
+        }
+        result.emplace_back(sub);
+    }
+    return result;
+}
+
+vector<vector<MatrixXd>> Classifier::pand(const vector<vector<MatrixXd>> &input, int val){
+    vector<vector<MatrixXd>> result;
+    for(int sample=0; sample<input.size(); sample++){
+        vector<MatrixXd> sub;
+        for(int channels=0; channels<input[0].size(); channels++){
+            MatrixXd mat = MatrixXd::Zero(input[0][0].rows()+2*val, input[0][0].cols()+2*val);
+            mat.block(val, val, input[0][0].rows(), input[0][0].cols()) = input[sample][channels];
+            sub.emplace_back(mat);
+        }
+        result.emplace_back(sub);
+    }
+    return result;
+}
+
+MatrixXd Classifier::conv(const MatrixXd &filter, const MatrixXd &input){
+    int result_rows = input.rows()-filter.rows()+1;
+    int result_cols = input.cols()-filter.cols()+1;
+    MatrixXd result(result_rows, result_cols);
+    for(int row=0; row<result_rows; row++){
+        for(int col=0; col<result_cols; col++){
+            double val=0;
+            for(int x=0; x<filter.cols(); x++){
+                for(int y=0; y<filter.cols(); y++){
+                    val += input(row+x, col+y) * filter(x,y);
+                }
+            }
+            result(row, col) = val;
+//            result(row, col) = (input.block(row, col, size, size).array() * input.array()).sum();
+        }
+    }
+    return result;
+}
+
+vector<vector<MatrixXd>> Classifier::conv2(const vector<vector<MatrixXd>> &filter, const vector<vector<MatrixXd>> &input){
+    if(filter.size() != input[0].size()){
+        LOGE("shape du not match, which is filter.size=%d, input[0].size()=%d", filter.size(), input[0].size());
+        exit(-1);
+    }
+    vector<vector<MatrixXd>> result;
+    int result_rows = input[0][0].rows()-filter[0][0].rows()+1;
+    int result_cols = input[0][0].cols()-filter[0][0].cols()+1;
+    for(int col=0; col<input.size(); col++){
+        vector<MatrixXd> sub;
+        for(int row=0; row<filter[0].size(); row++){
+            MatrixXd val = MatrixXd::Zero(result_rows, result_cols);
+            for(int x=0; x<filter.size(); x++){
+                val += conv(filter[x][row], input[col][x]);
+            }
+            sub.emplace_back(val);
+        }
+        result.emplace_back(sub);
+    }
+    return result;
+}
+
+MatrixXd Classifier::flatten(const vector<vector<MatrixXd>> &input){
+    int ones = input[0][0].rows()*input[0][0].cols();
+    int channels = input[0].size();
+    int samples = input.size();
+    int row = input[0][0].rows();
+    int col = input[0][0].cols();
+    MatrixXd output(channels*ones,samples);
+    for(int s=0; s<samples; s++) {
+        for(int r=0, cnt=0; r<row; r++){
+            for(int c=0; c<col; c++){
+                for (int i = 0; i < channels; i++) {
+                    output(cnt++, s) = input[s][i](r, c);
+                }
+            }
+        }
+
+    }
+    return output;
+}
+
+Classifier::Classifier(const string &folder) : state(true){
+    conv1_w = load_conv_w(folder+"conv1_w");
+    conv1_b = load_conv_b(folder+"conv1_b");
+    conv2_w = load_conv_w(folder+"conv2_w");
+    conv2_b = load_conv_b(folder+"conv2_b");
+    fc1_w = load_fc_w(folder+"fc1_w");
+    fc1_b = load_fc_b(folder+"fc1_b");
+    fc2_w = load_fc_w(folder+"fc2_w");
+    fc2_b = load_fc_b(folder+"fc2_b");
+}
+
+//#define PRINT_MAT(name) (cout << #name":\n" << name << endl)
+//#define PRINT_MULTI_MAT(name) (cout << #name":\n" << name[0][0] << endl)
+//#define PRINT_MAT_SHAPE(name) LOGM(#name":(%d, %d)", name.rows(), name.cols())
+//#define PRINT_MULTI_MAT_SHAPE(name) LOGM(#name":(%d, %d)", name[0][0].rows(), name[0][0].cols())
+
+MatrixXd Classifier::calculate(const vector<vector<MatrixXd>> &input) {
+    vector<vector<MatrixXd>> conv1_result = relu(apply_bias(conv2(conv1_w, input), conv1_b));
+    vector<vector<MatrixXd>> pool1_result = pool(conv1_result, 2);
+    vector<vector<MatrixXd>> conv2_result = relu(apply_bias(conv2(conv2_w, pool1_result), conv2_b));
+    vector<vector<MatrixXd>> pool2_result = pool(conv2_result, 2);
+    MatrixXd flattened = flatten(pool2_result);
+    MatrixXd y1 = fc1_w * flattened;
+    y1.colwise() += fc1_b;
+    MatrixXd fc1 = relu(y1);
+    MatrixXd y2 = fc2_w * fc1;
+    y2.colwise() += fc2_b;
+    MatrixXd fc2 = softmax(y2);
+    return fc2;
+}
+
+Classifier::operator bool() const {
+    return state;
+}
+
+int Classifier::operator()(const cv::Mat &image) {
+    MatrixXd x;
+    cv2eigen(image, x);
+    x /= 255;
+    vector<MatrixXd> sub = {x};
+    vector<vector<MatrixXd>> in = {sub};
+    MatrixXd result = calculate(in);
+    MatrixXd::Index minRow, minCol;
+    result.maxCoeff(&minRow, &minCol);
+    return minRow;
+}

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

@@ -99,7 +99,7 @@ bool isCoupleLight(const LightBlob &light_blob_i, const LightBlob &light_blob_j)
 
 }
 
-double centerDistance(cv::Rect2d box){
+double centerDistance(const cv::Rect2d &box){
     double dx = box.x-box.width/2 - 320;
     double dy = box.y-box.height/2 - 240;
     return dx*dx + dy*dy;
@@ -169,14 +169,23 @@ bool ArmorFinder::stateSearchingTarget(cv::Mat &src) {
 //    }
     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(classifier){
+        for(const auto &box : armor_boxes){
+            cv::Mat roi = src(box).clone();
+            cv::resize(roi, roi, cv::Size(60, 45));
+            if(classifier(roi)){
+                armor_box = box;
+                break;
+            }
+        }
+    }else{
+        armor_box = armor_boxes[0];
+    }
     if(show_armor_boxes){
         showArmorBoxVector("boxes", split, armor_boxes);
         cv::waitKey(1);

main.cpp

@@ -13,7 +13,6 @@
 #include "camera/wrapper_head.h"
 #include "armor_finder/armor_finder.h"
 #include <options/options.h>
-//#define LOG_LEVEL LOG_WARRING
 #include <log.h>
 
 #include <thread>
@@ -24,10 +23,9 @@ using namespace std;
 #define ENERGY_STATE 1
 #define ARMOR_STATE 0
 
-int state = ENERGY_STATE;
+int state = ARMOR_STATE;
 float curr_yaw=0, curr_pitch=0;
 float mark_yaw=0, mark_pitch=0;
-int mark = 0;
 
 void uartReceive(Uart* uart);
 
@@ -61,7 +59,7 @@ int main(int argc, char *argv[])
 
 		Mat energy_src, armor_src;
 
-		ArmorFinder armorFinder(ENEMY_BLUE, uart);
+		ArmorFinder armorFinder(ENEMY_BLUE, uart, "../paras/");
 
         Energy energy(uart);
         energy.setAllyColor(ally_color);
@@ -100,25 +98,24 @@ void uartReceive(Uart* uart){
         while((data=uart->receive()) != '\n'){
             buffer[cnt++] = data;
             if(cnt >= 100){
-//                LOGE("data receive over flow!");
+                LOGE("data receive over flow!");
             }
         }
         if(cnt == 10){
             if(buffer[8] == 'e'){
                 state = ENERGY_STATE;
-//                LOGM("Energy state");
+                LOGM("Energy state");
             }else if(buffer[8] == 'a'){
                 state = ARMOR_STATE;
-//                LOGM("Armor state");
+                LOGM("Armor state");
             }
             memcpy(&curr_yaw, buffer, 4);
             memcpy(&curr_pitch, buffer+4, 4);
-//            LOGM("Get yaw:%f pitch:%f", curr_yaw, curr_pitch);
+            LOGM("Get yaw:%f pitch:%f", curr_yaw, curr_pitch);
-            if(buffer[9] == 1 && mark == 0){
+            if(buffer[9] == 1){
-                    mark = 1;
+                mark_yaw = curr_yaw;
-                    mark_yaw = curr_yaw;
+                mark_pitch = curr_pitch;
-                    mark_pitch = curr_pitch;
+                LOGM("Marked");
-//                LOGM("Marked");
             }
         }
         cnt = 0;