From 83e9a0daf9833c46e4fedd653fd54e75cd131d7f Mon Sep 17 00:00:00 2001 From: xinyang Date: Sat, 20 Apr 2019 14:30:57 +0800 Subject: [PATCH 1/3] =?UTF-8?q?=E7=8E=B0=E5=8F=AF=E4=BD=BF=E7=94=A8CNN?= =?UTF-8?q?=E5=88=86=E7=B1=BB=E5=99=A8=E5=88=A4=E6=96=AD=E8=A3=85=E7=94=B2?= =?UTF-8?q?=E6=9D=BF=E5=8F=8A=E5=85=B6=E6=95=B0=E5=AD=97=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 11 +- armor/include/armor_finder/armor_finder.h | 6 +- armor/src/armor_finder/armor_finder.cpp | 7 +- .../searching_state/searching_state.cpp | 17 +- main.cpp | 246 +++++++++--------- 5 files changed, 152 insertions(+), 135 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 32751b5..99a0232 100644 --- a/CMakeLists.txt +++ b/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 ${OpenCV_LIBS}) -TARGET_LINK_LIBRARIES(run ${PROJECT_SOURCE_DIR}/libMVSDK.so) +TARGET_LINK_LIBRARIES(run ${CMAKE_THREAD_LIBS_INIT}) +TARGET_LINK_LIBRARIES(run ${OpenCV_LIBS}) +TARGET_LINK_LIBRARIES(run ${PROJECT_SOURCE_DIR}/libMVSDK.so) diff --git a/armor/include/armor_finder/armor_finder.h b/armor/include/armor_finder/armor_finder.h index 1525af5..9d74f97 100644 --- a/armor/include/armor_finder/armor_finder.h +++ b/armor/include/armor_finder/armor_finder.h @@ -8,6 +8,7 @@ #include #include #include +#include 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 tracker; + cv::Mat src_gray; + + Classifier classifier; int contour_area; Uart &uart; diff --git a/armor/src/armor_finder/armor_finder.cpp b/armor/src/armor_finder/armor_finder.cpp index 839b91e..9a19d59 100644 --- a/armor/src/armor_finder/armor_finder.cpp +++ b/armor/src/armor_finder/armor_finder.cpp @@ -4,10 +4,12 @@ #include #include -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); diff --git a/armor/src/armor_finder/state_machine/searching_state/searching_state.cpp b/armor/src/armor_finder/state_machine/searching_state/searching_state.cpp index f36ebc4..4415fd9 100644 --- a/armor/src/armor_finder/state_machine/searching_state/searching_state.cpp +++ b/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); diff --git a/main.cpp b/main.cpp index 7b0b159..1bf0f83 100644 --- a/main.cpp +++ b/main.cpp @@ -1,123 +1,123 @@ -// -// Created by xixiliadorabarry on 1/24/19. -// -#include -#include - -#include "energy/energy.h" -#include "uart/uart.h" -#include "energy/param_struct_define.h" -#include "energy/constant.h" -#include "camera/camera_wrapper.h" -#include "camera/video_wrapper.h" -#include "camera/wrapper_head.h" -#include "armor_finder/armor_finder.h" -#include -#include - -#include - -using namespace cv; -using namespace std; - -#define ENERGY_STATE 1 -#define ARMOR_STATE 0 - -int state = ARMOR_STATE; -float curr_yaw=0, curr_pitch=0; -float mark_yaw=0, mark_pitch=0; - -void uartReceive(Uart* uart); - -int main(int argc, char *argv[]) -{ - process_options(argc, argv); - Uart uart; - thread receive(uartReceive, &uart); - bool flag = true; - - while (flag) - { - int ally_color = ALLY_RED; - int energy_part_rotation = CLOCKWISE; - - int from_camera = 1; - if(!run_with_camera) { - cout << "Input 1 for camera, 0 for video files" << endl; - cin >> from_camera; - } - - WrapperHead *video; - if(from_camera) - video = new CameraWrapper; - else - video = new VideoWrapper("r_l_640.avi", "fan_640.avi"); - - if (video->init()) { - cout << "Video source initialization successfully." << endl; - } - - Mat energy_src, armor_src; - - ArmorFinder armorFinder(ENEMY_BLUE, uart); - - Energy energy(uart); - energy.setAllyColor(ally_color); - energy.setRotation(energy_part_rotation); - - while (video->read(energy_src, armor_src)) - { - if(show_origin) { - imshow("enery src", energy_src); - imshow("armor src", armor_src); - } - if(state == ENERGY_STATE){ - energy.run(energy_src); - }else{ - armorFinder.run(armor_src); - } - - if (waitKey(1) == 'q') { - flag = false; - break; - } - } - delete video; - cout << "Program fails. Restarting" << endl; - } - - return 0; -} - - -void uartReceive(Uart* uart){ - char buffer[100]; - int cnt=0; - while(true){ - char data; - while((data=uart->receive()) != '\n'){ - buffer[cnt++] = data; - if(cnt >= 100){ - LOGE("data receive over flow!"); - } - } - if(cnt == 10){ - if(buffer[8] == 'e'){ - state = ENERGY_STATE; - LOGM("Energy state"); - }else if(buffer[8] == 'a'){ - state = 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); - if(buffer[9] == 1){ - mark_yaw = curr_yaw; - mark_pitch = curr_pitch; - LOGM("Marked"); - } - } - cnt = 0; - } -} +// +// Created by xixiliadorabarry on 1/24/19. +// +#include +#include + +#include "energy/energy.h" +#include "uart/uart.h" +#include "energy/param_struct_define.h" +#include "energy/constant.h" +#include "camera/camera_wrapper.h" +#include "camera/video_wrapper.h" +#include "camera/wrapper_head.h" +#include "armor_finder/armor_finder.h" +#include +#include + +#include + +using namespace cv; +using namespace std; + +#define ENERGY_STATE 1 +#define ARMOR_STATE 0 + +int state = ARMOR_STATE; +float curr_yaw=0, curr_pitch=0; +float mark_yaw=0, mark_pitch=0; + +void uartReceive(Uart* uart); + +int main(int argc, char *argv[]) +{ + process_options(argc, argv); + Uart uart; + thread receive(uartReceive, &uart); + bool flag = true; + + while (flag) + { + int ally_color = ALLY_RED; + int energy_part_rotation = CLOCKWISE; + + int from_camera = 1; + if(!run_with_camera) { + cout << "Input 1 for camera, 0 for video files" << endl; + cin >> from_camera; + } + + WrapperHead *video; + if(from_camera) + video = new CameraWrapper; + else + video = new VideoWrapper("r_l_640.avi", "fan_640.avi"); + + if (video->init()) { + cout << "Video source initialization successfully." << endl; + } + + Mat energy_src, armor_src; + + ArmorFinder armorFinder(ENEMY_BLUE, uart, "../paras/"); + + Energy energy(uart); + energy.setAllyColor(ally_color); + energy.setRotation(energy_part_rotation); + + while (video->read(energy_src, armor_src)) + { + if(show_origin) { + imshow("enery src", energy_src); + imshow("armor src", armor_src); + } + if(state == ENERGY_STATE){ + energy.run(energy_src); + }else{ + armorFinder.run(armor_src); + } + + if (waitKey(1) == 'q') { + flag = false; + break; + } + } + delete video; + cout << "Program fails. Restarting" << endl; + } + + return 0; +} + + +void uartReceive(Uart* uart){ + char buffer[100]; + int cnt=0; + while(true){ + char data; + while((data=uart->receive()) != '\n'){ + buffer[cnt++] = data; + if(cnt >= 100){ + LOGE("data receive over flow!"); + } + } + if(cnt == 10){ + if(buffer[8] == 'e'){ + state = ENERGY_STATE; + LOGM("Energy state"); + }else if(buffer[8] == 'a'){ + state = 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); + if(buffer[9] == 1){ + mark_yaw = curr_yaw; + mark_pitch = curr_pitch; + LOGM("Marked"); + } + } + cnt = 0; + } +} From 81ac69fa9e3c10e8b357f8725fe60c503fe31fab Mon Sep 17 00:00:00 2001 From: xinyang Date: Sat, 20 Apr 2019 14:37:16 +0800 Subject: [PATCH 2/3] =?UTF-8?q?=E7=8E=B0=E5=8F=AF=E4=BD=BF=E7=94=A8CNN?= =?UTF-8?q?=E5=88=86=E7=B1=BB=E5=99=A8=E5=88=A4=E6=96=AD=E8=A3=85=E7=94=B2?= =?UTF-8?q?=E6=9D=BF=E5=8F=8A=E5=85=B6=E6=95=B0=E5=AD=97=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../armor_finder/classifier/classifier.h | 51 ++++ .../armor_finder/classifier/classifier.cpp | 269 ++++++++++++++++++ 2 files changed, 320 insertions(+) create mode 100644 armor/include/armor_finder/classifier/classifier.h create mode 100644 armor/src/armor_finder/classifier/classifier.cpp diff --git a/armor/include/armor_finder/classifier/classifier.h b/armor/include/armor_finder/classifier/classifier.h new file mode 100644 index 0000000..57b3bc6 --- /dev/null +++ b/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 +#include +#include +#include +#include + +using namespace std; +using namespace Eigen; + +class Classifier { +private: + bool state; + + vector> conv1_w, conv2_w; + vector conv1_b, conv2_b; + MatrixXd fc1_w, fc2_w; + VectorXd fc1_b, fc2_b; + + vector> load_conv_w(const string &file); + vector 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> apply_bias(const vector> &input, const vector &bias); + vector> relu(const vector> &input); + vector> pool(const vector> &input, int size); + vector> pand(const vector> &input, int val); + MatrixXd conv(const MatrixXd &filter, const MatrixXd &input); + vector> conv2(const vector> &filter, const vector> &input); + MatrixXd flatten(const vector> &input); + +public: + explicit Classifier(const string &folder); + ~Classifier() = default; + + MatrixXd calculate(const vector> &input); + explicit operator bool() const; + int operator()(const cv::Mat &image); +}; + + +#endif //RUNCNN_CLASSIFIER_H diff --git a/armor/src/armor_finder/classifier/classifier.cpp b/armor/src/armor_finder/classifier/classifier.cpp new file mode 100644 index 0000000..b303375 --- /dev/null +++ b/armor/src/armor_finder/classifier/classifier.cpp @@ -0,0 +1,269 @@ +// +// Created by xinyang on 19-4-19. +// + +#include +#include +#include +#include + + +vector> Classifier::load_conv_w(const string &file){ + vector> 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 sub; + for(int i=0; i Classifier::load_conv_b(const string &file){ + vector 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> Classifier::pool(const vector> &input, int size){ + vector> output; + for(int sample=0; sample 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> Classifier::apply_bias(const vector> &input, const vector &bias){ + assert(input[0].size()==bias.size()); + vector> result; + for(int samples=0; samples sub; + for(int channels=0; channels0)?(val):(0); + }); +} + +vector> Classifier::relu(const vector> &input){ + vector> result; + for(int samples=0; samples sub; + for(int channels=0; channels> Classifier::pand(const vector> &input, int val){ + vector> result; + for(int sample=0; sample sub; + for(int channels=0; channels> Classifier::conv2(const vector> &filter, const vector> &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> 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 sub; + for(int row=0; row> &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> &input) { + vector> conv1_result = relu(apply_bias(conv2(conv1_w, input), conv1_b)); + vector> pool1_result = pool(conv1_result, 2); + vector> conv2_result = relu(apply_bias(conv2(conv2_w, pool1_result), conv2_b)); + vector> 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); + vector sub = {x}; + vector> in = {sub}; + MatrixXd result = calculate(in); + MatrixXd::Index minRow, minCol; + result.maxCoeff(&minRow, &minCol); + return minRow; +} \ No newline at end of file From 5c7317140a56798cd9f71cac30971226742e5453 Mon Sep 17 00:00:00 2001 From: xinyang Date: Sat, 20 Apr 2019 15:43:04 +0800 Subject: [PATCH 3/3] =?UTF-8?q?=E7=8E=B0=E5=8F=AF=E4=BD=BF=E7=94=A8CNN?= =?UTF-8?q?=E5=88=86=E7=B1=BB=E5=99=A8=E5=88=A4=E6=96=AD=E8=A3=85=E7=94=B2?= =?UTF-8?q?=E6=9D=BF=E5=8F=8A=E5=85=B6=E6=95=B0=E5=AD=97=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- armor/src/armor_finder/classifier/classifier.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/armor/src/armor_finder/classifier/classifier.cpp b/armor/src/armor_finder/classifier/classifier.cpp index b303375..2c11ea5 100644 --- a/armor/src/armor_finder/classifier/classifier.cpp +++ b/armor/src/armor_finder/classifier/classifier.cpp @@ -260,6 +260,7 @@ Classifier::operator bool() const { int Classifier::operator()(const cv::Mat &image) { MatrixXd x; cv2eigen(image, x); + x /= 255; vector sub = {x}; vector> in = {sub}; MatrixXd result = calculate(in);